├── .clang-format ├── .git-blame-ignore-revs ├── .gitattributes ├── .github └── README.md ├── .gitignore ├── .gitmodules ├── LICENSE.md ├── README.md ├── co_options.h.in ├── include ├── co_api.h ├── co_obj.h └── co_rtk.h ├── options.h.in ├── samples ├── slave │ ├── main.c │ ├── slave.c │ ├── slave.eds │ └── slave.h └── slaveinfo │ ├── main.c │ ├── slaveinfo.c │ └── slaveinfo.h ├── src ├── co_bitmap.c ├── co_bitmap.h ├── co_emcy.c ├── co_emcy.h ├── co_heartbeat.c ├── co_heartbeat.h ├── co_log.c ├── co_log.h ├── co_lss.c ├── co_lss.h ├── co_main.c ├── co_main.h ├── co_nmt.c ├── co_nmt.h ├── co_node_guard.c ├── co_node_guard.h ├── co_obj.c ├── co_od.c ├── co_od.h ├── co_pdo.c ├── co_pdo.h ├── co_sdo.h ├── co_sdo_client.c ├── co_sdo_server.c ├── co_sync.c ├── co_sync.h ├── co_util.h ├── coal_can.h └── ports │ ├── linux │ ├── coal_can.c │ └── coal_can_sys.h │ ├── rt-kernel │ ├── coal_can.c │ └── coal_can_sys.h │ └── windows │ ├── coal_can.c │ └── coal_can_sys.h ├── test ├── co_test.cpp ├── mocks.cpp ├── mocks.h ├── test_bitmap.cpp ├── test_emcy.cpp ├── test_heartbeat.cpp ├── test_lss.cpp ├── test_main.cpp ├── test_nmt.cpp ├── test_node_guard.cpp ├── test_od.cpp ├── test_pdo.cpp ├── test_sdo_client.cpp ├── test_sdo_server.cpp ├── test_sync.cpp └── test_util.h └── version.h.in /.clang-format: -------------------------------------------------------------------------------- 1 | AlignAfterOpenBracket: AlwaysBreak 2 | AlignConsecutiveAssignments: true 3 | AlignConsecutiveMacros: true 4 | AllowAllArgumentsOnNextLine: false 5 | AllowAllParametersOfDeclarationOnNextLine: false 6 | AllowShortCaseLabelsOnASingleLine: false 7 | AllowShortFunctionsOnASingleLine: None 8 | AllowShortIfStatementsOnASingleLine: Never 9 | BinPackArguments: false 10 | BinPackParameters: false 11 | BreakBeforeBraces: Custom 12 | BraceWrapping: 13 | AfterCaseLabel: true 14 | AfterClass: true 15 | AfterControlStatement: true 16 | AfterEnum: true 17 | AfterFunction: true 18 | AfterNamespace: true 19 | AfterStruct: true 20 | AfterUnion: true 21 | BeforeCatch: true 22 | BeforeElse: true 23 | IndentBraces: false 24 | SplitEmptyFunction: true 25 | SplitEmptyRecord: true 26 | SplitEmptyNamespace: false 27 | AfterExternBlock: false 28 | ColumnLimit: 80 29 | ContinuationIndentWidth: 3 30 | IndentCaseLabels: false 31 | IndentWidth: 3 32 | PenaltyBreakAssignment: 10 33 | PenaltyBreakBeforeFirstCallParameter: 30 34 | PenaltyBreakComment: 10 35 | PenaltyBreakString: 1000 36 | PenaltyExcessCharacter: 15 37 | PenaltyReturnTypeOnItsOwnLine: 100000 38 | PointerAlignment: Middle 39 | SortIncludes: false 40 | SpaceBeforeParens: NonEmptyParentheses 41 | UseTab: Never 42 | -------------------------------------------------------------------------------- /.git-blame-ignore-revs: -------------------------------------------------------------------------------- 1 | # This file contains a list of commits that are not likely what you 2 | # are looking for in a blame, such as mass reformatting or renaming. 3 | # You can set this file as a default ignore file for blame by running 4 | # the following command. 5 | # 6 | # $ git config blame.ignoreRevsFile .git-blame-ignore-revs 7 | 8 | # Run clang-format 9 | feb35ba61528c4b74b2f2832963632ddb77418c5 10 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.github/README.md: -------------------------------------------------------------------------------- 1 | # Evaluation version 2 | 3 | This repository contains an evaluation version of **C-Open**, a CANopen 4 | stack for both master and slaves. It is especially well suited for 5 | embedded systems where resources are limited and efficiency is crucial. 6 | It is written in C and can be run on an RTOS such as rt-kernel, FreeRTOS, 7 | or on Linux. 8 | 9 | It does not contain any ports and cannot be built without adding additional sources. 10 | 11 | See: 12 | 13 | - [Readme](../README.md) for more information on the complete version of the stack. 14 | - [Releases](https://github.com/rtlabs-com/c-open/releases) for binary downloads for common targets. 15 | - [Product](https://rt-labs.com/product/c-open/) for license information as well as complete documentation. 16 | 17 | This version of C-Open can be used for evaluation purposes 18 | only. Contact if you intend to use this stack in a 19 | product or if you need assistance during evaluation. The commercial 20 | version of this stack is supplied with full sources. 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build*/ 2 | *~ 3 | *# 4 | CMakeFiles/ 5 | CMakeCache.txt 6 | CMakeUserPresets.json 7 | install 8 | docs/_build 9 | 10 | # Python files 11 | *env* 12 | 13 | .cproject 14 | .project 15 | .dir-locals.el 16 | 17 | # Object files 18 | *.o 19 | *.ko 20 | *.obj 21 | *.elf 22 | 23 | # Precompiled Headers 24 | *.gch 25 | *.pch 26 | 27 | # Libraries 28 | *.lib 29 | *.a 30 | *.la 31 | *.lo 32 | 33 | # Shared objects (inc. Windows DLLs) 34 | *.dll 35 | *.so 36 | *.so.* 37 | *.dylib 38 | 39 | # Executables 40 | *.exe 41 | *.out 42 | *.app 43 | *.i*86 44 | *.x86_64 45 | *.hex 46 | 47 | # Editors 48 | .vscode/ 49 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake/tools"] 2 | path = cmake/tools 3 | url = https://github.com/rtlabs-com/cmake-tools 4 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | # License 2 | 3 | This software is dual-licensed. 4 | 5 | ## GPL version 3 6 | 7 | This software is distributed under GPLv3. You are allowed to use this 8 | software for an open-source project with a compatible license. 9 | 10 | [GNU GPL license v3](https://www.gnu.org/licenses/gpl-3.0.html) 11 | 12 | ## Commercial license 13 | 14 | This software is also available under a commercial license with 15 | options for support and maintenance. Please contact sales@rt-labs.com 16 | for further details. 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | C-Open CANopen stack 2 | ==================== 3 | 4 | The RT-Labs CANopen stack C-Open can be used to implement a CANopen master or 5 | device. It supports multiple instances and can be run on bare-metal hardware, 6 | an RTOS such as RT-Kernel, or on Linux or Windows. 7 | 8 | The C-Open stack is supplied with full sources including a porting layer. A 9 | device application interfaces with the stack primarily using the object 10 | dictionary, while a master uses the full API to control the CANopen network. 11 | It is designed for minimal footprint and efficiency (memory usage on 12 | Cortex-M4, 14968 B ROM / 368 B RAM, plus user defined objects stored in RAM) 13 | and tested using the CANopen Conformance Test Tool. 14 | 15 | Web resources 16 | ------------- 17 | 18 | * Source repository: 19 | * Documentation: 20 | * Continuous integration: 21 | * RT-Labs (stack integration, certification services and training): 22 | 23 | Features 24 | -------- 25 | 26 | * CANopen master 27 | * CANopen device 28 | * CANopen services (CiA 301) 29 | * Network management (NMT) 30 | * Service data objects (SDO) 31 | * Process Data Objects (PDO) 32 | * Emergency object (EMCY) 33 | * Heartbeat 34 | * Node guarding 35 | * Layer Setting Services (LSS, CiA 305) 36 | * Bare-metal or RTOS 37 | * Porting layer provided 38 | * Linux (SocketCAN) 39 | * Windows (Kvaser CAN interface) 40 | 41 | License 42 | ------- 43 | 44 | This software is dual-licensed, with GPL version 3 and a commercial license. 45 | See LICENSE.md for more details. 46 | 47 | This repository contains a CANopen stack for both master and 48 | slaves. The stack implements most of CiA 301 and 305 (LSS). The stack 49 | is written to an OS abstraction layer and can also be used in a bare 50 | metal application. Using the abstraction layer, the stack can run on 51 | Linux, Windows or on an RTOS. 52 | 53 | A simple slave is included to serve as an example of how to use the 54 | stack. The slave can also be used to run the CiA Conformance Test 55 | Tool. 56 | 57 | Also included is a simple master example that lists all slaves on the 58 | bus and a comprehensive set of unit-tests. 59 | 60 | Contributions 61 | ------------- 62 | 63 | Contributions are welcome. If you want to contribute you will need to 64 | sign a Contributor License Agreement and send it to us either by 65 | e-mail or by physical mail. More information is available 66 | [here](https://rt-labs.com/contribution). 67 | -------------------------------------------------------------------------------- /co_options.h.in: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef CO_OPTIONS_H 17 | #define CO_OPTIONS_H 18 | 19 | #ifndef MAX_NODES 20 | #define MAX_NODES (@MAX_NODES@) 21 | #endif 22 | 23 | #ifndef MAX_EMCY_COBIDS 24 | #define MAX_EMCY_COBIDS (@MAX_EMCY_COBIDS@) 25 | #endif 26 | 27 | #ifndef MAX_HEARTBEATS 28 | #define MAX_HEARTBEATS (@MAX_HEARTBEATS@) 29 | #endif 30 | 31 | #ifndef MAX_PDO_ENTRIES 32 | #define MAX_PDO_ENTRIES (@MAX_PDO_ENTRIES@) 33 | #endif 34 | 35 | #ifndef MAX_TX_PDO 36 | #define MAX_TX_PDO (@MAX_TX_PDO@) 37 | #endif 38 | 39 | #ifndef MAX_RX_PDO 40 | #define MAX_RX_PDO (@MAX_RX_PDO@) 41 | #endif 42 | 43 | #ifndef MAX_ERRORS 44 | #define MAX_ERRORS (@MAX_ERRORS@) 45 | #endif 46 | 47 | #endif /* CO_OPTIONS_H */ 48 | -------------------------------------------------------------------------------- /include/co_rtk.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Support routines for rt-kernel 19 | */ 20 | #ifndef CO_RTK_H 21 | #define CO_RTK_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | #include 28 | 29 | /** 30 | * Get can configuration for bitrate 31 | * 32 | * This function returns the can configuration parameters for the 33 | * given bitrate. 34 | * 35 | * @param bitrate CAN bitrate (bits per second) 36 | * @param cfg CAN configuration parameters 37 | */ 38 | void co_can_get_cfg (int bitrate, can_cfg_t * cfg); 39 | 40 | #ifdef __cplusplus 41 | } 42 | #endif 43 | 44 | #endif /* CO_RTK_H */ 45 | -------------------------------------------------------------------------------- /options.h.in: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef OPTIONS_H 17 | #define OPTIONS_H 18 | 19 | #include "co_options.h" 20 | 21 | #ifndef LOG_LEVEL 22 | #define LOG_LEVEL (LOG_LEVEL_@LOG_LEVEL@) 23 | #endif 24 | 25 | #ifndef CO_ALLOC_LOG 26 | #define CO_ALLOC_LOG (LOG_STATE_@CO_ALLOC_LOG@) 27 | #endif 28 | 29 | #ifndef CO_CAN_LOG 30 | #define CO_CAN_LOG (LOG_STATE_@CO_CAN_LOG@) 31 | #endif 32 | 33 | #ifndef CO_SDO_LOG 34 | #define CO_SDO_LOG (LOG_STATE_@CO_SDO_LOG@) 35 | #endif 36 | 37 | #ifndef CO_OD_LOG 38 | #define CO_OD_LOG (LOG_STATE_@CO_OD_LOG@) 39 | #endif 40 | 41 | #ifndef CO_NMT_LOG 42 | #define CO_NMT_LOG (LOG_STATE_@CO_NMT_LOG@) 43 | #endif 44 | 45 | #ifndef CO_EMCY_LOG 46 | #define CO_EMCY_LOG (LOG_STATE_@CO_EMCY_LOG@) 47 | #endif 48 | 49 | #ifndef CO_HEARTBEAT_LOG 50 | #define CO_HEARTBEAT_LOG (LOG_STATE_@CO_HEARTBEAT_LOG@) 51 | #endif 52 | 53 | #ifndef CO_NODE_GUARD_LOG 54 | #define CO_NODE_GUARD_LOG (LOG_STATE_@CO_NODE_GUARD_LOG@) 55 | #endif 56 | 57 | #ifndef CO_LSS_LOG 58 | #define CO_LSS_LOG (LOG_STATE_@CO_LSS_LOG@) 59 | #endif 60 | 61 | #ifndef SDO_TIMEOUT 62 | #define SDO_TIMEOUT (@SDO_TIMEOUT@) 63 | #endif 64 | 65 | #ifndef CO_THREAD_PRIO 66 | #define CO_THREAD_PRIO (@CO_THREAD_PRIO@) 67 | #endif 68 | 69 | #ifndef CO_THREAD_STACK_SIZE 70 | #define CO_THREAD_STACK_SIZE (@CO_THREAD_STACK_SIZE@) 71 | #endif 72 | 73 | #endif /* OPTIONS_H */ 74 | -------------------------------------------------------------------------------- /samples/slave/main.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "slave.h" 17 | #include 18 | #include 19 | 20 | int main (int argc, char * argv[]) 21 | { 22 | uint32_t bitrate; 23 | 24 | if (argc != 3) 25 | { 26 | printf ("usage: %s \n", argv[0]); 27 | return -1; 28 | } 29 | 30 | bitrate = atoi (argv[2]); 31 | 32 | return slave_init (argv[1], bitrate); 33 | } 34 | -------------------------------------------------------------------------------- /samples/slave/slave.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include 17 | #include 18 | 19 | #include "co_api.h" 20 | #include "co_obj.h" 21 | 22 | #include "osal.h" 23 | 24 | /* This file contains a simple slave. It serves as an example of stack 25 | usage and can also be used to run the conformance test using the 26 | Conformance Test Tool (CTT). */ 27 | 28 | #define DEFAULT_NODE_ID 1 29 | 30 | static char mfr_device_name[] = "CANopen sample slave"; 31 | static char mfr_hw_version[] = "1.0"; 32 | static char mfr_sw_version[] = "0.1"; 33 | 34 | /* Contains 64-bit TPDO */ 35 | static uint64_t output; 36 | 37 | /* Contains 16-bit RPDO */ 38 | static uint16_t input; 39 | 40 | /* These are used for conformance test purposes */ 41 | static char os_command[20]; 42 | static char os_reply[20]; 43 | static uint8_t os_mode; 44 | 45 | /* Entry descriptor for Device type (1000h) */ 46 | static const co_entry_t OD1000[] = { 47 | {0x00, OD_RO, DTYPE_UNSIGNED32, 32, 0x12345678, NULL}, 48 | }; 49 | 50 | /* Entry descriptor for Manufacturer device name (1008h) */ 51 | static const co_entry_t OD1008[] = { 52 | {0x00, OD_RO, DTYPE_VISIBLE_STRING, 8 * sizeof (mfr_device_name), 0, mfr_device_name}, 53 | }; 54 | 55 | /* Entry descriptor for Manufacturer hardware version (1009h) */ 56 | static const co_entry_t OD1009[] = { 57 | {0x00, OD_RO, DTYPE_VISIBLE_STRING, 8 * sizeof (mfr_hw_version), 0, mfr_hw_version}, 58 | }; 59 | 60 | /* Entry descriptor for Manufacturer software version (100Ah) */ 61 | static const co_entry_t OD100A[] = { 62 | {0x00, OD_RO, DTYPE_VISIBLE_STRING, 8 * sizeof (mfr_sw_version), 0, mfr_sw_version}, 63 | }; 64 | 65 | /* Entry descriptor for Identity object (1018h) */ 66 | static const co_entry_t OD1018[] = { 67 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 4, NULL}, 68 | {0x01, OD_RO, DTYPE_UNSIGNED32, 32, 1, NULL}, 69 | {0x02, OD_RO, DTYPE_UNSIGNED32, 32, 2, NULL}, 70 | {0x03, OD_RO, DTYPE_UNSIGNED32, 32, 3, NULL}, 71 | {0x04, OD_RO, DTYPE_UNSIGNED32, 32, 4, NULL}, 72 | }; 73 | 74 | /* Entry descriptor for OS command (1023h) (for CTT) */ 75 | static const co_entry_t OD1023[] = { 76 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 3, NULL}, 77 | {0x01, 78 | OD_WO | OD_TRANSIENT, 79 | DTYPE_OCTET_STRING, 80 | 8 * sizeof (os_command), 81 | 0, 82 | os_command}, 83 | {0x02, OD_RO, DTYPE_UNSIGNED8, 8, 0, NULL}, 84 | {0x03, OD_RO, DTYPE_OCTET_STRING, 8 * sizeof (os_reply), 0, os_reply}, 85 | }; 86 | 87 | /* Entry descriptor for OS command mode (1024h) (for CTT) */ 88 | static const co_entry_t OD1024[] = { 89 | {0x00, OD_WO, DTYPE_UNSIGNED8, 8, 0, &os_mode}, 90 | }; 91 | 92 | /* Entry descriptor for sample 64-bit actuator (2000h) */ 93 | static const co_entry_t OD2000[] = { 94 | {0x00, OD_NOTIFY | OD_RW | OD_RPDO | OD_TRANSIENT, DTYPE_UNSIGNED64, 64, 0, &output}, 95 | }; 96 | 97 | /* Entry descriptor for sample 16-bit sensor (2001h) */ 98 | static const co_entry_t OD2001[] = { 99 | {0x00, OD_RO | OD_TPDO, DTYPE_UNSIGNED16, 16, 0, &input}, 100 | }; 101 | 102 | /* Object dictionary for the slave. Note that some objects are defined 103 | and handled by the stack. */ 104 | static const co_obj_t od[] = { 105 | // clang-format off 106 | {0x1000, OTYPE_VAR, 0, OD1000, NULL}, 107 | {0x1001, OTYPE_VAR, 0, OD1001, co_od1001_fn}, 108 | {0x1003, OTYPE_ARRAY, MAX_ERRORS, OD1003, co_od1003_fn}, 109 | {0x1005, OTYPE_VAR, 0, OD1005, co_od1005_fn}, 110 | {0x1006, OTYPE_VAR, 0, OD1006, co_od1006_fn}, 111 | {0x1007, OTYPE_VAR, 0, OD1007, co_od1007_fn}, 112 | {0x1008, OTYPE_VAR, 0, OD1008, NULL}, 113 | {0x1009, OTYPE_VAR, 0, OD1009, NULL}, 114 | {0x100A, OTYPE_VAR, 0, OD100A, NULL}, 115 | {0x100C, OTYPE_VAR, 0, OD100C, co_od100C_fn}, 116 | {0x100D, OTYPE_VAR, 0, OD100D, co_od100D_fn}, 117 | {0x1010, OTYPE_ARRAY, 4, OD1010, co_od1010_fn}, 118 | {0x1011, OTYPE_ARRAY, 4, OD1011, co_od1011_fn}, 119 | {0x1014, OTYPE_VAR, 0, OD1014, co_od1014_fn}, 120 | {0x1015, OTYPE_VAR, 0, OD1015, co_od1015_fn}, 121 | {0x1016, OTYPE_ARRAY, MAX_HEARTBEATS, OD1016, co_od1016_fn}, 122 | {0x1017, OTYPE_VAR, 0, OD1017, co_od1017_fn}, 123 | {0x1018, OTYPE_RECORD, 4, OD1018, NULL}, 124 | {0x1019, OTYPE_VAR, 0, OD1019, co_od1019_fn}, 125 | {0x1020, OTYPE_ARRAY, 2, OD1020, co_od1020_fn}, 126 | {0x1023, OTYPE_RECORD, 3, OD1023, NULL}, 127 | {0x1024, OTYPE_VAR, 0, OD1024, NULL}, 128 | {0x1028, OTYPE_ARRAY, MAX_EMCY_COBIDS, OD1028, co_od1028_fn}, 129 | {0x1029, OTYPE_ARRAY, 1, OD1029, co_od1029_fn}, 130 | {0x1400, OTYPE_RECORD, 5, OD1400, co_od1400_fn}, 131 | {0x1600, OTYPE_RECORD, MAX_PDO_ENTRIES, OD1600, co_od1600_fn}, 132 | {0x1800, OTYPE_RECORD, 6, OD1800, co_od1800_fn}, 133 | {0x1A00, OTYPE_RECORD, MAX_PDO_ENTRIES, OD1A00, co_od1A00_fn}, 134 | {0x2000, OTYPE_VAR, 0, OD2000, NULL}, 135 | {0x2001, OTYPE_VAR, 0, OD2001, NULL}, 136 | {0}, 137 | // clang-format on 138 | }; 139 | 140 | /* Default values to be set when the NMT state INIT is entered */ 141 | static const co_default_t od_defaults[] = { 142 | {0x1400, 1, 0x200 + DEFAULT_NODE_ID}, /* Setup 1 RPDO */ 143 | {0x1600, 0, 0x01}, /* One mapping */ 144 | {0x1600, 1, 0x20000040}, /* Map 2000h to RPDO (64 bits) */ 145 | {0x1800, 1, 0x180 + DEFAULT_NODE_ID}, /* Setup 1 TPDO */ 146 | {0x1A00, 0, 0x01}, /* One mapping */ 147 | {0x1A00, 1, 0x20010010}, /* Map 2001h to TPDO (16 bits) */ 148 | {0x2000, 0, 0xDEADCAFEFEEDBEEF}, /* Default value for output */ 149 | {0x2001, 0, 0x55AA}, /* Default value for input */ 150 | {0}}; 151 | 152 | /* Functions to simulate persistent storage. This uses RAM, while a 153 | real application would use filesystem or NVM for storage. */ 154 | 155 | static uint8_t stores[CO_STORE_LAST][1024]; 156 | static struct fd 157 | { 158 | uint8_t * p; 159 | } fd; 160 | 161 | static void * store_open (co_store_t store, co_mode_t mode) 162 | { 163 | if (store >= CO_STORE_LAST) 164 | return NULL; 165 | 166 | fd.p = stores[store]; 167 | return &fd; 168 | } 169 | 170 | static int store_read (void * arg, void * data, size_t size) 171 | { 172 | struct fd * fd = arg; 173 | memcpy (data, fd->p, size); 174 | fd->p += size; 175 | return 0; 176 | } 177 | 178 | static int store_write (void * arg, const void * data, size_t size) 179 | { 180 | struct fd * fd = arg; 181 | memcpy (fd->p, data, size); 182 | fd->p += size; 183 | return 0; 184 | } 185 | 186 | static int store_close (void * arg) 187 | { 188 | return 0; 189 | } 190 | 191 | /* Called when NMT command Reset node is received */ 192 | static void cb_reset (co_net_t * net) 193 | { 194 | /* Optionally reset hardware */ 195 | } 196 | 197 | /* Called when SYNC is received */ 198 | static void cb_sync (co_net_t * net) 199 | { 200 | input++; 201 | } 202 | 203 | /* Called when RPDO is received (if OD_NOTIFY is set) */ 204 | static void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex) 205 | { 206 | } 207 | 208 | int slave_init (const char * canif, int bitrate) 209 | { 210 | co_cfg_t cfg = { 211 | .node = DEFAULT_NODE_ID, 212 | .bitrate = bitrate, 213 | .od = od, 214 | .defaults = od_defaults, 215 | .cb_reset = cb_reset, 216 | .cb_sync = cb_sync, 217 | .cb_notify = cb_notify, 218 | .open = store_open, 219 | .read = store_read, 220 | .write = store_write, 221 | .close = store_close, 222 | }; 223 | 224 | /* Initialise and start stack */ 225 | co_net_t * net = co_init (canif, &cfg); 226 | if (net == NULL) 227 | { 228 | printf ("Init failed\n"); 229 | return -1; 230 | } 231 | 232 | /* Initialise client */ 233 | co_client_t * client = co_client_init (net); 234 | if (client == NULL) 235 | { 236 | printf ("Client init failed\n"); 237 | return -1; 238 | } 239 | 240 | printf ("Inited\n"); 241 | 242 | /* Wait a while, then generate error and emergency */ 243 | os_usleep (5 * 1000); 244 | printf ("Generating emergency\n"); 245 | co_error_set (client, CO_ERR_MANUFACTURER); 246 | co_emcy_issue (client, 0x1000, 0x1234, NULL); 247 | 248 | /* Clear the error */ 249 | os_usleep (5 * 1000); 250 | printf ("Clearing emergency\n"); 251 | co_error_clear (client, CO_ERR_MANUFACTURER); 252 | 253 | /* Loop forever */ 254 | printf ("Waiting for clients\n"); 255 | for (;;) 256 | { 257 | os_usleep (1000 * 1000); 258 | } 259 | return 0; 260 | } 261 | -------------------------------------------------------------------------------- /samples/slave/slave.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef SLAVE_H 17 | #define SLAVE_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #include 24 | 25 | int slave_init (const char * canif, int bitrate); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif /* SLAVE_H */ 32 | -------------------------------------------------------------------------------- /samples/slaveinfo/main.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "slaveinfo.h" 17 | #include 18 | #include 19 | 20 | int main (int argc, char * argv[]) 21 | { 22 | uint8_t node; 23 | uint32_t bitrate; 24 | 25 | if (argc != 4) 26 | { 27 | printf ("usage: %s \n", argv[0]); 28 | return -1; 29 | } 30 | 31 | node = atoi (argv[2]); 32 | bitrate = atoi (argv[3]); 33 | 34 | return slaveinfo (argv[1], node, bitrate); 35 | } 36 | -------------------------------------------------------------------------------- /samples/slaveinfo/slaveinfo.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "slaveinfo.h" 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include "osal.h" 24 | 25 | int slaveinfo (const char * canif, uint8_t node, int bitrate) 26 | { 27 | co_net_t * net; 28 | co_cfg_t cfg = {0}; 29 | co_client_t * client; 30 | static const co_obj_t od_none[] = { 31 | {0, OTYPE_NULL, 0, NULL, NULL}, 32 | }; 33 | 34 | cfg.node = node; 35 | cfg.bitrate = bitrate; 36 | cfg.od = od_none; 37 | 38 | net = co_init (canif, &cfg); 39 | if (net == NULL) 40 | { 41 | printf ("Init failed\n"); 42 | return -1; 43 | } 44 | 45 | client = co_client_init (net); 46 | if (client == NULL) 47 | { 48 | printf ("Client init failed\n"); 49 | return -1; 50 | } 51 | 52 | co_nmt (client, CO_NMT_RESET_COMMUNICATION, 0); 53 | os_usleep (500 * 1000); /* TODO: how to sync with slave responses? */ 54 | 55 | node = co_node_next (client, 0); 56 | if (node == 0) 57 | { 58 | printf ("No nodes found\n"); 59 | return -1; 60 | } 61 | 62 | while (node > 0) 63 | { 64 | char s[80]; 65 | int n; 66 | 67 | n = co_sdo_read (client, node, 0x1008, 0, s, sizeof (s)); 68 | if (n > 0) 69 | { 70 | s[n] = 0; 71 | printf ("(%d) %s\n", node, s); 72 | } 73 | 74 | n = co_sdo_read (client, node, 0x1009, 0, s, sizeof (s)); 75 | if (n > 0) 76 | { 77 | s[n] = 0; 78 | printf ("(%d) %s\n", node, s); 79 | } 80 | 81 | n = co_sdo_read (client, node, 0x100a, 0, s, sizeof (s)); 82 | if (n > 0) 83 | { 84 | s[n] = 0; 85 | printf ("(%d) %s\n\n", node, s); 86 | } 87 | 88 | node = co_node_next (client, node + 1); 89 | } 90 | 91 | co_nmt (client, CO_NMT_PRE_OPERATIONAL, 0); 92 | co_nmt (client, CO_NMT_OPERATIONAL, 0); 93 | 94 | co_sync (client); 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /samples/slaveinfo/slaveinfo.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef SLAVEINFO_H 17 | #define SLAVEINFO_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #include 24 | 25 | int slaveinfo (const char * canif, uint8_t node, int bitrate); 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif /* SLAVEINFO_H */ 32 | -------------------------------------------------------------------------------- /src/co_bitmap.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #endif 18 | 19 | #include "co_bitmap.h" 20 | #include "osal.h" 21 | 22 | void co_bitmap_set (uint32_t * bm, int bit) 23 | { 24 | int ix = bit / 32; 25 | int offset = bit % 32; 26 | 27 | CC_ASSERT (bit < 128); 28 | bm[ix] |= BIT (offset); 29 | } 30 | 31 | void co_bitmap_clear (uint32_t * bm, int bit) 32 | { 33 | int ix = bit / 32; 34 | int offset = bit % 32; 35 | 36 | CC_ASSERT (bit < 128); 37 | bm[ix] &= ~BIT (offset); 38 | } 39 | 40 | int co_bitmap_get (uint32_t * bm, int bit) 41 | { 42 | int ix = bit / 32; 43 | int offset = bit % 32; 44 | 45 | CC_ASSERT (bit < 128); 46 | return (bm[ix] & BIT (offset)) ? 1 : 0; 47 | } 48 | 49 | int co_bitmap_next (uint32_t * bm, int bit) 50 | { 51 | int ix; 52 | int offset = bit % 32; 53 | 54 | CC_ASSERT (bit < 128); 55 | 56 | for (ix = bit / 32; ix < 4; ix++) 57 | { 58 | uint32_t mask = -(int)BIT (offset); 59 | if (bm[ix] & mask) 60 | return __builtin_ctz (bm[ix] & mask) + 32 * ix; 61 | offset = 0; 62 | } 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /src/co_bitmap.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief bitmap utility functions 19 | */ 20 | 21 | #ifndef CO_BITMAP_H 22 | #define CO_BITMAP_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include 29 | 30 | /** 31 | * Set a bit in a 128-bit bitmap. 32 | * 33 | * @param bm bitmap 34 | * @param bit number of bit to set (0 to 127) 35 | */ 36 | void co_bitmap_set (uint32_t * bm, int bit); 37 | 38 | /** 39 | * Clear a bit in a 128-bit bitmap. 40 | * 41 | * @param bm bitmap 42 | * @param bit number of bit to clear (0 to 127) 43 | */ 44 | void co_bitmap_clear (uint32_t * bm, int bit); 45 | 46 | /** 47 | * Get a bit from a 128-bit bitmap. 48 | * 49 | * @param bm bitmap 50 | * @param bit number of bit to get (0 to 127) 51 | */ 52 | int co_bitmap_get (uint32_t * bm, int bit); 53 | 54 | /** 55 | * Return the next set bit in a 128-bit bitmap 56 | * 57 | * This function returns the next bit that is set in the bitmap, and 58 | * can be used to iterate over all set bits. 59 | 60 | * @param bm bitmap 61 | * @param bit start bit (0 to 127) 62 | */ 63 | int co_bitmap_next (uint32_t * bm, int bit); 64 | 65 | #ifdef __cplusplus 66 | } 67 | #endif 68 | 69 | #endif /* CO_BITMAP_H */ 70 | -------------------------------------------------------------------------------- /src/co_emcy.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #define os_channel_send mock_os_channel_send 18 | #define os_channel_get_state mock_os_channel_get_state 19 | #define os_channel_bus_on mock_os_channel_bus_on 20 | #define os_tick_current mock_os_tick_current 21 | #define os_tick_from_us mock_os_tick_from_us 22 | #endif 23 | 24 | #include "co_emcy.h" 25 | #include "co_nmt.h" 26 | #include "co_sdo.h" 27 | #include "co_util.h" 28 | 29 | #include 30 | 31 | static uint32_t co_emcy_error_get (co_net_t * net, uint8_t subindex, uint32_t * value) 32 | { 33 | uint8_t ix; 34 | 35 | if (subindex == 0) 36 | { 37 | *value = net->number_of_errors; 38 | return 0; 39 | } 40 | 41 | /* Get error from list */ 42 | ix = subindex - 1; 43 | if (ix < net->number_of_errors) 44 | { 45 | *value = net->errors[ix]; 46 | return 0; 47 | } 48 | 49 | return CO_SDO_ABORT_NO_DATA; 50 | } 51 | 52 | static uint32_t co_emcy_error_set (co_net_t * net, uint8_t subindex, uint32_t * value) 53 | { 54 | /* Only subindex 0 is writable. Assert that this function is only 55 | called for subindex 0. */ 56 | CC_ASSERT (subindex == 0); 57 | 58 | if (*value != 0) 59 | return CO_SDO_ABORT_VALUE; 60 | 61 | net->number_of_errors = 0; 62 | return 0; 63 | } 64 | 65 | static void co_trigger_error_behavior (co_net_t * net) 66 | { 67 | /* Transition state according to error behavior setting */ 68 | switch (net->error_behavior) 69 | { 70 | case 0: 71 | if (net->state == STATE_OP) 72 | co_nmt_event (net, EVENT_PREOP); 73 | break; 74 | case 2: 75 | co_nmt_event (net, EVENT_STOP); 76 | break; 77 | default: 78 | /* Do nothing */ 79 | break; 80 | } 81 | } 82 | 83 | uint32_t co_od1001_fn ( 84 | co_net_t * net, 85 | od_event_t event, 86 | const co_obj_t * obj, 87 | const co_entry_t * entry, 88 | uint8_t subindex, 89 | uint32_t * value) 90 | { 91 | if (event == OD_EVENT_READ) 92 | *value = co_emcy_error_register_get (net); 93 | return 0; 94 | } 95 | 96 | uint32_t co_od1003_fn ( 97 | co_net_t * net, 98 | od_event_t event, 99 | const co_obj_t * obj, 100 | const co_entry_t * entry, 101 | uint8_t subindex, 102 | uint32_t * value) 103 | { 104 | switch (event) 105 | { 106 | case OD_EVENT_READ: 107 | return co_emcy_error_get (net, subindex, value); 108 | case OD_EVENT_WRITE: 109 | return co_emcy_error_set (net, subindex, value); 110 | case OD_EVENT_RESTORE: 111 | net->number_of_errors = 0; 112 | return 0; 113 | default: 114 | return CO_SDO_ABORT_GENERAL; 115 | } 116 | } 117 | 118 | uint32_t co_od1014_fn ( 119 | co_net_t * net, 120 | od_event_t event, 121 | const co_obj_t * obj, 122 | const co_entry_t * entry, 123 | uint8_t subindex, 124 | uint32_t * value) 125 | { 126 | switch (event) 127 | { 128 | case OD_EVENT_READ: 129 | *value = net->emcy.cobid; 130 | return 0; 131 | case OD_EVENT_WRITE: 132 | if (!co_validate_cob_id (*value)) 133 | return CO_SDO_ABORT_VALUE; 134 | net->emcy.cobid = *value; 135 | return 0; 136 | case OD_EVENT_RESTORE: 137 | net->emcy.cobid = net->node + 0x80; 138 | return 0; 139 | default: 140 | return CO_SDO_ABORT_GENERAL; 141 | } 142 | } 143 | 144 | uint32_t co_od1015_fn ( 145 | co_net_t * net, 146 | od_event_t event, 147 | const co_obj_t * obj, 148 | const co_entry_t * entry, 149 | uint8_t subindex, 150 | uint32_t * value) 151 | { 152 | switch (event) 153 | { 154 | case OD_EVENT_READ: 155 | *value = net->emcy.inhibit; 156 | return 0; 157 | case OD_EVENT_WRITE: 158 | net->emcy.inhibit = *value; 159 | return 0; 160 | case OD_EVENT_RESTORE: 161 | net->emcy.inhibit = 0; 162 | return 0; 163 | default: 164 | return CO_SDO_ABORT_GENERAL; 165 | } 166 | } 167 | 168 | uint32_t co_od1028_fn ( 169 | co_net_t * net, 170 | od_event_t event, 171 | const co_obj_t * obj, 172 | const co_entry_t * entry, 173 | uint8_t subindex, 174 | uint32_t * value) 175 | { 176 | uint32_t cobid; 177 | 178 | if (subindex == 0 && event != OD_EVENT_RESTORE) 179 | return CO_SDO_ABORT_BAD_SUBINDEX; 180 | 181 | switch (event) 182 | { 183 | case OD_EVENT_READ: 184 | *value = net->emcy.cobids[subindex - 1]; 185 | return 0; 186 | case OD_EVENT_WRITE: 187 | cobid = net->emcy.cobids[subindex - 1]; 188 | if (((cobid | *value) & CO_COBID_INVALID) == 0) 189 | return CO_SDO_ABORT_VALUE; 190 | net->emcy.cobids[subindex - 1] = *value; 191 | return 0; 192 | case OD_EVENT_RESTORE: 193 | for (int ix = 0; ix < MAX_EMCY_COBIDS; ix++) 194 | net->emcy.cobids[ix] = CO_COBID_INVALID; 195 | return 0; 196 | default: 197 | return CO_SDO_ABORT_GENERAL; 198 | } 199 | 200 | return 0; 201 | } 202 | 203 | uint32_t co_od1029_fn ( 204 | co_net_t * net, 205 | od_event_t event, 206 | const co_obj_t * obj, 207 | const co_entry_t * entry, 208 | uint8_t subindex, 209 | uint32_t * value) 210 | { 211 | switch (event) 212 | { 213 | case OD_EVENT_READ: 214 | if (subindex == 0) 215 | return CO_SDO_ABORT_BAD_SUBINDEX; 216 | *value = net->error_behavior; 217 | return 0; 218 | case OD_EVENT_WRITE: 219 | if (subindex == 0) 220 | return CO_SDO_ABORT_BAD_SUBINDEX; 221 | net->error_behavior = (uint8_t)*value; 222 | return 0; 223 | case OD_EVENT_RESTORE: 224 | net->error_behavior = 0; 225 | return 0; 226 | default: 227 | return CO_SDO_ABORT_GENERAL; 228 | } 229 | } 230 | 231 | int co_emcy_tx (co_net_t * net, uint16_t code, uint16_t info, uint8_t msef[5]) 232 | { 233 | uint8_t msg[8] = {0}; 234 | uint8_t * p = msg; 235 | uint8_t reg; 236 | os_tick_t now; 237 | bool error_behavior = false; 238 | 239 | if (net->number_of_errors < MAX_ERRORS) 240 | net->number_of_errors++; 241 | 242 | /* Move down previous errors. The oldest error is discarded. Note 243 | that memmove supports overlapping copy. */ 244 | memmove ( 245 | &net->errors[1], 246 | &net->errors[0], 247 | (net->number_of_errors - 1) * sizeof (net->errors[0])); 248 | 249 | net->errors[0] = info << 16 | code; 250 | 251 | reg = co_emcy_error_register_get (net); 252 | 253 | p = co_put_uint16 (p, code); 254 | p = co_put_uint8 (p, reg); 255 | 256 | if (msef != NULL) 257 | { 258 | p = co_put_uint8 (p, msef[0]); 259 | p = co_put_uint8 (p, msef[1]); 260 | p = co_put_uint8 (p, msef[2]); 261 | p = co_put_uint8 (p, msef[3]); 262 | p = co_put_uint8 (p, msef[4]); 263 | (void)p; 264 | } 265 | 266 | /* Send EMCY if inhibit time has expired */ 267 | now = os_tick_current(); 268 | if (co_is_expired (now, net->emcy.timestamp, 100 * net->emcy.inhibit)) 269 | { 270 | LOG_ERROR (CO_EMCY_LOG, "emcy %x\n", code); 271 | os_channel_send (net->channel, net->emcy.cobid, msg, sizeof (msg)); 272 | net->emcy.timestamp = now; 273 | } 274 | 275 | /* Call user callback, except for bus-off recovery, where it was 276 | * called at the actual bus-off event. */ 277 | if (net->cb_emcy && code != 0x8140) 278 | { 279 | error_behavior = net->cb_emcy (net, net->node, code, reg, msef); 280 | } 281 | 282 | /* Always trigger error behavior on the mandatory events, 283 | * otherwise, follow the callback return value. The bus-off 284 | * event was handled when it happened. */ 285 | if (code == 0x8130 || error_behavior) { 286 | co_trigger_error_behavior (net); 287 | } 288 | 289 | return 0; 290 | } 291 | 292 | int co_emcy_rx (co_net_t * net, uint32_t id, uint8_t * msg, size_t dlc) 293 | { 294 | uint16_t code; 295 | uint8_t reg; 296 | uint8_t msef[5]; 297 | int ix; 298 | 299 | if (dlc != 8) 300 | return -1; 301 | 302 | for (ix = 0; ix < MAX_EMCY_COBIDS; ix++) 303 | { 304 | uint32_t cobid = net->emcy.cobids[ix]; 305 | 306 | /* Check for matching COB ID */ 307 | if (cobid != id) 308 | continue; 309 | 310 | /* Consume this EMCY */ 311 | code = co_fetch_uint16 (&msg[0]); 312 | reg = co_fetch_uint8 (&msg[2]); 313 | msef[0] = co_fetch_uint8 (&msg[3]); 314 | msef[1] = co_fetch_uint8 (&msg[4]); 315 | msef[2] = co_fetch_uint8 (&msg[5]); 316 | msef[3] = co_fetch_uint8 (&msg[6]); 317 | msef[4] = co_fetch_uint8 (&msg[7]); 318 | 319 | /* Call user callback */ 320 | if (net->cb_emcy) 321 | { 322 | net->cb_emcy (net, CO_NODE_GET (id), code, reg, msef); 323 | } 324 | } 325 | 326 | return 0; 327 | } 328 | 329 | void co_emcy_handle_can_state (co_net_t * net) 330 | { 331 | int status; 332 | os_tick_t now = os_tick_current(); 333 | os_channel_state_t previous = net->emcy.state; 334 | 335 | /* Get current state */ 336 | status = os_channel_get_state (net->channel, &net->emcy.state); 337 | if (status != 0) 338 | return; 339 | 340 | /* Check for new communication errors */ 341 | 342 | if (net->emcy.state.overrun && !previous.overrun) 343 | { 344 | /* CAN overrun */ 345 | co_emcy_error_register_set (net, CO_ERR_COMMUNICATION); 346 | co_emcy_tx (net, 0x8110, 0, NULL); 347 | } 348 | 349 | if (net->emcy.state.error_passive && !previous.error_passive) 350 | { 351 | /* CAN in error passive mode */ 352 | co_emcy_error_register_set (net, CO_ERR_COMMUNICATION); 353 | co_emcy_tx (net, 0x8120, 0, NULL); 354 | } 355 | 356 | if (net->emcy.state.bus_off && !previous.bus_off) 357 | { 358 | /* Entered bus off */ 359 | co_emcy_error_register_set (net, CO_ERR_COMMUNICATION); 360 | net->emcy.bus_off_timestamp = now; 361 | 362 | /* Call user callback directly, cannot call co_emcy_tx() now */ 363 | if (net->cb_emcy) 364 | { 365 | net->cb_emcy (net, net->node, 0x8140, 366 | co_emcy_error_register_get(net), NULL); 367 | } 368 | 369 | co_trigger_error_behavior (net); 370 | } 371 | 372 | if (!net->emcy.state.bus_off && previous.bus_off) 373 | { 374 | /* Recovered from bus off */ 375 | co_emcy_tx (net, 0x8140, 0, NULL); 376 | } 377 | 378 | /* Attempt to go bus on again. */ 379 | if (net->emcy.state.bus_off && net->restart_ms > 0 && 380 | co_is_expired (now, net->emcy.bus_off_timestamp, 1000 * net->restart_ms)) 381 | { 382 | os_channel_bus_on(net->channel); 383 | net->emcy.bus_off_timestamp = now; 384 | } 385 | 386 | /* Clear communication error state if all sub-errors are inactive */ 387 | if ( 388 | !net->emcy.state.overrun && !net->emcy.state.error_passive && 389 | !net->emcy.state.bus_off && !net->emcy.node_guard_error && 390 | !net->emcy.heartbeat_error && !net->emcy.rpdo_timeout) 391 | { 392 | co_emcy_error_register_clear (net, CO_ERR_COMMUNICATION); 393 | } 394 | } 395 | 396 | void co_emcy_error_register_set (co_net_t * net, uint8_t mask) 397 | { 398 | net->emcy.error |= mask; 399 | } 400 | 401 | void co_emcy_error_register_clear (co_net_t * net, uint8_t mask) 402 | { 403 | uint8_t previous = net->emcy.error; 404 | 405 | net->emcy.error &= ~mask; 406 | 407 | if (previous & mask) 408 | { 409 | /* Notify that an error was reset */ 410 | co_emcy_tx (net, 0, 0, NULL); 411 | } 412 | } 413 | 414 | uint8_t co_emcy_error_register_get (co_net_t * net) 415 | { 416 | uint8_t value = net->emcy.error; 417 | 418 | /* Set generic error if any other error is active */ 419 | if (value != 0) 420 | value |= CO_ERR_GENERIC; 421 | 422 | return value; 423 | } 424 | 425 | void co_emcy_job (co_net_t * net, co_job_t * job) 426 | { 427 | switch (job->type) 428 | { 429 | case CO_JOB_EMCY_TX: 430 | co_emcy_tx (net, job->emcy.code, job->emcy.info, job->emcy.msef); 431 | break; 432 | case CO_JOB_ERROR_SET: 433 | co_emcy_error_register_set (net, job->emcy.value); 434 | break; 435 | case CO_JOB_ERROR_CLEAR: 436 | co_emcy_error_register_clear (net, job->emcy.value); 437 | break; 438 | case CO_JOB_ERROR_GET: 439 | job->emcy.value = co_emcy_error_register_get (net); 440 | break; 441 | default: 442 | CC_ASSERT (0); 443 | } 444 | 445 | job->result = 0; 446 | if (job->callback) 447 | job->callback (job); 448 | } 449 | -------------------------------------------------------------------------------- /src/co_emcy.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles emergency object (EMCY) 19 | */ 20 | 21 | #ifndef CO_EMCY_H 22 | #define CO_EMCY_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_main.h" 29 | 30 | /** 31 | * Transmit emergency object (EMCY) 32 | * 33 | * This function transmits an emergency object. An optional 34 | * manufacturer-speficic error code can be included. 35 | * 36 | * Calling this function adds an error to the error history object 37 | * (1003h). It also signals an error to the NMT state-machine which 38 | * may change state according to the setting of the error behavior 39 | * object (1029h). 40 | * 41 | * The application will be notified via the EMCY callback. The node id 42 | * will be the active node id for this node. 43 | * 44 | * @param net network handle 45 | * @param code error code, lower 16 bits of error number 46 | * @param info additional info, upper 16 bits of error number 47 | * @param msef manufacturer-specific error code or NULL 48 | * 49 | * @return 0 always 50 | */ 51 | int co_emcy_tx (co_net_t * net, uint16_t code, uint16_t info, uint8_t msef[5]); 52 | 53 | /** 54 | * Receive emergency object (EMCY) 55 | * 56 | * This function should be called when an emergency object is 57 | * received. The application will be notified via the EMCY callback 58 | * function, if the EMCY object is being consumed. 59 | * 60 | * @param net network handle 61 | * @param id CAN ID 62 | * @param msg CAN message 63 | * @param dlc size of CAN message 64 | * 65 | * @return 0 on success, -1 on failure 66 | */ 67 | int co_emcy_rx (co_net_t * net, uint32_t node, uint8_t * msg, size_t dlc); 68 | 69 | /** 70 | * Handle CAN error states 71 | * 72 | * This function should be called periodically to check for CAN error 73 | * states. It generates emergencies for overrun, error passive mode 74 | * and bus off states. 75 | * 76 | * @param net network handle 77 | */ 78 | void co_emcy_handle_can_state (co_net_t * net); 79 | 80 | /** 81 | * Set error register bits 82 | * 83 | * This function sets bits in the CANopen error register. All bits can 84 | * be set, but note that the stack internally handles CO_ERR_GENERIC 85 | * and CO_ERR_COMMUNICATION. CO_ERR_GENERIC is set if any other bit is 86 | * set, and cleared otherwise. CO_ERR_COMMUNICATION is set if the 87 | * stack detects a communication problem. 88 | * 89 | * @param net network handle 90 | * @param mask bits to set 91 | */ 92 | void co_emcy_error_register_set (co_net_t * net, uint8_t mask); 93 | 94 | /** 95 | * Clear error register bits 96 | * 97 | * @param net network handle 98 | * @param mask bits to clear 99 | */ 100 | void co_emcy_error_register_clear (co_net_t * net, uint8_t mask); 101 | 102 | /** 103 | * Get error register value 104 | * 105 | * @param net network handle 106 | * 107 | * @return error register value 108 | */ 109 | uint8_t co_emcy_error_register_get (co_net_t * net); 110 | 111 | /** 112 | * Start emergency job 113 | * 114 | * @param net network handle 115 | * @param job emcy job 116 | */ 117 | void co_emcy_job (co_net_t * net, co_job_t * job); 118 | 119 | #ifdef __cplusplus 120 | } 121 | #endif 122 | 123 | #endif /* CO_EMCY_H */ 124 | -------------------------------------------------------------------------------- /src/co_heartbeat.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #define os_channel_send mock_os_channel_send 18 | #define os_tick_current mock_os_tick_current 19 | #define os_tick_from_us mock_os_tick_from_us 20 | #define co_emcy_tx mock_co_emcy_tx 21 | #endif 22 | 23 | #include "co_heartbeat.h" 24 | #include "co_sdo.h" 25 | #include "co_emcy.h" 26 | #include "co_util.h" 27 | #include "co_bitmap.h" 28 | 29 | #include 30 | 31 | uint32_t co_od1017_fn ( 32 | co_net_t * net, 33 | od_event_t event, 34 | const co_obj_t * obj, 35 | const co_entry_t * entry, 36 | uint8_t subindex, 37 | uint32_t * value) 38 | { 39 | switch (event) 40 | { 41 | case OD_EVENT_READ: 42 | *value = net->hb_time; 43 | return 0; 44 | case OD_EVENT_WRITE: 45 | net->hb_time = *value; 46 | return 0; 47 | case OD_EVENT_RESTORE: 48 | net->hb_time = 0; 49 | return 0; 50 | default: 51 | return CO_SDO_ABORT_GENERAL; 52 | } 53 | } 54 | 55 | uint32_t co_od1016_fn ( 56 | co_net_t * net, 57 | od_event_t event, 58 | const co_obj_t * obj, 59 | const co_entry_t * entry, 60 | uint8_t subindex, 61 | uint32_t * value) 62 | { 63 | co_heartbeat_t * heartbeat = &net->heartbeat[subindex - 1]; 64 | 65 | if (event != OD_EVENT_RESTORE) 66 | { 67 | if (subindex == 0 || subindex > MAX_HEARTBEATS) 68 | return CO_SDO_ABORT_BAD_SUBINDEX; 69 | } 70 | 71 | if (event == OD_EVENT_READ) 72 | { 73 | *value = heartbeat->node << 16 | heartbeat->time; 74 | } 75 | else if (event == OD_EVENT_WRITE) 76 | { 77 | uint8_t node = (*value >> 16) & 0xFF; 78 | uint16_t time = *value & 0xFFFF; 79 | int ix; 80 | 81 | if (node != 0) 82 | { 83 | for (ix = 0; ix < MAX_HEARTBEATS; ix++) 84 | { 85 | if (ix == subindex - 1) 86 | continue; /* Ignore this slot */ 87 | 88 | if (net->heartbeat[ix].node == node) 89 | return CO_SDO_ABORT_PARAM_INCOMPATIBLE; 90 | } 91 | } 92 | 93 | heartbeat->node = node; 94 | heartbeat->time = time; 95 | } 96 | else if (event == OD_EVENT_RESTORE) 97 | { 98 | memset (&net->heartbeat, 0, sizeof (net->heartbeat)); 99 | } 100 | 101 | return 0; 102 | } 103 | 104 | int co_heartbeat_rx (co_net_t * net, uint8_t node, void * msg, size_t dlc) 105 | { 106 | int ix; 107 | 108 | if (dlc != 1) 109 | return -1; 110 | 111 | co_bitmap_set (net->nodes, node); 112 | 113 | for (ix = 0; ix < MAX_HEARTBEATS; ix++) 114 | { 115 | if (net->heartbeat[ix].node == node) 116 | { 117 | co_heartbeat_t * heartbeat = &net->heartbeat[ix]; 118 | uint8_t state; 119 | 120 | heartbeat->timestamp = os_tick_current(); 121 | state = co_fetch_uint8 (msg); 122 | LOG_DEBUG ( 123 | CO_HEARTBEAT_LOG, 124 | "node %d got heartbeat state %02x\n", 125 | heartbeat->node, 126 | heartbeat->state); 127 | 128 | /* Check for node state change */ 129 | if (state != heartbeat->state) 130 | { 131 | LOG_DEBUG ( 132 | CO_HEARTBEAT_LOG, 133 | "node %d heartbeat state change\n", 134 | heartbeat->node); 135 | 136 | /* Call user callback */ 137 | if (net->cb_heartbeat_state) 138 | { 139 | net->cb_heartbeat_state (net, node, heartbeat->state, state); 140 | } 141 | 142 | /* Update node state */ 143 | heartbeat->state = state; 144 | } 145 | } 146 | } 147 | 148 | return 0; 149 | } 150 | 151 | int co_heartbeat_timer (co_net_t * net, os_tick_t now) 152 | { 153 | unsigned int ix; 154 | bool heartbeat_error = false; 155 | 156 | /* TODO: send on activation */ 157 | if (net->state == STATE_INIT) 158 | return -1; 159 | 160 | /* Heartbeat producer */ 161 | if (net->hb_time != 0) 162 | { 163 | if (co_is_expired (now, net->hb_timestamp, 1000 * net->hb_time)) 164 | { 165 | uint8_t msg[1]; 166 | uint8_t state; 167 | 168 | net->hb_timestamp = now; 169 | 170 | switch (net->state) 171 | { 172 | case STATE_STOP: 173 | state = 4; 174 | break; 175 | case STATE_OP: 176 | state = 5; 177 | break; 178 | case STATE_PREOP: 179 | state = 127; 180 | break; 181 | default: 182 | state = 0; 183 | break; 184 | } 185 | 186 | co_put_uint8 (msg, state); 187 | os_channel_send (net->channel, 0x700 + net->node, msg, sizeof (msg)); 188 | } 189 | } 190 | 191 | /* Heartbeat consumer */ 192 | for (ix = 0; ix < MAX_HEARTBEATS; ix++) 193 | { 194 | co_heartbeat_t * heartbeat = &net->heartbeat[ix]; 195 | 196 | /* Check active slot */ 197 | if (heartbeat->node == 0 || heartbeat->node > 127) 198 | continue; 199 | 200 | /* Check that heartbeat has not already expired */ 201 | if (heartbeat->state == 0) 202 | continue; 203 | 204 | /* Check heartbeat has not expired */ 205 | if (co_is_expired (now, heartbeat->timestamp, 1000 * heartbeat->time)) 206 | { 207 | /* Expired */ 208 | co_bitmap_clear (net->nodes, heartbeat->node); 209 | LOG_ERROR ( 210 | CO_HEARTBEAT_LOG, 211 | "node %d heartbeat expired\n", 212 | heartbeat->node); 213 | 214 | /* Call user callback */ 215 | if (net->cb_heartbeat_state) 216 | { 217 | net->cb_heartbeat_state (net, heartbeat->node, heartbeat->state, 0); 218 | } 219 | 220 | heartbeat->state = 0; 221 | heartbeat_error = true; 222 | 223 | co_emcy_error_register_set (net, CO_ERR_COMMUNICATION); 224 | co_emcy_tx (net, 0x8130, 0, NULL); 225 | } 226 | } 227 | 228 | /* Update heartbeat state */ 229 | net->emcy.heartbeat_error = heartbeat_error; 230 | 231 | return 0; 232 | } 233 | -------------------------------------------------------------------------------- /src/co_heartbeat.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles heartbeat protocol 19 | */ 20 | 21 | #ifndef CO_HEARTBEAT_H 22 | #define CO_HEARTBEAT_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | /** 32 | * Receive heartbeat message 33 | * 34 | * This function should be called when a heartbeat message is 35 | * received. The heartbeat for the sending node will be updated. 36 | * 37 | * 38 | * @param net network handle 39 | * @param node Node ID 40 | * @param msg CAN message 41 | * @param dlc size of CAN message 42 | * 43 | * @return 0 on success, -1 on failure 44 | */ 45 | int co_heartbeat_rx (co_net_t * net, uint8_t node, void * msg, size_t dlc); 46 | 47 | /** 48 | * Heartbeat timer 49 | * 50 | * This function performs the heartbeat producer and consumer 51 | * protocols and should be called periodically. An emergency will be 52 | * triggered if a consumed heartbeat has expired. 53 | * 54 | * @param net network handle 55 | * @param now current timestamp 56 | * 57 | * @return 0 on success, -1 on failure 58 | */ 59 | int co_heartbeat_timer (co_net_t * net, os_tick_t now); 60 | 61 | #ifdef __cplusplus 62 | } 63 | #endif 64 | 65 | #endif /* CO_HEARTBEAT_H */ 66 | -------------------------------------------------------------------------------- /src/co_log.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_log.h" 17 | #include "co_main.h" 18 | 19 | #include 20 | #include 21 | 22 | void co_msg_log (char * prefix, uint32_t id, const uint8_t * data, size_t dlc) 23 | { 24 | unsigned int ix; 25 | char s[80]; 26 | char * p = s; 27 | 28 | *p = '\0'; 29 | if (id & CO_RTR_MASK) 30 | { 31 | sprintf (p, " RTR %d", (int)dlc); 32 | } 33 | else 34 | { 35 | for (ix = 0; ix < dlc; ix++) 36 | { 37 | p += sprintf (p, " %02x", data[ix]); 38 | } 39 | } 40 | 41 | LOG_DEBUG (CO_CAN_LOG, "%s %04" PRIx32 ":%s\n", prefix, id & CO_ID_MASK, s); 42 | } 43 | -------------------------------------------------------------------------------- /src/co_log.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief CAN message logging utilities 19 | */ 20 | 21 | #ifndef CO_LOG_H 22 | #define CO_LOG_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include 29 | #include 30 | 31 | /** 32 | * Log CAN message 33 | * 34 | * This function logs CAN messages. 35 | * 36 | * @param prefix Prefix of message 37 | * @param id CAN ID 38 | * @param data CAN message 39 | * @param dlc size of CAN message 40 | */ 41 | void co_msg_log (char * prefix, uint32_t id, const uint8_t * data, size_t dlc); 42 | 43 | #ifdef __cplusplus 44 | } 45 | #endif 46 | 47 | #endif /* CO_LOG_H */ 48 | -------------------------------------------------------------------------------- /src/co_lss.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handle LSS protocol 19 | */ 20 | 21 | #ifndef CO_LSS_H 22 | #define CO_LSS_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | /** 32 | * Get LSS persistent node ID 33 | * 34 | * This function returns the persistent (configured) node ID. 35 | * 36 | * @param net network handle 37 | * 38 | * @return persistent node ID 39 | */ 40 | uint8_t co_lss_get_persistent_node_id (co_net_t * net); 41 | 42 | /** 43 | * Get LSS persistent bitrate 44 | * 45 | * This function returns the persistent (configured) bitrate. 46 | * 47 | * @param net network handle 48 | * 49 | * @return persistent bitrate 50 | */ 51 | int co_lss_get_persistent_bitrate (co_net_t * net); 52 | 53 | /** 54 | * Receive LSS message 55 | * 56 | * This function handles the LSS protocol and should be called when an 57 | * LSS message is received. 58 | * 59 | * @param net network handle 60 | * @param id CAN ID 61 | * @param msg CAN message 62 | * @param dlc size of CAN message 63 | * 64 | * @return 0 on success, -1 otherwise 65 | */ 66 | int co_lss_rx (co_net_t * net, uint32_t id, uint8_t * msg, size_t dlc); 67 | 68 | /** 69 | * Initialise LSS state 70 | * 71 | * This function initialises the LSS state and should be called when 72 | * the stack is started. TODO: From NMT poweron? 73 | * 74 | * @param net network handle 75 | * @param id CAN ID 76 | * @param msg CAN message 77 | * @param dlc size of CAN message 78 | */ 79 | void co_lss_init (co_net_t * net); 80 | 81 | #ifdef __cplusplus 82 | } 83 | #endif 84 | 85 | #endif /* CO_LSS_H */ 86 | -------------------------------------------------------------------------------- /src/co_main.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Core definitions 19 | */ 20 | 21 | #ifndef CO_MAIN_H 22 | #define CO_MAIN_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "osal.h" 30 | #include "coal_can.h" 31 | #include "options.h" 32 | #include "osal_log.h" 33 | 34 | #include 35 | 36 | #define CO_BYTELENGTH(bitlength) (((bitlength) + 7) / 8) 37 | 38 | #define CO_RTR_MASK BIT (30) 39 | #define CO_EXT_MASK BIT (29) 40 | #define CO_ID_MASK 0x1FFFFFFF 41 | #define CO_EXTID_MASK (CO_EXT_MASK | CO_ID_MASK) 42 | 43 | #define CO_COBID_INVALID BIT (31) 44 | #define CO_NODE_GET(id) ((id)&0x7F) 45 | 46 | /* Pre-defined connection set (see CiA 301 chapter 7.3.3) */ 47 | #define CO_FUNCTION_NMT (0 << 7) 48 | #define CO_FUNCTION_SYNC (1 << 7) 49 | #define CO_FUNCTION_TIME (2 << 7) 50 | #define CO_FUNCTION_EMCY (1 << 7) 51 | #define CO_FUNCTION_PDO1_TX (3 << 7) 52 | #define CO_FUNCTION_PDO1_RX (4 << 7) 53 | #define CO_FUNCTION_PDO2_TX (5 << 7) 54 | #define CO_FUNCTION_PDO2_RX (6 << 7) 55 | #define CO_FUNCTION_PDO3_TX (7 << 7) 56 | #define CO_FUNCTION_PDO3_RX (8 << 7) 57 | #define CO_FUNCTION_PDO4_TX (9 << 7) 58 | #define CO_FUNCTION_PDO4_RX (10 << 7) 59 | #define CO_FUNCTION_SDO_TX (11 << 7) 60 | #define CO_FUNCTION_SDO_RX (12 << 7) 61 | #define CO_FUNCTION_NMT_ERR (14 << 7) 62 | #define CO_FUNCTION_LSS (15 << 7) 63 | #define CO_FUNCTION_MASK (15 << 7) 64 | 65 | /** 66 | * Process data object (PDO) 67 | */ 68 | typedef struct co_pdo 69 | { 70 | uint16_t number; 71 | uint32_t cobid; 72 | uint8_t transmission_type; 73 | uint8_t sync_start; 74 | uint8_t sync_counter; 75 | uint16_t inhibit_time; 76 | uint16_t event_timer; 77 | os_tick_t timestamp; 78 | uint64_t frame; 79 | size_t bitlength; 80 | uint8_t number_of_mappings; 81 | struct 82 | { 83 | bool queued : 1; 84 | bool sync_wait : 1; 85 | bool rpdo_monitoring : 1; 86 | bool rpdo_timeout : 1; 87 | }; 88 | uint32_t mappings[MAX_PDO_ENTRIES]; 89 | const co_obj_t * objs[MAX_PDO_ENTRIES]; 90 | const co_entry_t * entries[MAX_PDO_ENTRIES]; 91 | } co_pdo_t; 92 | 93 | typedef enum co_job_type 94 | { 95 | CO_JOB_NONE, 96 | CO_JOB_PERIODIC, 97 | CO_JOB_RX, 98 | CO_JOB_PDO_EVENT, 99 | CO_JOB_PDO_OBJ_EVENT, 100 | CO_JOB_SDO_READ, 101 | CO_JOB_SDO_WRITE, 102 | CO_JOB_SDO_UPLOAD, 103 | CO_JOB_SDO_DOWNLOAD, 104 | CO_JOB_EMCY_TX, 105 | CO_JOB_ERROR_SET, 106 | CO_JOB_ERROR_CLEAR, 107 | CO_JOB_ERROR_GET, 108 | CO_JOB_EXIT, 109 | } co_job_type_t; 110 | 111 | /** Parameters for SDO job */ 112 | typedef struct co_sdo_job 113 | { 114 | uint8_t node; 115 | uint16_t index; 116 | uint8_t subindex; 117 | uint8_t * data; 118 | uint64_t value; 119 | size_t remain; 120 | size_t total; 121 | struct 122 | { 123 | bool toggle : 1; 124 | bool cached : 1; 125 | }; 126 | } co_sdo_job_t; 127 | 128 | /** Parameters for emergency job */ 129 | typedef struct co_emcy_job 130 | { 131 | uint16_t code; 132 | uint16_t info; 133 | uint8_t * msef; 134 | uint8_t value; 135 | } co_emcy_job_t; 136 | 137 | /** Parameters for PDO job */ 138 | typedef struct co_pdo_job 139 | { 140 | uint16_t index; 141 | uint8_t subindex; 142 | } co_pdo_job_t; 143 | 144 | /** Generic job */ 145 | typedef struct co_job 146 | { 147 | co_job_type_t type; 148 | union 149 | { 150 | co_sdo_job_t sdo; 151 | co_emcy_job_t emcy; 152 | co_pdo_job_t pdo; 153 | }; 154 | os_tick_t timestamp; 155 | struct co_client * client; 156 | void (*callback) (struct co_job * job); 157 | int result; 158 | } co_job_t; 159 | 160 | /** Client state */ 161 | struct co_client 162 | { 163 | os_sem_t * sem; 164 | co_job_t job; 165 | co_net_t * net; 166 | }; 167 | 168 | /** Heartbeat consumer state */ 169 | typedef struct co_heartbeat 170 | { 171 | uint8_t node; 172 | uint8_t state; 173 | uint16_t time; 174 | os_tick_t timestamp; 175 | } co_heartbeat_t; 176 | 177 | /** Node guarding state */ 178 | typedef struct co_node_guard 179 | { 180 | bool is_alive; 181 | uint16_t guard_time; 182 | uint8_t life_time_factor; 183 | uint8_t toggle; 184 | os_tick_t timestamp; 185 | } co_node_guard_t; 186 | 187 | /** LSS states */ 188 | typedef enum lss_state 189 | { 190 | LSS_STATE_WAITING = 0, 191 | LSS_STATE_CONFIG = 1, 192 | } lss_state_t; 193 | 194 | /** LSS state */ 195 | typedef struct lss 196 | { 197 | lss_state_t state; 198 | int bitrate; 199 | uint8_t node; 200 | const co_obj_t * identity; 201 | uint8_t match; 202 | } lss_t; 203 | 204 | /** SYNC producer state */ 205 | typedef struct co_sync 206 | { 207 | uint32_t cobid; 208 | uint8_t counter; 209 | uint8_t overflow; 210 | uint32_t period; 211 | os_tick_t timestamp; 212 | } co_sync_t; 213 | 214 | /** EMCY state */ 215 | typedef struct co_emcy 216 | { 217 | uint32_t cobid; /**< EMCY COB ID */ 218 | os_tick_t timestamp; /**< Timestamp of last EMCY */ 219 | uint32_t bus_off_timestamp; /**< Timestamp of bus-off event */ 220 | uint16_t inhibit; /**< Inhibit time [100 us] */ 221 | uint8_t error; /**< Error register */ 222 | os_channel_state_t state; /**< CAN state */ 223 | bool node_guard_error; /**< Node guard error */ 224 | bool heartbeat_error; /**< Heartbeat error */ 225 | bool rpdo_timeout; /**< RPDO timeout */ 226 | uint32_t cobids[MAX_EMCY_COBIDS]; /**< EMCY consumer object */ 227 | } co_emcy_t; 228 | 229 | /** CANopen network state */ 230 | struct co_net 231 | { 232 | os_channel_t * channel; /**< CAN channel */ 233 | int bitrate; /**< CAN bitrate (bits per second) */ 234 | os_mbox_t * mbox; /**< Mailbox for job submission */ 235 | co_job_type_t job_periodic; /**< Static message for periodic job */ 236 | co_job_type_t job_rx; /**< Static message for rx job */ 237 | co_job_t job_sdo_server; /**< Current SDO server job */ 238 | co_job_t * job_client; /**< Pointer to current client job */ 239 | uint32_t nodes[4]; /**< Discovered nodes. 128-bit bitmap */ 240 | uint8_t node; /**< Node ID for this node */ 241 | co_emcy_t emcy; /**< EMCY state */ 242 | co_sync_t sync; /**< SYNC state */ 243 | co_state_t state; /**< NMT state */ 244 | os_tick_t hb_timestamp; /**< Heartbeat producer timestamp */ 245 | uint32_t hb_time; /**< Heartbeat producer time */ 246 | os_tick_t sync_timestamp; /**< Timestamp of last SYNC */ 247 | uint32_t sync_window; /**< Synchronous window length */ 248 | uint32_t restart_ms; /**< Delay before attempting to recover from bus-off */ 249 | co_pdo_t pdo_tx[MAX_TX_PDO]; /**< TPDOs */ 250 | co_pdo_t pdo_rx[MAX_RX_PDO]; /**< RPDOs */ 251 | co_node_guard_t node_guard; /**< Node guarding state */ 252 | co_heartbeat_t heartbeat[MAX_HEARTBEATS]; /**< Heartbeat consumer state */ 253 | uint8_t number_of_errors; /**< Number of active errors */ 254 | uint32_t errors[MAX_ERRORS]; /**< List of active errors */ 255 | uint8_t error_behavior; /**< Error behavior object */ 256 | uint32_t config_date; /**< Configuration date */ 257 | uint32_t config_time; /**< Configuration time */ 258 | uint8_t config_dirty; /**< Configuration has changed */ 259 | lss_t lss; /**< LSS state */ 260 | const co_obj_t * od; /**< Object dictionary */ 261 | const co_default_t * defaults; /**< Dictionary default values */ 262 | void * cb_arg; /**< Callback opaque argument */ 263 | uint32_t mbox_overrun; /**< Mailbox overruns (for debugging) */ 264 | 265 | /** Reset callback */ 266 | void (*cb_reset) (co_net_t * net); 267 | 268 | /** NMT callback */ 269 | void (*cb_nmt) (co_net_t * net, co_state_t state); 270 | 271 | /** SYNC callback */ 272 | void (*cb_sync) (co_net_t * net); 273 | 274 | /** EMCY callback */ 275 | bool (*cb_emcy) ( 276 | co_net_t * net, 277 | uint8_t node, 278 | uint16_t code, 279 | uint8_t reg, 280 | uint8_t msef[5]); 281 | 282 | /** Notify callback */ 283 | void (*cb_notify) (co_net_t * net, uint16_t index, uint8_t subindex); 284 | 285 | /** Heartbeat node state change callback */ 286 | void (*cb_heartbeat_state) ( 287 | co_net_t * net, 288 | uint8_t node, 289 | uint8_t old_state, 290 | uint8_t new_state); 291 | 292 | /** Function to open dictionary store */ 293 | void * (*open) (co_store_t store, co_mode_t mode); 294 | 295 | /** Function to read from dictionary store */ 296 | int (*read) (void * arg, void * data, size_t size); 297 | 298 | /** Function to write to dictionary store */ 299 | int (*write) (void * arg, const void * data, size_t size); 300 | 301 | /** Function to close dictionary store */ 302 | int (*close) (void * arg); 303 | }; 304 | 305 | #ifdef __cplusplus 306 | } 307 | #endif 308 | 309 | #endif /* CO_MAIN_H */ 310 | -------------------------------------------------------------------------------- /src/co_nmt.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #define os_channel_send mock_os_channel_send 18 | #define os_channel_bus_off mock_os_channel_bus_off 19 | #define os_channel_bus_on mock_os_channel_bus_on 20 | #define os_channel_set_bitrate mock_os_channel_set_bitrate 21 | #define os_channel_set_filter mock_os_channel_set_filter 22 | #define co_od_reset mock_co_od_reset 23 | #endif 24 | 25 | #include "co_nmt.h" 26 | #include "co_od.h" 27 | #include "co_pdo.h" 28 | #include "co_lss.h" 29 | 30 | typedef struct co_fsm 31 | { 32 | co_state_t next; 33 | co_fsm_event_t (*action) (co_net_t * net, co_fsm_event_t event); 34 | } co_fsm_t; 35 | 36 | typedef struct co_fsm_transition 37 | { 38 | co_state_t state; 39 | co_fsm_event_t event; 40 | co_state_t next; 41 | co_fsm_event_t (*action) (co_net_t * net, co_fsm_event_t event); 42 | } co_fsm_transition_t; 43 | 44 | static co_fsm_t fsm[STATE_LAST][EVENT_LAST]; 45 | 46 | static co_fsm_event_t co_nmt_reset_app (co_net_t * net, co_fsm_event_t event) 47 | { 48 | /* Reset application and manufacturer-specific area*/ 49 | co_od_reset (net, CO_STORE_MFG, 0x2000, 0x5FFF); 50 | co_od_reset (net, CO_STORE_APP, 0x6000, 0x9FFF); 51 | 52 | /* Copy persistent node-ID to pending node-ID */ 53 | net->lss.node = co_lss_get_persistent_node_id (net); 54 | 55 | return EVENT_INITDONE; 56 | } 57 | 58 | static co_fsm_event_t co_nmt_reset_comm (co_net_t * net, co_fsm_event_t event) 59 | { 60 | /* Reset communication */ 61 | co_od_reset (net, CO_STORE_COMM, 0x1000, 0x1FFF); 62 | os_channel_bus_off (net->channel); 63 | os_channel_set_bitrate (net->channel, net->bitrate); 64 | os_channel_set_filter (net->channel, NULL, 0); 65 | os_channel_bus_on (net->channel); 66 | 67 | if (net->lss.node != 0xFF) 68 | { 69 | /* Copy pending node-ID to active node-ID */ 70 | net->node = net->lss.node; 71 | return EVENT_INITDONE; 72 | } 73 | 74 | /* Remain in INIT_COMM state if pending node-ID invalid */ 75 | return EVENT_NONE; 76 | } 77 | 78 | static co_fsm_event_t co_nmt_bootup (co_net_t * net, co_fsm_event_t event) 79 | { 80 | uint8_t msg[] = {0}; 81 | os_channel_send ( 82 | net->channel, 83 | CO_FUNCTION_NMT_ERR + net->node, 84 | msg, 85 | sizeof (msg)); 86 | return EVENT_NONE; 87 | } 88 | 89 | static co_fsm_event_t co_nmt_start (co_net_t * net, co_fsm_event_t event) 90 | { 91 | co_pdo_mapping_init (net); 92 | co_pdo_trigger (net); 93 | return EVENT_NONE; 94 | } 95 | 96 | static co_fsm_event_t co_nmt_poweron (co_net_t * net, co_fsm_event_t event) 97 | { 98 | co_lss_init (net); 99 | 100 | /* Copy persistent bitrate to pending/active bitrate */ 101 | net->lss.bitrate = co_lss_get_persistent_bitrate (net); 102 | net->bitrate = net->lss.bitrate; 103 | 104 | return EVENT_INITDONE; 105 | } 106 | 107 | /* State transitions, CiA 301 chapter 7.3.2.1 and 7.3.2.2 */ 108 | const co_fsm_transition_t transitions[] = { 109 | {STATE_OFF, EVENT_RESET, STATE_INIT_PWRON, co_nmt_poweron}, 110 | {STATE_INIT_PWRON, EVENT_INITDONE, STATE_INIT_APP, co_nmt_reset_app}, 111 | {STATE_INIT_APP, EVENT_INITDONE, STATE_INIT_COMM, co_nmt_reset_comm}, 112 | {STATE_INIT_COMM, EVENT_INITDONE, STATE_PREOP, co_nmt_bootup}, 113 | {STATE_PREOP, EVENT_START, STATE_OP, co_nmt_start}, 114 | {STATE_OP, EVENT_PREOP, STATE_PREOP, NULL}, 115 | {STATE_PREOP, EVENT_STOP, STATE_STOP, NULL}, 116 | {STATE_STOP, EVENT_START, STATE_OP, co_nmt_start}, 117 | {STATE_STOP, EVENT_PREOP, STATE_PREOP, NULL}, 118 | {STATE_OP, EVENT_STOP, STATE_STOP, NULL}, 119 | {STATE_OP, EVENT_RESET, STATE_INIT_APP, co_nmt_reset_app}, 120 | {STATE_STOP, EVENT_RESET, STATE_INIT_APP, co_nmt_reset_app}, 121 | {STATE_PREOP, EVENT_RESET, STATE_INIT_APP, co_nmt_reset_app}, 122 | {STATE_OP, EVENT_RESETCOMM, STATE_INIT_COMM, co_nmt_reset_comm}, 123 | {STATE_STOP, EVENT_RESETCOMM, STATE_INIT_COMM, co_nmt_reset_comm}, 124 | {STATE_PREOP, EVENT_RESETCOMM, STATE_INIT_COMM, co_nmt_reset_comm}, 125 | }; 126 | 127 | const char * co_state_literals[] = { 128 | "STATE_OFF", 129 | "STATE_INIT_PWRON", 130 | "STATE_INIT_APP", 131 | "STATE_INIT_COMM", 132 | "STATE_PREOP", 133 | "STATE_OP", 134 | "STATE_STOP", 135 | }; 136 | 137 | void co_nmt_event (co_net_t * net, co_fsm_event_t event) 138 | { 139 | do 140 | { 141 | co_state_t previous = net->state; 142 | co_fsm_t * element = &fsm[previous][event]; 143 | 144 | /* Transition to next state */ 145 | net->state = element->next; 146 | LOG_INFO (CO_NMT_LOG, "state = %s\n", co_state_literals[net->state]); 147 | 148 | /* Perform state action */ 149 | event = (element->action) ? element->action (net, event) : EVENT_NONE; 150 | 151 | /* Call user callback if state has changed */ 152 | if (previous != net->state) 153 | { 154 | if (net->cb_nmt) 155 | { 156 | net->cb_nmt (net, net->state); 157 | } 158 | } 159 | 160 | } while (event != EVENT_NONE); 161 | } 162 | 163 | void co_nmt_init (co_net_t * net) 164 | { 165 | unsigned int i, j; 166 | 167 | /* Set FSM defaults */ 168 | for (i = 0; i < STATE_LAST; i++) 169 | { 170 | for (j = 0; j < EVENT_LAST; j++) 171 | { 172 | /* Stay in state, no action */ 173 | fsm[i][j].next = i; 174 | fsm[i][j].action = NULL; 175 | } 176 | } 177 | 178 | /* Set FSM transitions from table */ 179 | for (i = 0; i < NELEMENTS (transitions); i++) 180 | { 181 | const co_fsm_transition_t * t = &transitions[i]; 182 | fsm[t->state][t->event].next = t->next; 183 | fsm[t->state][t->event].action = t->action; 184 | } 185 | 186 | /* Transition from OFF to INIT on poweron */ 187 | net->state = STATE_OFF; 188 | co_nmt_event (net, EVENT_RESET); 189 | } 190 | 191 | int co_nmt_rx (co_net_t * net, uint32_t id, uint8_t * msg, size_t dlc) 192 | { 193 | co_nmt_cmd_t cmd; 194 | uint8_t node; 195 | co_fsm_event_t event; 196 | 197 | /* Check ID */ 198 | if (id != 0) 199 | return -1; 200 | 201 | /* Check size */ 202 | if (dlc != 2) 203 | return -1; 204 | 205 | cmd = msg[0]; 206 | node = msg[1]; 207 | 208 | /* Check recipient */ 209 | if (node != 0 && node != net->node) 210 | return -1; 211 | 212 | switch (cmd) 213 | { 214 | case CO_NMT_OPERATIONAL: 215 | event = EVENT_START; 216 | break; 217 | case CO_NMT_STOPPED: 218 | event = EVENT_STOP; 219 | break; 220 | case CO_NMT_PRE_OPERATIONAL: 221 | event = EVENT_PREOP; 222 | break; 223 | case CO_NMT_RESET_NODE: 224 | event = EVENT_RESET; 225 | if (net->cb_reset) 226 | { 227 | net->cb_reset (net); 228 | } 229 | break; 230 | case CO_NMT_RESET_COMMUNICATION: 231 | event = EVENT_RESETCOMM; 232 | break; 233 | default: 234 | LOG_ERROR (CO_NMT_LOG, "bad nmt command %x\n", cmd); 235 | return -1; 236 | } 237 | 238 | /* Trigger FSM transition */ 239 | co_nmt_event (net, event); 240 | 241 | return 0; 242 | } 243 | -------------------------------------------------------------------------------- /src/co_nmt.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles NMT state-machine 19 | */ 20 | 21 | #ifndef CO_NMT_H 22 | #define CO_NMT_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | /** 32 | * NMT state-machine events. 33 | * 34 | * NMT events trigger state transitions. See CiA 301 chapter 7.3.2.1. 35 | */ 36 | typedef enum co_fsm_event 37 | { 38 | EVENT_NONE, 39 | EVENT_POWERON, 40 | EVENT_INITDONE, 41 | EVENT_START, 42 | EVENT_PREOP, 43 | EVENT_STOP, 44 | EVENT_RESET, 45 | EVENT_RESETCOMM, 46 | EVENT_LAST 47 | } co_fsm_event_t; 48 | 49 | /** 50 | * Trigger NMT state transition 51 | * 52 | * This function triggers an NMT state transition according to the 53 | * event. 54 | * 55 | * @param net network handle 56 | * @param event NMT event 57 | */ 58 | void co_nmt_event (co_net_t * net, co_fsm_event_t event); 59 | 60 | /** 61 | * Receive NMT message 62 | * 63 | * This function handles the NMT protocols and should be called when 64 | * an NMT message is received. The NMT state-machine will change state 65 | * according to the message. 66 | * 67 | * @param net network handle 68 | * @param id CAN ID 69 | * @param msg CAN message 70 | * @param dlc size of CAN message 71 | * 72 | * @return 0 on success, -1 otherwise 73 | */ 74 | int co_nmt_rx (co_net_t * net, uint32_t id, uint8_t * msg, size_t dlc); 75 | 76 | /** 77 | * Initialise NMT state-machine 78 | * 79 | * This function initialises the NMT state-machine and should be 80 | * called when the stack is started. 81 | * 82 | * @param net network handle 83 | */ 84 | void co_nmt_init (co_net_t * net); 85 | 86 | #ifdef __cplusplus 87 | } 88 | #endif 89 | 90 | #endif /* CO_NMT_H */ 91 | -------------------------------------------------------------------------------- /src/co_node_guard.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #define os_channel_send mock_os_channel_send 18 | #define os_channel_receive mock_os_channel_receive 19 | #define os_tick_current mock_os_tick_current 20 | #define os_tick_from_us mock_os_tick_from_us 21 | #endif 22 | 23 | #include "co_node_guard.h" 24 | #include "co_sdo.h" 25 | #include "co_emcy.h" 26 | #include "co_util.h" 27 | 28 | uint32_t co_od100C_fn ( 29 | co_net_t * net, 30 | od_event_t event, 31 | const co_obj_t * obj, 32 | const co_entry_t * entry, 33 | uint8_t subindex, 34 | uint32_t * value) 35 | { 36 | switch (event) 37 | { 38 | case OD_EVENT_READ: 39 | *value = net->node_guard.guard_time; 40 | return 0; 41 | case OD_EVENT_WRITE: 42 | net->node_guard.guard_time = *value; 43 | return 0; 44 | case OD_EVENT_RESTORE: 45 | net->node_guard.guard_time = 0; 46 | return 0; 47 | default: 48 | return CO_SDO_ABORT_GENERAL; 49 | } 50 | } 51 | 52 | uint32_t co_od100D_fn ( 53 | co_net_t * net, 54 | od_event_t event, 55 | const co_obj_t * obj, 56 | const co_entry_t * entry, 57 | uint8_t subindex, 58 | uint32_t * value) 59 | { 60 | switch (event) 61 | { 62 | case OD_EVENT_READ: 63 | *value = net->node_guard.life_time_factor; 64 | return 0; 65 | case OD_EVENT_WRITE: 66 | net->node_guard.life_time_factor = *value; 67 | return 0; 68 | case OD_EVENT_RESTORE: 69 | net->node_guard.life_time_factor = 0; 70 | return 0; 71 | default: 72 | return CO_SDO_ABORT_GENERAL; 73 | } 74 | } 75 | 76 | int co_node_guard_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc) 77 | { 78 | uint8_t _msg[1]; 79 | uint8_t state; 80 | 81 | if (id != (CO_RTR_MASK | CO_FUNCTION_NMT_ERR | net->node)) 82 | return -1; 83 | 84 | if (dlc != 1) 85 | return -1; 86 | 87 | net->node_guard.is_alive = true; 88 | net->node_guard.timestamp = os_tick_current(); 89 | 90 | /* Heartbeat producer (heartbeat is prioritised over node guarding)*/ 91 | if (net->hb_time == 0) 92 | { 93 | switch (net->state) 94 | { 95 | case STATE_STOP: 96 | state = 4; 97 | break; 98 | case STATE_OP: 99 | state = 5; 100 | break; 101 | case STATE_PREOP: 102 | state = 127; 103 | break; 104 | default: 105 | state = 0; 106 | break; 107 | } 108 | 109 | co_put_uint8 (_msg, net->node_guard.toggle | state); 110 | os_channel_send (net->channel, 0x700 + net->node, _msg, sizeof (_msg)); 111 | net->node_guard.toggle = ~net->node_guard.toggle & 0x80; 112 | } 113 | 114 | return 0; 115 | } 116 | 117 | int co_node_guard_timer (co_net_t * net, os_tick_t now) 118 | { 119 | uint32_t guard_factor = 120 | (net->node_guard.guard_time * net->node_guard.life_time_factor); 121 | 122 | if (net->state == STATE_INIT) 123 | return -1; 124 | 125 | /* Node guarding */ 126 | if (guard_factor != 0 && net->node_guard.is_alive) 127 | { 128 | /* Check node guarding has not expired */ 129 | if (co_is_expired (now, net->node_guard.timestamp, 1000 * guard_factor)) 130 | { 131 | /* Expired */ 132 | net->node_guard.is_alive = false; 133 | LOG_ERROR (CO_NODE_GUARD_LOG, "node guarding expired\n"); 134 | 135 | net->emcy.node_guard_error = true; 136 | co_emcy_error_register_set (net, CO_ERR_COMMUNICATION); 137 | co_emcy_tx (net, 0x8130, 0, NULL); 138 | } 139 | } 140 | 141 | return 0; 142 | } 143 | -------------------------------------------------------------------------------- /src/co_node_guard.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles node guarding protocol 19 | */ 20 | 21 | #ifndef CO_NODE_GUARD_H 22 | #define CO_NODE_GUARD_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | /** 32 | * Receive node guard message 33 | * 34 | * This function should be called when a node guard message is 35 | * received. The node guard timestamp will be updated. 36 | * 37 | * @param net network handle 38 | * @param id CAN ID 39 | * @param msg CAN message 40 | * @param dlc size of CAN message 41 | * 42 | * @return 0 always 43 | */ 44 | int co_node_guard_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc); 45 | 46 | /** 47 | * Node guard timer 48 | * 49 | * This function performs the node guard protocol and should be called 50 | * periodically. An emergency will be triggered if node guarding has 51 | * expired. 52 | * 53 | * @param net network handle 54 | * @param now current timestamp 55 | * 56 | * @return 0 on success, -1 on failure 57 | */ 58 | int co_node_guard_timer (co_net_t * net, os_tick_t now); 59 | 60 | #ifdef __cplusplus 61 | } 62 | #endif 63 | 64 | #endif /* CO_NODE_GUARD_H */ 65 | -------------------------------------------------------------------------------- /src/co_obj.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_obj.h" 17 | 18 | /* Entry descriptor for Error register object (1001h) */ 19 | const co_entry_t OD1001[] = { 20 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 0, NULL}, 21 | }; 22 | 23 | /* Entry descriptor for Pre-defined error field object (1003h) */ 24 | const co_entry_t OD1003[] = { 25 | {0x00, OD_RW | OD_TRANSIENT, DTYPE_UNSIGNED8, 8, 0, NULL}, 26 | {0x01, OD_RO | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL}, 27 | }; 28 | 29 | /* Entry descriptor for COB-ID SYNC message object (1005h) */ 30 | const co_entry_t OD1005[] = { 31 | {0x00, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 32 | }; 33 | 34 | /* Entry descriptor for Communication cycle object (1006h) */ 35 | const co_entry_t OD1006[] = { 36 | {0x00, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 37 | }; 38 | 39 | /* Entry descriptor for Synchronous window length object (1007h) */ 40 | const co_entry_t OD1007[] = { 41 | {0x00, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 42 | }; 43 | 44 | /* Entry descriptor for Guard time object (100Ch) */ 45 | const co_entry_t OD100C[] = { 46 | {0x00, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 47 | }; 48 | 49 | /* Entry descriptor for Life time factor object (100Dh) */ 50 | const co_entry_t OD100D[] = { 51 | {0x00, OD_RW, DTYPE_UNSIGNED8, 8, 0, NULL}, 52 | }; 53 | 54 | /* Entry descriptor for Store Parameters object (1010h) */ 55 | const co_entry_t OD1010[] = { 56 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 4, NULL}, 57 | {0x01, OD_RW | OD_ARRAY | OD_TRANSIENT, DTYPE_UNSIGNED32, 32, 0, NULL}, 58 | }; 59 | 60 | /* Entry descriptor for Restore Default Parameters object (1011h) */ 61 | const co_entry_t OD1011[] = { 62 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 4, NULL}, 63 | {0x01, OD_RW | OD_ARRAY | OD_TRANSIENT, DTYPE_UNSIGNED32, 32, 0, NULL}, 64 | }; 65 | 66 | /* Entry descriptor for COB-ID EMCY object (1014h) */ 67 | const co_entry_t OD1014[] = { 68 | {0x00, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 69 | }; 70 | 71 | /* Entry descriptor for Inhibit time EMCY object (1015h) */ 72 | const co_entry_t OD1015[] = { 73 | {0x00, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 74 | }; 75 | 76 | /* Entry descriptor for Consumer heartbeat time object (1016h) */ 77 | const co_entry_t OD1016[] = { 78 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, MAX_HEARTBEATS, NULL}, 79 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL}, 80 | }; 81 | 82 | /* Entry descriptor for Producer heartbeat time object (1017h) */ 83 | const co_entry_t OD1017[] = { 84 | {0x00, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 85 | }; 86 | 87 | /* Entry descriptor for Synchronous counter overflow value object (1019h) */ 88 | const co_entry_t OD1019[] = { 89 | {0x00, OD_RW, DTYPE_UNSIGNED8, 8, 0, NULL}, 90 | }; 91 | 92 | /* Entry descriptor for Verify Configuration object (1020h) */ 93 | const co_entry_t OD1020[] = { 94 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 2, NULL}, 95 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL}, 96 | }; 97 | 98 | /* Entry descriptor for Emergency consumer object (1028h) */ 99 | const co_entry_t OD1028[] = { 100 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, MAX_EMCY_COBIDS, NULL}, 101 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL}, 102 | }; 103 | 104 | /* Entry descriptor for Error behavior object (1029h) */ 105 | const co_entry_t OD1029[] = { 106 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 1, NULL}, 107 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED8, 8, 0, NULL}, 108 | }; 109 | 110 | /* Entry descriptor for RPDO communication parameter object (1400h - 15FFh) */ 111 | const co_entry_t OD1400[] = { 112 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 5, NULL}, 113 | {0x01, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 114 | {0x02, OD_RW, DTYPE_UNSIGNED8, 8, 0, NULL}, 115 | {0x03, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 116 | {0x05, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 117 | }; 118 | 119 | /* Entry descriptor for RPDO mapping parameter object (1600h - 17FFh) */ 120 | const co_entry_t OD1600[] = { 121 | {0x00, OD_RW, DTYPE_UNSIGNED8, 8, MAX_PDO_ENTRIES, NULL}, 122 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL}, 123 | }; 124 | 125 | /* Entry descriptor for TPDO communication parameter object (1800h - 19FFh) */ 126 | const co_entry_t OD1800[] = { 127 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 6, NULL}, 128 | {0x01, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 129 | {0x02, OD_RW, DTYPE_UNSIGNED8, 8, 0, NULL}, 130 | {0x03, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 131 | {0x05, OD_RW, DTYPE_UNSIGNED16, 16, 0, NULL}, 132 | {0x06, OD_RW, DTYPE_UNSIGNED8, 8, 0, NULL}, 133 | }; 134 | 135 | /* Entry descriptor for RPDO mapping parameter object (1A00h - 1BFFh) */ 136 | const co_entry_t OD1A00[] = { 137 | {0x00, OD_RW, DTYPE_UNSIGNED8, 8, MAX_PDO_ENTRIES, NULL}, 138 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, NULL}, 139 | }; 140 | -------------------------------------------------------------------------------- /src/co_od.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Object dictionary interface 19 | */ 20 | 21 | #ifndef CO_OD_H 22 | #define CO_OD_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | #include "co_sdo.h" 31 | 32 | /** 33 | * Traverse and call function on all subindexes in an object. 34 | * 35 | * This function traverses all subindexes until \a fn returns a value 36 | * other than 0. 37 | * 38 | * This function can e.g. be used to search for an entry. 39 | * 40 | * @param net network handle 41 | * @param obj object descriptor 42 | * @param fn function to call 43 | * @param arg opaque argument for function 44 | * @param max_subindex max subindex for traversal 45 | * 46 | * @return entry descriptor for subindex where \a fn != 0, or NULL 47 | */ 48 | static inline const co_entry_t * co_obj_traverse ( 49 | co_net_t * net, 50 | const co_obj_t * obj, 51 | int (*fn) ( 52 | co_net_t * net, 53 | const co_obj_t * obj, 54 | const co_entry_t * entry, 55 | uintptr_t arg), 56 | uintptr_t arg, 57 | uint8_t max_subindex) 58 | { 59 | const co_entry_t * entry = obj->entries; 60 | uint8_t subindex; 61 | 62 | do 63 | { 64 | subindex = entry->subindex; 65 | if (fn (net, obj, entry, arg) != 0) 66 | return entry; 67 | entry++; 68 | } while (subindex < max_subindex); 69 | 70 | return NULL; 71 | } 72 | 73 | /** 74 | * Traverse and call function with state on all subindexes in an 75 | * object. 76 | * 77 | * This function traverses all subindexes and calls \a fn supplying 78 | * the current state. 79 | * 80 | * This function can e.g. be used to compute a property of the object, 81 | * such as the total bit-length of all subindexes. 82 | * 83 | * @param net network handle 84 | * @param obj object descriptor 85 | * @param fn function to call 86 | * @param arg opaque argument for function 87 | * @param state initial state 88 | * @param max_subindex max subindex for traversal 89 | * 90 | * @return final state 91 | */ 92 | static inline int co_obj_reduce ( 93 | co_net_t * net, 94 | const co_obj_t * obj, 95 | int (*fn) (co_net_t * net, const co_entry_t * entry, uintptr_t arg, int state), 96 | uintptr_t arg, 97 | int state, 98 | uint8_t max_subindex) 99 | { 100 | const co_entry_t * entry = obj->entries; 101 | uint8_t subindex; 102 | 103 | do 104 | { 105 | subindex = entry->subindex; 106 | state = fn (net, entry, arg, state); 107 | entry++; 108 | } while (subindex < max_subindex); 109 | 110 | return state; 111 | } 112 | 113 | /** 114 | * Set dictionary default values 115 | * 116 | * This function sets the default value for dictionary entries that 117 | * have default values. Only indices that are within the given minimum 118 | * and maximum values are considered. 119 | * 120 | * @param net network handle 121 | * @param min minimum index 122 | * @param max maximum index 123 | */ 124 | void co_od_set_defaults (co_net_t * net, uint16_t min, uint16_t max); 125 | 126 | /** 127 | * Zero dictionary values 128 | * 129 | * This function sets dictionary entries to zero. Only indices that 130 | * are within the given minimum and maximum values are considered. 131 | * 132 | * @param net network handle 133 | * @param min minimum index 134 | * @param max maximum index 135 | */ 136 | void co_od_zero (co_net_t * net, uint16_t min, uint16_t max); 137 | 138 | /** 139 | * Load dictionary from store 140 | * 141 | * This function loads dictionary values from a store. 142 | * 143 | * @param net network handle 144 | * @param store store identifier 145 | * 146 | * @return sdo abort code 147 | */ 148 | uint32_t co_od_load (co_net_t * net, co_store_t store); 149 | 150 | /** 151 | * Reset dictionary 152 | * 153 | * This function resets dictionary values related to the given 154 | * store. Only indices that are within the minimum and maximum values 155 | * are considered. The following operations will be performed: 156 | * 157 | * -# Zero values 158 | * -# Set default values 159 | * -# Load stored values 160 | * 161 | * @param net network handle 162 | * @param store store identifier 163 | * @param min minimum index 164 | * @param max maximum index 165 | */ 166 | void co_od_reset (co_net_t * net, co_store_t store, uint16_t min, uint16_t max); 167 | 168 | /** 169 | * Save dictionary in store 170 | * 171 | * This function saves dictionary values in a store. Only indices that 172 | * are within the minimum and maximum values are considered. 173 | * 174 | * @param net network handle 175 | * @param store store identifier 176 | * @param min minimum index 177 | * @param max maximum index 178 | * 179 | * @return sdo abort code 180 | */ 181 | uint32_t co_od_store (co_net_t * net, co_store_t store, uint16_t min, uint16_t max); 182 | 183 | /** 184 | * Find object in dictionary 185 | * 186 | * This function finds the object with the given index. 187 | * 188 | * @param net network handle 189 | * @param index index to find 190 | * 191 | * @return object descriptor, or NULL if not found 192 | */ 193 | const co_obj_t * co_obj_find (co_net_t * net, uint16_t index); 194 | 195 | /** 196 | * Find entry in object 197 | * 198 | * This function finds the entry descriptor with the given subindex. 199 | * 200 | * @param net network handle 201 | * @param obj object descriptor 202 | * @param subindex subindex to find 203 | * 204 | * @return entry descriptor, or NULL if not found 205 | */ 206 | const co_entry_t * co_entry_find ( 207 | co_net_t * net, 208 | const co_obj_t * obj, 209 | uint8_t subindex); 210 | 211 | /** 212 | * Get subindex pointer 213 | * 214 | * This function returns a pointer to the subindex value. Note that 215 | * accessing the value in this manner is not thread-safe. 216 | * 217 | * @param net network handle 218 | * @param obj object descriptor 219 | * @param entry entry descriptor 220 | * @param subindex subindex 221 | * @param ptr result on success 222 | * 223 | * @return 0 or CO_SDO_ABORT_GENERAL if object has no storage 224 | */ 225 | uint32_t co_od_get_ptr ( 226 | co_net_t * net, 227 | const co_obj_t * obj, 228 | const co_entry_t * entry, 229 | uint8_t subindex, 230 | uint8_t ** ptr); 231 | 232 | /** 233 | * Get subindex value 234 | * 235 | * This function returns the subindex value. The value is read 236 | * atomically. This function is thread-safe. 237 | * 238 | * @param net network handle 239 | * @param obj object descriptor 240 | * @param entry entry descriptor 241 | * @param subindex subindex 242 | * @param value value on success 243 | * 244 | * @return 0 or sdo abort code 245 | */ 246 | uint32_t co_od_get_value ( 247 | co_net_t * net, 248 | const co_obj_t * obj, 249 | const co_entry_t * entry, 250 | uint8_t subindex, 251 | uint64_t * value); 252 | 253 | /** 254 | * Set subindex value 255 | * 256 | * This function sets the subindex value. The value is written 257 | * atomically. This function is thread-safe. 258 | * 259 | * @param net network handle 260 | * @param obj object descriptor 261 | * @param entry entry descriptor 262 | * @param subindex subindex 263 | * @param value value to set 264 | * 265 | * @return 0 or sdo abort code 266 | */ 267 | uint32_t co_od_set_value ( 268 | co_net_t * net, 269 | const co_obj_t * obj, 270 | const co_entry_t * entry, 271 | uint8_t subindex, 272 | uint64_t value); 273 | 274 | /** 275 | * Trigger notification callback 276 | * 277 | * This functions triggers the notification callback of the subindex, 278 | * if any. 279 | * 280 | * @param net network handle 281 | * @param obj object descriptor 282 | * @param entry entry descriptor 283 | * @param subindex subindex 284 | */ 285 | void co_od_notify ( 286 | co_net_t * net, 287 | const co_obj_t * obj, 288 | const co_entry_t * entry, 289 | uint8_t subindex); 290 | 291 | #ifdef __cplusplus 292 | } 293 | #endif 294 | 295 | #endif /* CO_OD_H */ 296 | -------------------------------------------------------------------------------- /src/co_pdo.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles process data object (PDO) 19 | */ 20 | 21 | #ifndef CO_PDO_H 22 | #define CO_PDO_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | /** 32 | * @internal 33 | * Pack PDO into CAN message 34 | * 35 | * This function packs a PDO into a CAN message for transmission. This 36 | * function is exported from the module to simplify unit-testing. 37 | * 38 | * @param net network handle 39 | * @param pdo PDO to pack 40 | */ 41 | void co_pdo_pack (co_net_t * net, co_pdo_t * pdo); 42 | 43 | /** 44 | * @internal 45 | * Unpack PDO from CAN message 46 | * 47 | * This function unpacks a PDO from a received CAN message. This 48 | * function is exported from the module to simplify unit-testing. 49 | * 50 | * @param net network handle 51 | * @param pdo PDO to pack 52 | */ 53 | void co_pdo_unpack (co_net_t * net, co_pdo_t * pdo); 54 | 55 | /** 56 | * @internal 57 | * Initialise PDO mapping 58 | * 59 | * This function is called internally on an NMT reset event to 60 | * initialise the PDO mapping. It should only be called internally. 61 | * 62 | * @param net network handle 63 | * @param pdo PDO to pack 64 | */ 65 | void co_pdo_mapping_init (co_net_t * net); 66 | 67 | /** 68 | * Receive SYNC object 69 | * 70 | * This function handles synchronously triggered PDOs and should be 71 | * called when the SYNC object is received. Synchronous TPDOs will be 72 | * transmitted. Synchronous RPDOs will be delivered. 73 | * 74 | * @param net network handle 75 | * @param msg CAN message 76 | * @param dlc size of CAN message 77 | * 78 | * @return 0 on success, -1 on failure 79 | */ 80 | int co_pdo_sync (co_net_t * net, uint8_t * msg, size_t dlc); 81 | 82 | /** 83 | * Receive RPDO or remotely requested TPDO 84 | * 85 | * This function handles reception of RPDOs and remotely requested 86 | * TPDOs. 87 | * 88 | * @param net network handle 89 | * @param id CAN ID 90 | * @param msg CAN message 91 | * @param dlc size of CAN message 92 | */ 93 | void co_pdo_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc); 94 | 95 | /** 96 | * PDO timer 97 | * 98 | * This function handles TPDOs with event timer and should be called 99 | * periodically. The TPDO is transmitted if the timer has expired. 100 | * 101 | * @param net network handle 102 | * @param now current timestamp 103 | * 104 | * @return 0 on success, -1 on failure 105 | */ 106 | int co_pdo_timer (co_net_t * net, os_tick_t now); 107 | 108 | /** 109 | * PDO trigger 110 | * 111 | * This function triggers an event on event-driven and acyclic 112 | * TPDOs. Event-driven TPDOs will be transmitted immediately while 113 | * acyclic TPDOs will be queued for transmission at next SYNC. 114 | * 115 | * @param net network handle 116 | */ 117 | void co_pdo_trigger (co_net_t * net); 118 | 119 | /** 120 | * PDO trigger with object 121 | * 122 | * This function triggers an event on event-driven and acyclic 123 | * TPDOs that map the specified object. Event-driven TPDOs will be 124 | * transmitted immediately while acyclic TPDOs will be queued for 125 | * transmission at next SYNC. 126 | * 127 | * @param net network handle 128 | * @param index index 129 | * @param subindex subindex 130 | */ 131 | void co_pdo_trigger_with_obj (co_net_t * net, uint16_t index, uint8_t subindex); 132 | 133 | /** 134 | * Start PDO job 135 | * 136 | * @param net network handle 137 | * @param job emcy job 138 | */ 139 | void co_pdo_job (co_net_t * net, co_job_t * job); 140 | 141 | /** 142 | * Initialise PDOs 143 | * 144 | * @param net network handle 145 | * 146 | * @return 0 on success, -1 on failure 147 | */ 148 | int co_pdo_init (co_net_t * net); 149 | 150 | #ifdef __cplusplus 151 | } 152 | #endif 153 | 154 | #endif /* CO_PDO_H */ 155 | -------------------------------------------------------------------------------- /src/co_sdo.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles service data object (SDO) 19 | */ 20 | 21 | #ifndef CO_SDO_H 22 | #define CO_SDO_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | #define CO_SDO_xCS(v) ((v)&0xE0) 32 | #define CO_SDO_N(v) (((v) >> 2) & 0x03) 33 | #define CO_SDO_E BIT (1) 34 | #define CO_SDO_S BIT (0) 35 | 36 | #define CO_SDO_CCS_DOWNLOAD_SEG_REQ (0 << 5) 37 | #define CO_SDO_CCS_DOWNLOAD_INIT_REQ (1 << 5) 38 | #define CO_SDO_CCS_UPLOAD_INIT_REQ (2 << 5) 39 | #define CO_SDO_CCS_UPLOAD_SEG_REQ (3 << 5) 40 | 41 | #define CO_SDO_SCS_UPLOAD_SEG_RSP (0 << 5) 42 | #define CO_SDO_SCS_DOWNLOAD_SEG_RSP (1 << 5) 43 | #define CO_SDO_SCS_UPLOAD_INIT_RSP (2 << 5) 44 | #define CO_SDO_SCS_DOWNLOAD_INIT_RSP (3 << 5) 45 | 46 | #define CO_SDO_xCS_ABORT (4 << 5) 47 | 48 | #define CO_SDO_TOGGLE BIT (4) 49 | #define CO_SDO_N_SEG(v) (((v) >> 1) & 0x07) 50 | #define CO_SDO_C BIT (0) 51 | 52 | #define CO_SDO_INDEX(d) (d[1] << 8 | d[2]) 53 | #define CO_SDO_SUBINDEX(d) (d[3]) 54 | 55 | /** 56 | * Send SDO abort message 57 | * 58 | * This function sends an SDO abort message. 59 | * 60 | * @param net network handle 61 | * @param id COB ID 62 | * @param index failed index 63 | * @param subindex failed subindex 64 | * @param code abort code 65 | */ 66 | void co_sdo_abort ( 67 | co_net_t * net, 68 | uint16_t id, 69 | uint16_t index, 70 | uint8_t subindex, 71 | uint32_t code); 72 | 73 | /** 74 | * @internal 75 | * SDO toggle protocol 76 | * 77 | * This function handles the SDO toggle protocol. 78 | * 79 | * @param job job descriptor 80 | * @param type transfer type (first byte of message) 81 | */ 82 | int co_sdo_toggle_update (co_job_t * job, uint8_t type); 83 | 84 | /** 85 | * Receive SDO TX message 86 | * 87 | * This function should be called when an SDO TX message is 88 | * received. The SDO client will process the message. TODO: rename to 89 | * sdo_client_rx? 90 | * 91 | * @param net network handle 92 | * @param node Node ID 93 | * @param msg CAN message 94 | * @param dlc size of CAN message 95 | * 96 | * @return 0 always 97 | */ 98 | int co_sdo_tx (co_net_t * net, uint8_t node, void * msg, size_t dlc); 99 | 100 | /** 101 | * Receive SDO RX message 102 | * 103 | * This function should be called when an SDO RX message is 104 | * received. The SDO server will process the message. TODO: rename to 105 | * sdo_server_rx? 106 | * 107 | * @param net network handle 108 | * @param node Node ID 109 | * @param msg CAN message 110 | * @param dlc size of CAN message 111 | * 112 | * @return 0 always 113 | */ 114 | int co_sdo_rx (co_net_t * net, uint8_t node, void * msg, size_t dlc); 115 | 116 | /** 117 | * SDO server timer 118 | * 119 | * This function checks for SDO server timeouts and should be called 120 | * periodically. An SDO abort will be triggered if the timeout has 121 | * expired. 122 | * 123 | * @param net network handle 124 | * @param now current timestamp 125 | * 126 | * @return 0 on success, -1 on failure 127 | */ 128 | int co_sdo_server_timer (co_net_t * net, os_tick_t now); 129 | 130 | /** 131 | * SDO client timer 132 | * 133 | * This function checks for SDO client timeouts and should be called 134 | * periodically. An SDO abort will be triggered if the timeout has 135 | * expired. 136 | * 137 | * @param net network handle 138 | * @param now current timestamp 139 | * 140 | * @return 0 on success, -1 on failure 141 | */ 142 | int co_sdo_client_timer (co_net_t * net, os_tick_t now); 143 | 144 | /** 145 | * Issue SDO request 146 | * 147 | * This function issues an SDO request. It used to start an SDO client 148 | * transfer. The SDO parameters are specified in the SDO job. 149 | * 150 | * @param net network handle 151 | * @param job SDO job 152 | */ 153 | void co_sdo_issue (co_net_t * net, co_job_t * job); 154 | 155 | #ifdef __cplusplus 156 | } 157 | #endif 158 | 159 | #endif /* CO_SDO_H */ 160 | -------------------------------------------------------------------------------- /src/co_sdo_client.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #define os_channel_send mock_os_channel_send 18 | #define os_channel_receive mock_os_channel_receive 19 | #define co_obj_find mock_co_obj_find 20 | #define co_entry_find mock_co_entry_find 21 | #define os_tick_from_us mock_os_tick_from_us 22 | #endif 23 | 24 | #include "co_sdo.h" 25 | #include "co_od.h" 26 | #include "co_util.h" 27 | 28 | #include 29 | 30 | static void co_sdo_done (co_net_t * net) 31 | { 32 | co_job_t * job = net->job_client; 33 | 34 | net->job_client = NULL; 35 | 36 | if (job->callback) 37 | job->callback (job); 38 | } 39 | 40 | static int co_sdo_tx_upload_init_rsp ( 41 | co_net_t * net, 42 | uint8_t node, 43 | uint8_t type, 44 | uint8_t * data) 45 | { 46 | co_job_t * job = net->job_client; 47 | 48 | /* Complete if e = 1 */ 49 | if (type & CO_SDO_E) 50 | { 51 | size_t size = (type & CO_SDO_S) ? 4 - CO_SDO_N (type) : 4; 52 | 53 | size = MIN (job->sdo.remain, size); 54 | memcpy (job->sdo.data, &data[4], size); 55 | 56 | job->sdo.data += size; 57 | job->sdo.remain -= size; 58 | job->sdo.total += size; 59 | 60 | job->result = job->sdo.total; /* actual size */ 61 | co_sdo_done (net); 62 | return 1; 63 | } 64 | else 65 | { 66 | uint8_t msg[8] = {0}; 67 | size_t size = co_fetch_uint32 (&data[4]); 68 | size = MIN (job->sdo.remain, size); 69 | 70 | job->sdo.remain = size; 71 | job->sdo.toggle = 0; 72 | job->sdo.total = 0; 73 | 74 | msg[0] = CO_SDO_CCS_UPLOAD_SEG_REQ; 75 | 76 | os_channel_send (net->channel, 0x600 + node, msg, sizeof (msg)); 77 | } 78 | 79 | return 0; 80 | } 81 | 82 | static int co_sdo_tx_upload_seg_rsp ( 83 | co_net_t * net, 84 | uint8_t node, 85 | uint8_t type, 86 | uint8_t * data) 87 | { 88 | co_job_t * job = net->job_client; 89 | int error; 90 | 91 | error = co_sdo_toggle_update (job, type); 92 | if (error < 0) 93 | { 94 | co_sdo_abort ( 95 | net, 96 | 0x600 + net->node, 97 | job->sdo.index, 98 | job->sdo.subindex, 99 | CO_SDO_ABORT_TOGGLE); 100 | job->result = CO_STATUS_ERROR; 101 | co_sdo_done (net); 102 | return error; 103 | } 104 | 105 | size_t size = 7 - CO_SDO_N_SEG (type); 106 | size = MIN (job->sdo.remain, size); 107 | memcpy (job->sdo.data, &data[1], size); 108 | 109 | job->sdo.data += size; 110 | job->sdo.remain -= size; 111 | job->sdo.total += size; 112 | 113 | /* Complete if c = 1 */ 114 | if (type & CO_SDO_C) 115 | { 116 | job->result = job->sdo.total; 117 | co_sdo_done (net); 118 | return 1; 119 | } 120 | else 121 | { 122 | uint8_t msg[8] = {0}; 123 | 124 | msg[0] = CO_SDO_CCS_UPLOAD_SEG_REQ; 125 | if (job->sdo.toggle) 126 | msg[0] |= CO_SDO_TOGGLE; 127 | 128 | os_channel_send (net->channel, 0x600 + node, msg, sizeof (msg)); 129 | } 130 | 131 | return 0; 132 | } 133 | 134 | static int co_sdo_tx_download_init_rsp ( 135 | co_net_t * net, 136 | uint8_t node, 137 | uint8_t type, 138 | uint8_t * data) 139 | { 140 | co_job_t * job = net->job_client; 141 | 142 | if (job->sdo.remain == 0) 143 | { 144 | /* Complete */ 145 | job->result = job->sdo.total; 146 | co_sdo_done (net); 147 | return 1; 148 | } 149 | else 150 | { 151 | uint8_t msg[8] = {0}; 152 | 153 | size_t size = MIN (job->sdo.remain, 7); 154 | memcpy (&msg[1], job->sdo.data, size); 155 | 156 | msg[0] = CO_SDO_CCS_DOWNLOAD_SEG_REQ | ((7 - (size & 0x07)) << 1); 157 | if (size < 7) 158 | msg[0] |= CO_SDO_C; 159 | 160 | job->sdo.toggle = 0; 161 | 162 | job->sdo.data += size; 163 | job->sdo.remain -= size; 164 | job->sdo.total += size; 165 | 166 | os_channel_send (net->channel, 0x600 + node, msg, sizeof (msg)); 167 | } 168 | 169 | return 0; 170 | } 171 | 172 | static int co_sdo_tx_download_seg_rsp ( 173 | co_net_t * net, 174 | uint8_t node, 175 | uint8_t type, 176 | uint8_t * data) 177 | { 178 | co_job_t * job = net->job_client; 179 | int error; 180 | 181 | error = co_sdo_toggle_update (job, type); 182 | if (error < 0) 183 | { 184 | co_sdo_abort ( 185 | net, 186 | 0x600 + net->node, 187 | job->sdo.index, 188 | job->sdo.subindex, 189 | CO_SDO_ABORT_TOGGLE); 190 | job->result = CO_STATUS_ERROR; 191 | co_sdo_done (net); 192 | return error; 193 | } 194 | 195 | if (job->sdo.remain == 0) 196 | { 197 | /* Complete */ 198 | job->result = job->sdo.total; 199 | co_sdo_done (net); 200 | return 1; 201 | } 202 | else 203 | { 204 | uint8_t msg[8] = {0}; 205 | 206 | size_t size = MIN (job->sdo.remain, 7); 207 | memcpy (&msg[1], job->sdo.data, size); 208 | 209 | msg[0] = CO_SDO_CCS_DOWNLOAD_SEG_REQ | ((7 - (size & 0x07)) << 1); 210 | if (job->sdo.toggle) 211 | msg[0] |= CO_SDO_TOGGLE; 212 | if (size < 7) 213 | msg[0] |= CO_SDO_C; 214 | 215 | job->sdo.data += size; 216 | job->sdo.remain -= size; 217 | job->sdo.total += size; 218 | 219 | os_channel_send (net->channel, 0x600 + node, msg, sizeof (msg)); 220 | } 221 | 222 | return 0; 223 | } 224 | 225 | int co_sdo_tx (co_net_t * net, uint8_t node, void * msg, size_t dlc) 226 | { 227 | uint8_t * data = (uint8_t *)msg; 228 | uint8_t type; 229 | uint8_t scs; 230 | co_job_t * job = net->job_client; 231 | 232 | /* Check for ongoing job */ 233 | if (job == NULL) 234 | return -1; 235 | 236 | /* Check for correct response */ 237 | if (job->sdo.node != node) 238 | return -1; 239 | 240 | /* Check DLC - must be complete frame */ 241 | if (dlc != 8) 242 | { 243 | co_sdo_abort (net, 0x600 + net->node, 0, 0, CO_SDO_ABORT_GENERAL); 244 | job->result = CO_STATUS_ERROR; 245 | co_sdo_done (net); 246 | return -1; 247 | } 248 | 249 | type = data[0]; 250 | scs = CO_SDO_xCS (type); 251 | 252 | /* Check response type */ 253 | switch (scs) 254 | { 255 | case CO_SDO_SCS_UPLOAD_INIT_RSP: 256 | return co_sdo_tx_upload_init_rsp (net, node, type, data); 257 | 258 | case CO_SDO_SCS_UPLOAD_SEG_RSP: 259 | return co_sdo_tx_upload_seg_rsp (net, node, type, data); 260 | 261 | case CO_SDO_SCS_DOWNLOAD_INIT_RSP: 262 | return co_sdo_tx_download_init_rsp (net, node, type, data); 263 | 264 | case CO_SDO_SCS_DOWNLOAD_SEG_RSP: 265 | return co_sdo_tx_download_seg_rsp (net, node, type, data); 266 | 267 | case CO_SDO_xCS_ABORT: 268 | { 269 | uint32_t error = co_fetch_uint32 (&data[4]); 270 | (void)error; 271 | LOG_WARNING (CO_SDO_LOG, "sdo abort (%08" PRIx32 ")\n", error); 272 | job->result = CO_STATUS_ERROR; 273 | co_sdo_done (net); 274 | return 1; 275 | } 276 | 277 | default: 278 | co_sdo_abort (net, 0x600 + net->node, 0, 0, CO_SDO_ABORT_UNKNOWN); 279 | LOG_ERROR (CO_SDO_LOG, "sdo unknown command (%X)\n", scs); 280 | job->result = CO_STATUS_ERROR; 281 | co_sdo_done (net); 282 | return 1; 283 | } 284 | } 285 | 286 | void co_sdo_issue (co_net_t * net, co_job_t * job) 287 | { 288 | uint8_t msg[8] = {0}; 289 | 290 | net->job_client = job; 291 | job->sdo.total = 0; 292 | 293 | if (job->type == CO_JOB_SDO_READ) 294 | { 295 | msg[0] = CO_SDO_CCS_UPLOAD_INIT_REQ; 296 | } 297 | else 298 | { 299 | msg[0] = CO_SDO_CCS_DOWNLOAD_INIT_REQ; 300 | if (job->sdo.remain <= 4) 301 | { 302 | int n = 4 - job->sdo.remain; 303 | msg[0] |= (n << 2) | CO_SDO_E | CO_SDO_S; 304 | memcpy (&msg[4], job->sdo.data, job->sdo.remain); 305 | job->sdo.total = job->sdo.remain; 306 | job->sdo.remain = 0; 307 | } 308 | else 309 | { 310 | msg[0] |= CO_SDO_S; 311 | co_put_uint32 (&msg[4], job->sdo.remain); 312 | } 313 | } 314 | 315 | co_put_uint16 (&msg[1], job->sdo.index); 316 | co_put_uint8 (&msg[3], job->sdo.subindex); 317 | 318 | os_channel_send (net->channel, 0x600 + job->sdo.node, msg, sizeof (msg)); 319 | } 320 | 321 | int co_sdo_client_timer (co_net_t * net, os_tick_t now) 322 | { 323 | co_job_t * job = net->job_client; 324 | 325 | if (job == NULL) 326 | return 0; 327 | 328 | if (job->type == CO_JOB_SDO_READ || job->type == CO_JOB_SDO_WRITE) 329 | { 330 | if (co_is_expired (now, job->timestamp, 1000 * SDO_TIMEOUT)) 331 | { 332 | co_sdo_abort ( 333 | net, 334 | 0x580 + net->node, 335 | job->sdo.index, 336 | job->sdo.subindex, 337 | CO_SDO_ABORT_TIMEOUT); 338 | 339 | job->result = CO_STATUS_ERROR; 340 | co_sdo_done (net); 341 | } 342 | } 343 | 344 | return 0; 345 | } 346 | -------------------------------------------------------------------------------- /src/co_sync.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifdef UNIT_TEST 17 | #define os_channel_send mock_os_channel_send 18 | #define os_tick_from_us mock_os_tick_from_us 19 | #endif 20 | 21 | #include "co_sync.h" 22 | #include "co_pdo.h" 23 | #include "co_sdo.h" 24 | #include "co_util.h" 25 | 26 | uint32_t co_od1005_fn ( 27 | co_net_t * net, 28 | od_event_t event, 29 | const co_obj_t * obj, 30 | const co_entry_t * entry, 31 | uint8_t subindex, 32 | uint32_t * value) 33 | { 34 | co_sync_t * sync = &net->sync; 35 | 36 | switch (event) 37 | { 38 | case OD_EVENT_READ: 39 | *value = sync->cobid; 40 | return 0; 41 | 42 | case OD_EVENT_WRITE: 43 | if (!co_validate_cob_id (*value)) 44 | return CO_SDO_ABORT_VALUE; 45 | 46 | if ((*value & sync->cobid & BIT (30)) == 0) 47 | { 48 | sync->cobid = *value; 49 | sync->timestamp = os_tick_current(); 50 | return 0; 51 | } 52 | 53 | return CO_SDO_ABORT_GENERAL; 54 | 55 | case OD_EVENT_RESTORE: 56 | sync->cobid = 0x80; 57 | return 0; 58 | 59 | default: 60 | return CO_SDO_ABORT_GENERAL; 61 | } 62 | return 0; 63 | } 64 | 65 | uint32_t co_od1006_fn ( 66 | co_net_t * net, 67 | od_event_t event, 68 | const co_obj_t * obj, 69 | const co_entry_t * entry, 70 | uint8_t subindex, 71 | uint32_t * value) 72 | { 73 | co_sync_t * sync = &net->sync; 74 | 75 | switch (event) 76 | { 77 | case OD_EVENT_READ: 78 | *value = sync->period; 79 | return 0; 80 | 81 | case OD_EVENT_WRITE: 82 | sync->period = *value; 83 | return 0; 84 | 85 | case OD_EVENT_RESTORE: 86 | sync->period = 0; 87 | sync->counter = 1; 88 | return 0; 89 | 90 | default: 91 | return CO_SDO_ABORT_GENERAL; 92 | } 93 | return 0; 94 | } 95 | 96 | uint32_t co_od1019_fn ( 97 | co_net_t * net, 98 | od_event_t event, 99 | const co_obj_t * obj, 100 | const co_entry_t * entry, 101 | uint8_t subindex, 102 | uint32_t * value) 103 | { 104 | co_sync_t * sync = &net->sync; 105 | 106 | switch (event) 107 | { 108 | case OD_EVENT_READ: 109 | *value = sync->overflow; 110 | return 0; 111 | 112 | case OD_EVENT_WRITE: 113 | if (net->state == STATE_INIT) 114 | { 115 | sync->overflow = *value; 116 | return 0; 117 | } 118 | 119 | if (sync->period != 0) 120 | return CO_SDO_ABORT_WRITE_STATE_DENIED; 121 | 122 | if (*value == 1 || *value > 240) 123 | return CO_SDO_ABORT_VALUE; 124 | 125 | sync->overflow = *value; 126 | sync->timestamp = os_tick_current(); 127 | return 0; 128 | 129 | case OD_EVENT_RESTORE: 130 | sync->overflow = 0; 131 | return 0; 132 | 133 | default: 134 | return CO_SDO_ABORT_GENERAL; 135 | } 136 | return 0; 137 | } 138 | 139 | int co_sync_timer (co_net_t * net, os_tick_t now) 140 | { 141 | co_sync_t * sync = &net->sync; 142 | 143 | if (net->state != STATE_PREOP && net->state != STATE_OP) 144 | return -1; 145 | 146 | if ((sync->cobid & BIT (30)) == 0) 147 | return -1; 148 | 149 | /* Sync producer */ 150 | if (sync->period != 0) 151 | { 152 | if (co_is_expired (now, sync->timestamp, sync->period)) 153 | { 154 | sync->timestamp = now; 155 | 156 | if (sync->overflow) 157 | { 158 | uint8_t msg[1]; 159 | co_put_uint8 (msg, sync->counter); 160 | os_channel_send ( 161 | net->channel, 162 | sync->cobid & CO_EXTID_MASK, 163 | msg, 164 | sizeof (msg)); 165 | co_pdo_sync (net, msg, sizeof (msg)); 166 | 167 | if (sync->counter++ == sync->overflow) 168 | sync->counter = 1; 169 | } 170 | else 171 | { 172 | os_channel_send (net->channel, sync->cobid & CO_EXTID_MASK, NULL, 0); 173 | co_pdo_sync (net, NULL, 0); 174 | } 175 | 176 | /* Call user callback */ 177 | if (net->cb_sync) 178 | { 179 | net->cb_sync (net); 180 | } 181 | } 182 | } 183 | 184 | return 0; 185 | } 186 | -------------------------------------------------------------------------------- /src/co_sync.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | /** 17 | * @file 18 | * @brief Handles synchronization object (SYNC) 19 | */ 20 | 21 | #ifndef CO_SYNC_H 22 | #define CO_SYNC_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | #include "co_api.h" 29 | #include "co_main.h" 30 | 31 | /** 32 | * SYNC timer 33 | * 34 | * This function performs the SYNC producer protocol and should be 35 | * called periodically. 36 | * 37 | * @param net network handle 38 | * @param now current timestamp 39 | * 40 | * @return 0 on success, -1 on failure 41 | */ 42 | int co_sync_timer (co_net_t * net, os_tick_t now); 43 | 44 | #ifdef __cplusplus 45 | } 46 | #endif 47 | 48 | #endif /* CO_SYNC_H */ 49 | -------------------------------------------------------------------------------- /src/co_util.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef CO_UTIL_H 17 | #define CO_UTIL_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #include 24 | #include 25 | 26 | #include "osal.h" 27 | 28 | static inline int co_is_expired (os_tick_t now, os_tick_t timestamp, uint32_t timeout) 29 | { 30 | os_tick_t delta = now - timestamp; 31 | return delta >= os_tick_from_us(timeout); 32 | } 33 | 34 | static inline bool co_validate_cob_id (uint32_t id) 35 | { 36 | id = id & CO_ID_MASK; 37 | 38 | /* See CiA 301, chapter 7.3.5 */ 39 | 40 | if (id == 0) 41 | return false; 42 | else if (id >= 0x001 && id <= 0x07F) 43 | return false; 44 | else if (id >= 0x101 && id < 0x180) /* Allow 0x180 to support CiA 417 */ 45 | return false; 46 | else if (id >= 0x581 && id <= 0x5FF) 47 | return false; 48 | else if (id >= 0x601 && id <= 0x67F) 49 | return false; 50 | else if (id >= 0x6E0 && id <= 0x6FF) 51 | return false; 52 | else if (id >= 0x701 && id <= 0x77F) 53 | return false; 54 | else if (id >= 0x780 && id <= 0x7FF) 55 | return false; 56 | 57 | return true; 58 | } 59 | 60 | static inline bool co_is_padding (uint16_t index, uint8_t subindex) 61 | { 62 | const uint32_t bitmask = 0x0F7D00FE; 63 | 64 | /* See CiA 301, chapter 7.4.7.1 */ 65 | 66 | if (subindex != 0) 67 | return false; 68 | 69 | if (index > 0x1B) 70 | return false; 71 | 72 | return (BIT (index) & bitmask); 73 | } 74 | 75 | static inline uint8_t co_fetch_uint8 (const void * data) 76 | { 77 | uint8_t * p = (uint8_t *)data; 78 | return p[0]; 79 | } 80 | 81 | static inline uint16_t co_fetch_uint16 (const void * data) 82 | { 83 | uint16_t value; 84 | 85 | memcpy (&value, data, sizeof (value)); 86 | return CC_TO_LE16 (value); 87 | } 88 | 89 | static inline uint32_t co_fetch_uint32 (const void * data) 90 | { 91 | uint32_t value; 92 | 93 | memcpy (&value, data, sizeof (value)); 94 | return CC_TO_LE32 (value); 95 | } 96 | 97 | static inline uint64_t co_fetch_uint64 (const void * data) 98 | { 99 | uint64_t value; 100 | 101 | memcpy (&value, data, sizeof (value)); 102 | return CC_TO_LE64 (value); 103 | } 104 | 105 | static inline void * co_put_uint8 (void * data, uint8_t value) 106 | { 107 | uint8_t * p = (uint8_t *)data; 108 | 109 | *p = value; 110 | return p + sizeof (value); 111 | } 112 | 113 | static inline void * co_put_uint16 (void * data, uint16_t value) 114 | { 115 | uint8_t * p = (uint8_t *)data; 116 | 117 | value = CC_TO_LE16 (value); 118 | memcpy (data, &value, sizeof (value)); 119 | return p + sizeof (value); 120 | } 121 | 122 | static inline void * co_put_uint32 (void * data, uint32_t value) 123 | { 124 | uint8_t * p = (uint8_t *)data; 125 | 126 | value = CC_TO_LE32 (value); 127 | memcpy (data, &value, sizeof (value)); 128 | return p + sizeof (value); 129 | } 130 | 131 | static inline void * co_put_uint64 (void * data, uint64_t value) 132 | { 133 | uint8_t * p = (uint8_t *)data; 134 | 135 | value = CC_TO_LE64 (value); 136 | memcpy (data, &value, sizeof (value)); 137 | return p + sizeof (value); 138 | } 139 | 140 | static inline uint8_t co_atomic_get_uint8 (const void * data) 141 | { 142 | uint8_t * p = (uint8_t *)data; 143 | return CC_ATOMIC_GET8 (p); 144 | } 145 | 146 | static inline uint16_t co_atomic_get_uint16 (const void * data) 147 | { 148 | uint16_t * p = (uint16_t *)data; 149 | CC_ASSERT (((uintptr_t)p & 0x01) == 0); 150 | return CC_ATOMIC_GET16 (p); 151 | } 152 | 153 | static inline uint32_t co_atomic_get_uint32 (const void * data) 154 | { 155 | uint32_t * p = (uint32_t *)data; 156 | CC_ASSERT (((uintptr_t)p & 0x03) == 0); 157 | return CC_ATOMIC_GET32 (p); 158 | } 159 | 160 | static inline uint64_t co_atomic_get_uint64 (const void * data) 161 | { 162 | uint64_t * p = (uint64_t *)data; 163 | CC_ASSERT (((uintptr_t)p & 0x07) == 0); 164 | return CC_ATOMIC_GET64 (p); 165 | } 166 | 167 | static inline void co_atomic_set_uint8 (void * data, uint8_t value) 168 | { 169 | uint8_t * p = (uint8_t *)data; 170 | CC_ATOMIC_SET8 (p, value); 171 | } 172 | 173 | static inline void co_atomic_set_uint16 (void * data, uint16_t value) 174 | { 175 | uint16_t * p = (uint16_t *)data; 176 | CC_ASSERT (((uintptr_t)p & 0x01) == 0); 177 | CC_ATOMIC_SET16 (p, value); 178 | } 179 | 180 | static inline void co_atomic_set_uint32 (void * data, uint32_t value) 181 | { 182 | uint32_t * p = (uint32_t *)data; 183 | CC_ASSERT (((uintptr_t)p & 0x03) == 0); 184 | CC_ATOMIC_SET32 (p, value); 185 | } 186 | 187 | static inline void co_atomic_set_uint64 (void * data, uint64_t value) 188 | { 189 | uint64_t * p = (uint64_t *)data; 190 | CC_ASSERT (((uintptr_t)p & 0x07) == 0); 191 | CC_ATOMIC_SET64 (p, value); 192 | } 193 | 194 | #ifdef __cplusplus 195 | } 196 | #endif 197 | 198 | #endif /* CO_UTIL_H */ 199 | -------------------------------------------------------------------------------- /src/coal_can.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef COAL_CAN_H 17 | #define COAL_CAN_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "coal_can_sys.h" 28 | 29 | #ifndef OS_CHANNEL 30 | typedef void os_channel_t; 31 | #endif 32 | 33 | typedef struct os_channel_state 34 | { 35 | bool overrun; 36 | bool error_passive; 37 | bool bus_off; 38 | } os_channel_state_t; 39 | 40 | os_channel_t * os_channel_open (const char * name, void * callback, void * arg); 41 | int os_channel_send ( 42 | os_channel_t * channel, 43 | uint32_t id, 44 | const void * data, 45 | size_t dlc); 46 | int os_channel_send_rtr (os_channel_t * channel, uint32_t id, size_t dlc); 47 | int os_channel_receive ( 48 | os_channel_t * channel, 49 | uint32_t * id, 50 | void * data, 51 | size_t * dlc); 52 | int os_channel_set_bitrate (os_channel_t * channel, int bitrate); 53 | int os_channel_set_filter (os_channel_t * channel, uint8_t * filter, size_t size); 54 | int os_channel_bus_on (os_channel_t * channel); 55 | int os_channel_bus_off (os_channel_t * channel); 56 | int os_channel_get_state (os_channel_t * channel, os_channel_state_t * state); 57 | 58 | #ifdef __cplusplus 59 | } 60 | #endif 61 | 62 | #endif /* COAL_CAN_H */ 63 | -------------------------------------------------------------------------------- /src/ports/linux/coal_can.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "coal_can.h" 17 | #include "osal.h" 18 | #include "options.h" 19 | #include "osal_log.h" 20 | #include "co_log.h" 21 | #include "co_main.h" 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | static void os_channel_rx (void * arg) 38 | { 39 | os_channel_t * channel = arg; 40 | struct epoll_event ev, events[1]; 41 | int epollfd; 42 | int nfds; 43 | int n; 44 | 45 | epollfd = epoll_create1 (0); 46 | if (epollfd == -1) 47 | { 48 | LOG_ERROR (CO_CAN_LOG, "epoll_create1 failed\n"); 49 | return; 50 | } 51 | 52 | /* Create edge-triggered event on input */ 53 | ev.events = EPOLLIN | EPOLLET; 54 | ev.data.fd = channel->handle; 55 | if (epoll_ctl (epollfd, EPOLL_CTL_ADD, channel->handle, &ev) == -1) 56 | { 57 | LOG_ERROR (CO_CAN_LOG, "epoll_ctl failed\n"); 58 | return; 59 | } 60 | 61 | for (;;) 62 | { 63 | nfds = epoll_wait (epollfd, events, 1, -1); 64 | if (nfds == -1) 65 | { 66 | if (errno == EINTR) 67 | continue; 68 | 69 | LOG_ERROR (CO_CAN_LOG, "epoll_wait failed\n"); 70 | return; 71 | } 72 | 73 | for (n = 0; n < nfds; n++) 74 | { 75 | if (events[n].data.fd == channel->handle) 76 | { 77 | channel->callback (channel->arg); 78 | } 79 | } 80 | } 81 | } 82 | 83 | os_channel_t * os_channel_open (const char * name, void * callback, void * arg) 84 | { 85 | os_channel_t * channel = malloc (sizeof (*channel)); 86 | struct sockaddr_can addr; 87 | struct ifreq ifr; 88 | 89 | channel->handle = socket (PF_CAN, SOCK_RAW, CAN_RAW); 90 | if (channel->handle < 0) 91 | { 92 | free (channel); 93 | return NULL; 94 | } 95 | 96 | fcntl (channel->handle, F_SETFL, O_NONBLOCK); 97 | 98 | strcpy (ifr.ifr_name, name); 99 | if (ioctl (channel->handle, SIOCGIFINDEX, &ifr) < 0) 100 | { 101 | close (channel->handle); 102 | free (channel); 103 | return NULL; 104 | } 105 | 106 | addr.can_family = AF_CAN; 107 | addr.can_ifindex = ifr.ifr_ifindex; 108 | 109 | LOG_DEBUG (CO_CAN_LOG, "%s at index %d\n", name, ifr.ifr_ifindex); 110 | 111 | if (bind (channel->handle, (struct sockaddr *)&addr, sizeof (addr)) < 0) 112 | { 113 | close (channel->handle); 114 | free (channel); 115 | return NULL; 116 | } 117 | 118 | channel->callback = callback; 119 | channel->arg = arg; 120 | 121 | os_thread_create ("co_rx", 5, 1024, os_channel_rx, channel); 122 | return channel; 123 | } 124 | 125 | int os_channel_send (os_channel_t * channel, uint32_t id, const void * data, size_t dlc) 126 | { 127 | struct can_frame frame; 128 | int n; 129 | 130 | co_msg_log ("Tx", id, data, dlc); 131 | 132 | frame.can_id = id & CO_ID_MASK; 133 | frame.can_id |= (id & CO_RTR_MASK) ? CAN_RTR_FLAG : 0; 134 | frame.can_id |= (id & CO_EXT_MASK) ? CAN_EFF_FLAG : 0; 135 | frame.can_dlc = dlc; 136 | memcpy (frame.data, data, dlc); 137 | 138 | n = write (channel->handle, &frame, sizeof (struct can_frame)); 139 | if (n != sizeof (struct can_frame)) 140 | return -1; 141 | 142 | return 0; 143 | } 144 | 145 | int os_channel_receive ( 146 | os_channel_t * channel, 147 | uint32_t * id, 148 | void * data, 149 | size_t * dlc) 150 | { 151 | struct can_frame frame; 152 | int n; 153 | 154 | n = read (channel->handle, &frame, sizeof (struct can_frame)); 155 | if (n != sizeof (struct can_frame)) 156 | return -1; 157 | 158 | *id = frame.can_id; 159 | *id |= (frame.can_id & CAN_RTR_FLAG) ? CO_RTR_MASK : 0; 160 | *id |= (frame.can_id & CAN_EFF_FLAG) ? CO_EXT_MASK : 0; 161 | *dlc = frame.can_dlc; 162 | memcpy (data, frame.data, frame.can_dlc); 163 | 164 | co_msg_log ("Rx", *id, data, *dlc); 165 | 166 | return 0; 167 | } 168 | 169 | int os_channel_set_bitrate (os_channel_t * channel, int bitrate) 170 | { 171 | return 0; 172 | } 173 | 174 | int os_channel_set_filter (os_channel_t * channel, uint8_t * filter, size_t size) 175 | { 176 | return 0; 177 | } 178 | 179 | int os_channel_bus_on (os_channel_t * channel) 180 | { 181 | return 0; 182 | } 183 | 184 | int os_channel_bus_off (os_channel_t * channel) 185 | { 186 | return 0; 187 | } 188 | 189 | int os_channel_get_state (os_channel_t * channel, os_channel_state_t * state) 190 | { 191 | return 0; 192 | } 193 | -------------------------------------------------------------------------------- /src/ports/linux/coal_can_sys.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef COAL_CAN_SYS_H 17 | #define COAL_CAN_SYS_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #define OS_CHANNEL 24 | 25 | typedef struct 26 | { 27 | int handle; 28 | void (*callback) (void * arg); 29 | void * arg; 30 | } os_channel_t; 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif /* COAL_CAN_SYS_H */ 37 | -------------------------------------------------------------------------------- /src/ports/rt-kernel/coal_can.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "coal_can.h" 17 | #include "osal.h" 18 | #include "options.h" 19 | #include "osal_log.h" 20 | #include "co_log.h" 21 | #include "co_main.h" 22 | #include "co_rtk.h" 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | static void os_can_callback (void * arg, can_event_t event) 29 | { 30 | os_channel_t * channel = arg; 31 | 32 | if (channel->callback) 33 | channel->callback (channel->arg); 34 | } 35 | 36 | os_channel_t * os_channel_open (const char * name, void * callback, void * arg) 37 | { 38 | os_channel_t * channel = malloc (sizeof (*channel)); 39 | can_filter_t filter = { 40 | .id = 0xFFFFFFFF, 41 | .mask = 0, 42 | }; 43 | 44 | /* Create file descriptor for CAN bus */ 45 | channel->handle = open (name, O_RDWR); 46 | if (channel->handle < 0) 47 | { 48 | free (channel); 49 | return NULL; 50 | } 51 | 52 | channel->callback = callback; 53 | channel->arg = arg; 54 | 55 | can_set_callback ( 56 | channel->handle, 57 | os_can_callback, 58 | CAN_EVENT_MSG_RECEIVED, 59 | channel); 60 | 61 | can_filter (channel->handle, &filter); 62 | 63 | return channel; 64 | } 65 | 66 | int os_channel_send (os_channel_t * channel, uint32_t id, const void * data, size_t dlc) 67 | { 68 | can_frame_t frame = {}; 69 | 70 | co_msg_log ("Tx", id, data, dlc); 71 | 72 | frame.id = id & CO_ID_MASK; 73 | frame.id |= (id & CO_RTR_MASK) ? CAN_ID_RTR : 0; 74 | frame.id |= (id & CO_EXT_MASK) ? CAN_ID_EXT : 0; 75 | frame.id = id; 76 | frame.dlc = dlc; 77 | memcpy (frame.data, data, dlc); 78 | 79 | can_transmit (channel->handle, &frame); 80 | 81 | return 0; 82 | } 83 | 84 | int os_channel_receive ( 85 | os_channel_t * channel, 86 | uint32_t * id, 87 | void * data, 88 | size_t * dlc) 89 | { 90 | can_frame_t frame = {}; 91 | int result; 92 | 93 | result = can_receive (channel->handle, &frame); 94 | if (result != 0) 95 | return -1; 96 | 97 | *id = frame.id & CAN_ID_MASK; 98 | *id |= (frame.id & CAN_ID_RTR) ? CO_RTR_MASK : 0; 99 | *id |= (frame.id & CAN_ID_EXT) ? CO_EXT_MASK : 0; 100 | *dlc = MIN(8, frame.dlc); 101 | memcpy (data, frame.data, frame.dlc); 102 | 103 | co_msg_log ("Rx", *id, data, *dlc); 104 | 105 | return 0; 106 | } 107 | 108 | CC_ATTRIBUTE_WEAK void co_can_get_cfg (int bitrate, can_cfg_t * cfg) 109 | { 110 | ASSERT (0); 111 | } 112 | 113 | int os_channel_set_bitrate (os_channel_t * channel, int bitrate) 114 | { 115 | can_cfg_t cfg; 116 | 117 | /* Get can parameters from application */ 118 | co_can_get_cfg (bitrate, &cfg); 119 | 120 | /* Set can parameters */ 121 | can_set_cfg (channel->handle, &cfg); 122 | 123 | return 0; 124 | } 125 | 126 | int os_channel_set_filter (os_channel_t * channel, uint8_t * filter, size_t size) 127 | { 128 | return 0; 129 | } 130 | 131 | int os_channel_bus_on (os_channel_t * channel) 132 | { 133 | can_bus_on (channel->handle); 134 | return 0; 135 | } 136 | 137 | int os_channel_bus_off (os_channel_t * channel) 138 | { 139 | can_bus_off (channel->handle); 140 | return 0; 141 | } 142 | 143 | int os_channel_get_state (os_channel_t * channel, os_channel_state_t * state) 144 | { 145 | can_status_t can_status; 146 | 147 | can_get_status (channel->handle, &can_status); 148 | 149 | state->overrun = can_status.flags & CAN_STATUS_BUFFER_OVERFLOW; 150 | state->error_passive = can_status.flags & CAN_STATUS_ERROR_PASSIVE; 151 | state->bus_off = can_status.flags & CAN_STATUS_BUS_OFF; 152 | 153 | return 0; 154 | } 155 | -------------------------------------------------------------------------------- /src/ports/rt-kernel/coal_can_sys.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef COAL_CAN_SYS_H 17 | #define COAL_CAN_SYS_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #define OS_CHANNEL 24 | 25 | typedef struct 26 | { 27 | int handle; 28 | void (*callback) (void * arg); 29 | void * arg; 30 | } os_channel_t; 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif /* COAL_CAN_SYS_H */ 37 | -------------------------------------------------------------------------------- /src/ports/windows/coal_can.c: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "coal_can.h" 17 | #include "osal.h" 18 | #include "options.h" 19 | #include "osal_log.h" 20 | #include "co_log.h" 21 | #include "co_main.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | static void CANLIBAPI 30 | os_can_callback (CanHandle handle, void * context, unsigned int event) 31 | { 32 | os_channel_t * channel = context; 33 | 34 | if (channel->callback) 35 | channel->callback (channel->arg); 36 | } 37 | 38 | os_channel_t * os_channel_open (const char * name, void * callback, void * arg) 39 | { 40 | os_channel_t * channel = malloc (sizeof (*channel)); 41 | unsigned long ix = strtoul (name, NULL, 0); 42 | 43 | canInitializeLibrary(); 44 | 45 | channel->handle = canOpenChannel (ix, 0); 46 | if (channel->handle < 0) 47 | { 48 | free (channel); 49 | return NULL; 50 | } 51 | 52 | channel->callback = callback; 53 | channel->arg = arg; 54 | 55 | kvSetNotifyCallback (channel->handle, os_can_callback, channel, canNOTIFY_RX); 56 | 57 | return channel; 58 | } 59 | 60 | int os_channel_send (os_channel_t * channel, uint32_t id, const void * data, size_t dlc) 61 | { 62 | canStatus status; 63 | uint32_t flags = 0; 64 | 65 | co_msg_log ("Tx", id, data, dlc); 66 | 67 | flags |= (id & CO_RTR_MASK) ? canMSG_RTR : 0; 68 | flags |= (id & CO_EXT_MASK) ? canMSG_EXT : 0; 69 | 70 | status = canWrite (channel->handle, id & CO_ID_MASK, (void *)data, dlc, flags); 71 | if (status < canOK) 72 | return status; 73 | 74 | return 0; 75 | } 76 | 77 | int os_channel_receive ( 78 | os_channel_t * channel, 79 | uint32_t * id, 80 | void * data, 81 | size_t * dlc) 82 | { 83 | canStatus status; 84 | unsigned int flags; 85 | long id_api; 86 | unsigned int dlc_api; 87 | 88 | status = canRead (channel->handle, &id_api, data, &dlc_api, &flags, NULL); 89 | if (status < canOK) 90 | return status; 91 | 92 | *dlc = (size_t)dlc_api; 93 | *id = (uint32_t)(unsigned long)id_api; 94 | *id |= (flags & canMSG_RTR) ? CO_RTR_MASK : 0; 95 | *id |= (flags & canMSG_EXT) ? CO_EXT_MASK : 0; 96 | 97 | co_msg_log ("Rx", *id, data, *dlc); 98 | 99 | return 0; 100 | } 101 | 102 | int os_channel_set_bitrate (os_channel_t * channel, int bitrate) 103 | { 104 | canStatus status; 105 | 106 | switch (bitrate) 107 | { 108 | case 100 * 1000: 109 | bitrate = canBITRATE_100K; 110 | break; 111 | case 125 * 1000: 112 | bitrate = canBITRATE_125K; 113 | break; 114 | case 250 * 1000: 115 | bitrate = canBITRATE_250K; 116 | break; 117 | case 500 * 1000: 118 | bitrate = canBITRATE_500K; 119 | break; 120 | case 1000 * 1000: 121 | bitrate = canBITRATE_1M; 122 | break; 123 | default: 124 | assert (0); 125 | } 126 | 127 | status = canSetBusParams (channel->handle, bitrate, 0, 0, 0, 0, 0); 128 | if (status < canOK) 129 | return status; 130 | 131 | return 0; 132 | } 133 | 134 | int os_channel_set_filter (os_channel_t * channel, uint8_t * filter, size_t size) 135 | { 136 | return 0; 137 | } 138 | 139 | int os_channel_bus_on (os_channel_t * channel) 140 | { 141 | canStatus status; 142 | 143 | status = canBusOn (channel->handle); 144 | if (status < canOK) 145 | return status; 146 | 147 | return 0; 148 | } 149 | 150 | int os_channel_bus_off (os_channel_t * channel) 151 | { 152 | canStatus status; 153 | 154 | status = canBusOff (channel->handle); 155 | if (status < canOK) 156 | return status; 157 | 158 | return 0; 159 | } 160 | 161 | int os_channel_get_state (os_channel_t * channel, os_channel_state_t * state) 162 | { 163 | canStatus status; 164 | unsigned long flags; 165 | 166 | status = canReadStatus (channel->handle, &flags); 167 | if (status < canOK) 168 | return status; 169 | 170 | state->overrun = flags & canSTAT_OVERRUN; 171 | state->error_passive = flags & canSTAT_ERROR_PASSIVE; 172 | state->bus_off = flags & canSTAT_BUS_OFF; 173 | 174 | return 0; 175 | } 176 | -------------------------------------------------------------------------------- /src/ports/windows/coal_can_sys.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef COAL_CAN_SYS_H 17 | #define COAL_CAN_SYS_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #ifdef __cplusplus 24 | } 25 | #endif 26 | 27 | #define OS_CHANNEL 28 | 29 | typedef struct 30 | { 31 | int handle; 32 | void (*callback) (void * arg); 33 | void * arg; 34 | } os_channel_t; 35 | 36 | #endif /* COAL_CAN_SYS_H */ 37 | -------------------------------------------------------------------------------- /test/co_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include 17 | 18 | int main (int argc, char * argv[]) 19 | { 20 | if (argc > 0) 21 | ::testing::InitGoogleTest (&argc, argv); 22 | else 23 | ::testing::InitGoogleTest(); 24 | 25 | int result = RUN_ALL_TESTS(); 26 | exit (result); 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /test/mocks.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "mocks.h" 17 | 18 | #include 19 | #include 20 | 21 | os_tick_t mock_os_tick_current_result = 0; 22 | os_tick_t mock_os_tick_current (void) 23 | { 24 | return mock_os_tick_current_result; 25 | } 26 | 27 | os_tick_t mock_os_tick_from_us (uint32_t us) 28 | { 29 | return us; 30 | } 31 | 32 | unsigned int mock_os_channel_send_calls = 0; 33 | uint32_t mock_os_channel_send_id; 34 | uint8_t mock_os_channel_send_data[8]; 35 | size_t mock_os_channel_send_dlc; 36 | int mock_os_channel_send_result; 37 | int mock_os_channel_send ( 38 | os_channel_t * channel, 39 | uint32_t id, 40 | const uint8_t * data, 41 | size_t dlc) 42 | { 43 | (void)channel; 44 | EXPECT_LE (dlc, 8u); 45 | mock_os_channel_send_calls++; 46 | mock_os_channel_send_id = id; 47 | mock_os_channel_send_dlc = dlc; 48 | memcpy (mock_os_channel_send_data, data, dlc); 49 | return mock_os_channel_send_result; 50 | } 51 | 52 | unsigned int mock_os_channel_receive_calls = 0; 53 | uint32_t mock_os_channel_receive_id; 54 | uint8_t mock_os_channel_receive_data[8]; 55 | size_t mock_os_channel_receive_dlc; 56 | int mock_os_channel_receive_result; 57 | int mock_os_channel_receive ( 58 | os_channel_t * channel, 59 | uint32_t * id, 60 | uint8_t * data, 61 | size_t * dlc, 62 | int tmo) 63 | { 64 | (void)channel; 65 | (void)tmo; 66 | mock_os_channel_receive_calls++; 67 | *id = mock_os_channel_receive_id; 68 | *dlc = mock_os_channel_receive_dlc; 69 | memcpy (data, mock_os_channel_receive_data, *dlc); 70 | return mock_os_channel_receive_result; 71 | } 72 | 73 | unsigned int mock_os_channel_bus_off_calls = 0; 74 | int mock_os_channel_bus_off (os_channel_t * channel) 75 | { 76 | mock_os_channel_bus_off_calls++; 77 | return 0; 78 | } 79 | 80 | unsigned int mock_os_channel_bus_on_calls = 0; 81 | int mock_os_channel_bus_on (os_channel_t * channel) 82 | { 83 | mock_os_channel_bus_on_calls++; 84 | return 0; 85 | } 86 | 87 | unsigned int mock_os_channel_set_bitrate_calls = 0; 88 | int mock_os_channel_set_bitrate_bitrate = 0; 89 | int mock_os_channel_set_bitrate (os_channel_t * channel, int bitrate) 90 | { 91 | mock_os_channel_set_bitrate_calls++; 92 | mock_os_channel_set_bitrate_bitrate = bitrate; 93 | return 0; 94 | } 95 | 96 | unsigned int mock_os_channel_set_filter_calls = 0; 97 | int mock_os_channel_set_filter_filter = 0; 98 | int mock_os_channel_set_filter (os_channel_t * channel, int filter) 99 | { 100 | mock_os_channel_set_filter_calls++; 101 | mock_os_channel_set_filter_filter = filter; 102 | return 0; 103 | } 104 | 105 | unsigned int mock_os_channel_get_state_calls = 0; 106 | os_channel_state_t mock_os_channel_get_state_state; 107 | int mock_os_channel_get_state (os_channel_t * channel, os_channel_state * state) 108 | { 109 | mock_os_channel_get_state_calls++; 110 | *state = mock_os_channel_get_state_state; 111 | return 0; 112 | } 113 | 114 | const co_obj_t * mock_co_obj_find_result; 115 | const co_obj_t * mock_co_obj_find (co_net_t * net, uint16_t index) 116 | { 117 | return mock_co_obj_find_result; 118 | } 119 | 120 | const co_entry_t * mock_co_entry_find_result; 121 | const co_entry_t * mock_co_entry_find (co_net_t * net, co_obj_t * obj, uint8_t subindex) 122 | { 123 | return mock_co_entry_find_result; 124 | } 125 | 126 | unsigned int mock_co_od_reset_calls = 0; 127 | void mock_co_od_reset (co_net_t * net) 128 | { 129 | mock_co_od_reset_calls++; 130 | } 131 | 132 | unsigned int mock_co_emcy_tx_calls = 0; 133 | uint16_t mock_co_emcy_tx_code = 0; 134 | void mock_co_emcy_tx (co_net_t * net, uint16_t code) 135 | { 136 | mock_co_emcy_tx_calls++; 137 | mock_co_emcy_tx_code = code; 138 | } 139 | 140 | unsigned int cb_reset_calls; 141 | void cb_reset (co_net_t * net) 142 | { 143 | cb_reset_calls++; 144 | } 145 | 146 | unsigned int cb_nmt_calls; 147 | void cb_nmt (co_net_t * net, co_state_t state) 148 | { 149 | cb_nmt_calls++; 150 | } 151 | 152 | unsigned int cb_emcy_calls; 153 | uint8_t cb_emcy_node; 154 | uint16_t cb_emcy_code; 155 | uint8_t cb_emcy_reg; 156 | uint8_t cb_emcy_msef[5]; 157 | bool cb_emcy_result; 158 | bool cb_emcy (co_net_t * net, uint8_t node, uint16_t code, uint8_t reg, uint8_t msef[5]) 159 | { 160 | cb_emcy_calls++; 161 | cb_emcy_node = node; 162 | cb_emcy_code = code; 163 | cb_emcy_reg = reg; 164 | if (msef != NULL) 165 | memcpy (cb_emcy_msef, msef, sizeof (cb_emcy_msef)); 166 | return cb_emcy_result; 167 | } 168 | 169 | unsigned int cb_sync_calls; 170 | void cb_sync (co_net_t * net) 171 | { 172 | cb_sync_calls++; 173 | } 174 | 175 | unsigned int cb_notify_calls; 176 | uint16_t cb_notify_index; 177 | uint16_t cb_notify_subindex; 178 | void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex) 179 | { 180 | cb_notify_calls++; 181 | cb_notify_index = index; 182 | cb_notify_subindex = subindex; 183 | } 184 | 185 | unsigned int cb_heartbeat_state_calls; 186 | uint8_t cb_heartbeat_state_node; 187 | uint8_t cb_heartbeat_state_old_state; 188 | uint8_t cb_heartbeat_state_new_state; 189 | void cb_heartbeat_state (co_net_t * net, uint8_t node, uint8_t old_state, uint8_t new_state) 190 | { 191 | cb_heartbeat_state_calls++; 192 | cb_heartbeat_state_node = node; 193 | cb_heartbeat_state_old_state = old_state; 194 | cb_heartbeat_state_new_state = new_state; 195 | } 196 | 197 | uint8_t the_store[2 * 1024]; 198 | struct fd 199 | { 200 | uint8_t * p; 201 | } _fd; 202 | 203 | void store_init (void) 204 | { 205 | memset (the_store, 0, sizeof (the_store)); 206 | } 207 | 208 | unsigned int store_open_calls; 209 | void * store_open (co_store_t store, co_mode_t mode) 210 | { 211 | store_open_calls++; 212 | _fd.p = the_store; 213 | return &_fd; 214 | } 215 | 216 | int store_read (void * arg, void * data, size_t size) 217 | { 218 | struct fd * fd = (struct fd *)arg; 219 | memcpy (data, fd->p, size); 220 | fd->p += size; 221 | return 0; 222 | } 223 | 224 | int store_write (void * arg, const void * data, size_t size) 225 | { 226 | struct fd * fd = (struct fd *)arg; 227 | memcpy (fd->p, data, size); 228 | fd->p += size; 229 | return 0; 230 | } 231 | 232 | int store_close (void * arg) 233 | { 234 | return 0; 235 | } 236 | -------------------------------------------------------------------------------- /test/mocks.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef MOCKS_H 17 | #define MOCKS_H 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | #include 24 | 25 | #include "osal.h" 26 | #include "co_api.h" 27 | #include "co_main.h" 28 | 29 | extern os_tick_t mock_os_tick_current_result; 30 | os_tick_t mock_os_tick_current (void); 31 | 32 | os_tick_t mock_os_tick_from_us (uint32_t us); 33 | 34 | extern unsigned int mock_os_channel_send_calls; 35 | extern uint32_t mock_os_channel_send_id; 36 | extern uint8_t mock_os_channel_send_data[8]; 37 | extern size_t mock_os_channel_send_dlc; 38 | extern int mock_os_channel_send_result; 39 | int mock_os_channel_send ( 40 | os_channel_t * channel, 41 | uint32_t id, 42 | const uint8_t * data, 43 | size_t dlc); 44 | 45 | extern unsigned int mock_os_channel_receive_calls; 46 | extern uint32_t mock_os_channel_receive_id; 47 | extern uint8_t mock_os_channel_receive_data[8]; 48 | extern size_t mock_os_channel_receive_dlc; 49 | extern int mock_os_channel_receive_result; 50 | int mock_os_channel_receive ( 51 | os_channel_t * channel, 52 | uint32_t * id, 53 | uint8_t * data, 54 | size_t * dlc, 55 | int tmo); 56 | 57 | extern unsigned int mock_os_channel_bus_off_calls; 58 | int mock_os_channel_bus_off (os_channel_t * channel); 59 | 60 | extern unsigned int mock_os_channel_bus_on_calls; 61 | int mock_os_channel_bus_on (os_channel_t * channel); 62 | 63 | extern unsigned int mock_os_channel_set_bitrate_calls; 64 | extern int mock_os_channel_set_bitrate_bitrate; 65 | int mock_os_channel_set_bitrate (os_channel_t * channel, int bitrate); 66 | 67 | extern unsigned int mock_os_channel_set_filter_calls; 68 | extern int mock_os_channel_set_filter_filter; 69 | int mock_os_channel_set_filter (os_channel_t * channel, int filter); 70 | 71 | extern unsigned int mock_os_channel_get_state_calls; 72 | extern os_channel_state_t mock_os_channel_get_state_state; 73 | int mock_os_channel_get_state (os_channel_t * channel, os_channel_state * state); 74 | 75 | extern const co_obj_t * mock_co_obj_find_result; 76 | const co_obj_t * mock_co_obj_find (co_net_t * net, uint16_t index); 77 | 78 | extern const co_entry_t * mock_co_entry_find_result; 79 | const co_entry_t * mock_co_entry_find ( 80 | co_net_t * net, 81 | co_obj_t * obj, 82 | uint8_t subindex); 83 | 84 | extern unsigned int mock_co_od_reset_calls; 85 | void mock_co_od_reset (co_net_t * net); 86 | 87 | extern unsigned int mock_co_emcy_tx_calls; 88 | extern uint16_t mock_co_emcy_tx_code; 89 | void mock_co_emcy_tx (co_net_t * net, uint16_t code); 90 | 91 | extern unsigned int cb_reset_calls; 92 | void cb_reset (co_net_t * net); 93 | 94 | extern unsigned int cb_nmt_calls; 95 | void cb_nmt (co_net_t * net, co_state_t state); 96 | 97 | extern unsigned int cb_emcy_calls; 98 | extern uint8_t cb_emcy_node; 99 | extern uint16_t cb_emcy_code; 100 | extern uint8_t cb_emcy_reg; 101 | extern uint8_t cb_emcy_msef[5]; 102 | extern bool cb_emcy_result; 103 | bool cb_emcy (co_net_t * net, uint8_t node, uint16_t code, uint8_t reg, uint8_t msef[5]); 104 | 105 | extern unsigned int cb_sync_calls; 106 | void cb_sync (co_net_t * net); 107 | 108 | extern unsigned int cb_notify_calls; 109 | extern uint16_t cb_notify_index; 110 | extern uint16_t cb_notify_subindex; 111 | void cb_notify (co_net_t * net, uint16_t index, uint8_t subindex); 112 | 113 | extern unsigned int cb_heartbeat_state_calls; 114 | extern uint8_t cb_heartbeat_state_node; 115 | extern uint8_t cb_heartbeat_state_old_state; 116 | extern uint8_t cb_heartbeat_state_new_state; 117 | void cb_heartbeat_state (co_net_t * net, uint8_t node, uint8_t old_state, uint8_t new_state); 118 | 119 | void store_init (void); 120 | extern unsigned int store_open_calls; 121 | void * store_open (co_store_t store, co_mode_t mode); 122 | int store_read (void * arg, void * data, size_t size); 123 | int store_write (void * arg, const void * data, size_t size); 124 | int store_close (void * arg); 125 | 126 | #ifdef __cplusplus 127 | } 128 | #endif 129 | 130 | #endif /* MOCKS_H */ 131 | -------------------------------------------------------------------------------- /test/test_bitmap.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_bitmap.h" 17 | #include "test_util.h" 18 | 19 | TEST (Bitmap, SetClearGet) 20 | { 21 | uint32_t bm[4] = {0}; 22 | 23 | co_bitmap_set (bm, 0); 24 | co_bitmap_set (bm, 31); 25 | co_bitmap_set (bm, 32); 26 | co_bitmap_set (bm, 64); 27 | co_bitmap_set (bm, 67); 28 | co_bitmap_set (bm, 95); 29 | co_bitmap_set (bm, 127); 30 | 31 | EXPECT_EQ (0x80000001u, bm[0]); 32 | EXPECT_EQ (0x00000001u, bm[1]); 33 | EXPECT_EQ (0x80000009u, bm[2]); 34 | EXPECT_EQ (0x80000000u, bm[3]); 35 | 36 | co_bitmap_clear (bm, 67); 37 | co_bitmap_clear (bm, 95); 38 | 39 | EXPECT_EQ (1, co_bitmap_get (bm, 0)); 40 | EXPECT_EQ (0, co_bitmap_get (bm, 1)); 41 | EXPECT_EQ (1, co_bitmap_get (bm, 31)); 42 | EXPECT_EQ (1, co_bitmap_get (bm, 32)); 43 | EXPECT_EQ (1, co_bitmap_get (bm, 64)); 44 | EXPECT_EQ (0, co_bitmap_get (bm, 67)); 45 | EXPECT_EQ (0, co_bitmap_get (bm, 95)); 46 | EXPECT_EQ (0, co_bitmap_get (bm, 126)); 47 | EXPECT_EQ (1, co_bitmap_get (bm, 127)); 48 | } 49 | 50 | TEST (Bitmap, Next) 51 | { 52 | uint32_t bm[4] = {0}; 53 | 54 | co_bitmap_set (bm, 0); 55 | co_bitmap_set (bm, 31); 56 | co_bitmap_set (bm, 32); 57 | co_bitmap_set (bm, 64); 58 | co_bitmap_set (bm, 67); 59 | co_bitmap_set (bm, 95); 60 | co_bitmap_set (bm, 127); 61 | 62 | EXPECT_EQ (0, co_bitmap_next (bm, 0)); 63 | EXPECT_EQ (31, co_bitmap_next (bm, 1)); 64 | EXPECT_EQ (32, co_bitmap_next (bm, 32)); 65 | EXPECT_EQ (64, co_bitmap_next (bm, 33)); 66 | EXPECT_EQ (67, co_bitmap_next (bm, 65)); 67 | EXPECT_EQ (95, co_bitmap_next (bm, 68)); 68 | EXPECT_EQ (127, co_bitmap_next (bm, 96)); 69 | 70 | co_bitmap_clear (bm, 127); 71 | 72 | EXPECT_EQ (0, co_bitmap_next (bm, 96)); 73 | } 74 | -------------------------------------------------------------------------------- /test/test_heartbeat.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_heartbeat.h" 17 | #include "co_bitmap.h" 18 | #include "test_util.h" 19 | 20 | // Test fixture 21 | 22 | class HeartbeatTest : public TestBase 23 | { 24 | }; 25 | 26 | // Tests 27 | 28 | TEST_F (HeartbeatTest, OD1017) 29 | { 30 | uint32_t value; 31 | uint32_t result; 32 | 33 | value = 1000; 34 | 35 | result = co_od1017_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 36 | EXPECT_EQ (0u, result); 37 | EXPECT_EQ (net.hb_time, value); 38 | 39 | net.hb_time = 2000; 40 | 41 | result = co_od1017_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 42 | EXPECT_EQ (0u, result); 43 | EXPECT_EQ (value, net.hb_time); 44 | } 45 | 46 | TEST_F (HeartbeatTest, OD1016) 47 | { 48 | uint32_t value; 49 | uint32_t result; 50 | 51 | // Monitor node 1 52 | value = (1 << 16) | 1000; 53 | result = co_od1016_fn (&net, OD_EVENT_WRITE, NULL, NULL, 1, &value); 54 | EXPECT_EQ (0u, result); 55 | EXPECT_EQ (1u, net.heartbeat[0].node); 56 | EXPECT_EQ (1000u, net.heartbeat[0].time); 57 | 58 | // Duplicate entry for node 1, should fail 59 | value = (1 << 16) | 2000; 60 | result = co_od1016_fn (&net, OD_EVENT_WRITE, NULL, NULL, 2, &value); 61 | EXPECT_EQ (CO_SDO_ABORT_PARAM_INCOMPATIBLE, result); 62 | 63 | // Read back previously written value 64 | value = 0; 65 | result = co_od1016_fn (&net, OD_EVENT_READ, NULL, NULL, 1, &value); 66 | EXPECT_EQ (0u, result); 67 | EXPECT_EQ ((1 << 16) | 1000u, value); 68 | } 69 | 70 | TEST_F (HeartbeatTest, HeartbeatProducer) 71 | { 72 | uint8_t expected[] = {127, 4, 5}; 73 | 74 | net.hb_time = 1000; 75 | 76 | // Timer has expired, should send heartbeat 77 | co_heartbeat_timer (&net, 1000 * 1000); 78 | EXPECT_EQ (1u, mock_os_channel_send_calls); 79 | EXPECT_TRUE (CanMatch (0x701, &expected[0], 1)); 80 | 81 | net.state = STATE_STOP; 82 | 83 | // Timer has not expired, should not send heartbeat 84 | co_heartbeat_timer (&net, 1500 * 1000); 85 | EXPECT_EQ (1u, mock_os_channel_send_calls); 86 | 87 | // Timer has expired, should send heartbeat 88 | co_heartbeat_timer (&net, 2000 * 1000); 89 | EXPECT_EQ (2u, mock_os_channel_send_calls); 90 | EXPECT_TRUE (CanMatch (0x701, &expected[1], 1)); 91 | 92 | net.state = STATE_OP; 93 | 94 | // Timer has expired, should send heartbeat 95 | co_heartbeat_timer (&net, 3000 * 1000); 96 | EXPECT_EQ (3u, mock_os_channel_send_calls); 97 | EXPECT_TRUE (CanMatch (0x701, &expected[2], 1)); 98 | } 99 | 100 | TEST_F (HeartbeatTest, HeartbeatConsumer) 101 | { 102 | uint8_t heartbeat = 5; 103 | int error; 104 | 105 | net.heartbeat[0].node = 1; 106 | net.heartbeat[0].time = 1000; 107 | 108 | // Should ignore too small message 109 | error = co_heartbeat_rx (&net, 1, &heartbeat, 0); 110 | EXPECT_EQ (-1, error); 111 | EXPECT_EQ (0u, cb_heartbeat_state_calls); 112 | 113 | // Should ignore too large message 114 | error = co_heartbeat_rx (&net, 1, &heartbeat, 2); 115 | EXPECT_EQ (-1, error); 116 | EXPECT_EQ (0u, cb_heartbeat_state_calls); 117 | 118 | // Receive heartbeat within timer window 119 | mock_os_tick_current_result = 500 * 1000; 120 | co_heartbeat_rx (&net, 1, &heartbeat, 1); 121 | EXPECT_EQ (5, net.heartbeat[0].state); 122 | EXPECT_EQ (1u, cb_heartbeat_state_calls); 123 | 124 | // Timer has expired, should send EMCY 125 | co_heartbeat_timer (&net, 1500 * 1000); 126 | EXPECT_EQ (0, net.heartbeat[0].state); 127 | EXPECT_EQ (1u, mock_co_emcy_tx_calls); 128 | EXPECT_EQ (2u, cb_heartbeat_state_calls); 129 | 130 | // Timer already expired, should not send EMCY 131 | co_heartbeat_timer (&net, 1600 * 1000); 132 | EXPECT_EQ (0, net.heartbeat[0].state); 133 | EXPECT_EQ (1u, mock_co_emcy_tx_calls); 134 | EXPECT_EQ (2u, cb_heartbeat_state_calls); 135 | 136 | // Receive heartbeat, should set state 137 | mock_os_tick_current_result = 2000 * 1000; 138 | co_heartbeat_rx (&net, 1, &heartbeat, 1); 139 | EXPECT_EQ (5, net.heartbeat[0].state); 140 | EXPECT_EQ (3u, cb_heartbeat_state_calls); 141 | 142 | // Receive heartbeat with different state, should call cb_heartbeat_state 143 | mock_os_tick_current_result = 2500 * 1000; 144 | heartbeat = 127; 145 | co_heartbeat_rx (&net, 1, &heartbeat, 1); 146 | EXPECT_EQ (4u, cb_heartbeat_state_calls); 147 | EXPECT_EQ (5, cb_heartbeat_state_old_state); 148 | EXPECT_EQ (127, cb_heartbeat_state_new_state); 149 | } 150 | 151 | TEST_F (HeartbeatTest, ShouldMaintainNodeMap) 152 | { 153 | uint8_t heartbeat = 0x01; 154 | 155 | net.heartbeat[0].node = 1; 156 | net.heartbeat[0].time = 1000; 157 | 158 | // Receive heartbeat within timer window, should set active node ID 159 | mock_os_tick_current_result = 500 * 1000; 160 | co_heartbeat_rx (&net, 1, &heartbeat, 1); 161 | EXPECT_TRUE (co_bitmap_get (net.nodes, 1)); 162 | 163 | // Timer has expired, should clear active node ID 164 | co_heartbeat_timer (&net, 1500 * 1000); 165 | EXPECT_FALSE (co_bitmap_get (net.nodes, 1)); 166 | } 167 | -------------------------------------------------------------------------------- /test/test_lss.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_lss.h" 17 | #include "test_util.h" 18 | 19 | // Test fixture 20 | 21 | class LssTest : public TestBase 22 | { 23 | protected: 24 | virtual void SetUp() 25 | { 26 | TestBase::SetUp(); 27 | co_lss_init (&net); 28 | } 29 | }; 30 | 31 | // Tests 32 | 33 | TEST_F (LssTest, SwitchGlobal) 34 | { 35 | uint8_t command[][8] = { 36 | {0x04, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 37 | {0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 38 | {0x04, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 39 | }; 40 | 41 | co_lss_rx (&net, 0x7E5, command[0], 8); 42 | EXPECT_EQ (LSS_STATE_CONFIG, net.lss.state); 43 | 44 | co_lss_rx (&net, 0x7E5, command[1], 8); 45 | EXPECT_EQ (LSS_STATE_WAITING, net.lss.state); 46 | 47 | // Invalid - should ignore 48 | co_lss_rx (&net, 0x7E5, command[2], 8); 49 | EXPECT_EQ (LSS_STATE_WAITING, net.lss.state); 50 | } 51 | 52 | TEST_F (LssTest, SwitchSelective) 53 | { 54 | uint8_t command[][8] = { 55 | {0x40, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 56 | {0x41, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 57 | {0x42, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 58 | {0x43, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 59 | }; 60 | 61 | co_lss_rx (&net, 0x7E5, command[0], 8); 62 | EXPECT_EQ (0u, mock_os_channel_send_calls); 63 | 64 | co_lss_rx (&net, 0x7E5, command[1], 8); 65 | EXPECT_EQ (0u, mock_os_channel_send_calls); 66 | 67 | co_lss_rx (&net, 0x7E5, command[2], 8); 68 | EXPECT_EQ (0u, mock_os_channel_send_calls); 69 | 70 | co_lss_rx (&net, 0x7E5, command[3], 8); 71 | EXPECT_EQ (1u, mock_os_channel_send_calls); 72 | EXPECT_EQ (0x7E4u, mock_os_channel_send_id); 73 | 74 | // Send one extra matching command - should fail 75 | co_lss_rx (&net, 0x7E5, command[3], 8); 76 | EXPECT_EQ (1u, mock_os_channel_send_calls); 77 | } 78 | 79 | TEST_F (LssTest, SwitchSelectiveFail) 80 | { 81 | uint8_t command[][8] = { 82 | {0x40, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 83 | {0x41, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 84 | {0x42, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 85 | {0x43, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 86 | }; 87 | 88 | // Send same matching command 4 times - should fail 89 | for (int ix = 0; ix < 4; ix++) 90 | { 91 | co_lss_rx (&net, 0x7E5, command[0], 8); 92 | } 93 | EXPECT_EQ (0u, mock_os_channel_send_calls); 94 | 95 | // Last command is not match - should fail 96 | for (int ix = 0; ix < 4; ix++) 97 | { 98 | co_lss_rx (&net, 0x7E5, command[ix], 8); 99 | } 100 | EXPECT_EQ (0u, mock_os_channel_send_calls); 101 | } 102 | 103 | TEST_F (LssTest, Identify) 104 | { 105 | uint8_t command[][8] = { 106 | {0x5A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 107 | {0x5E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 108 | }; 109 | uint8_t expected[][8] = { 110 | {0x5A, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 111 | {0x5E, 0x42, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 112 | }; 113 | 114 | net.lss.state = LSS_STATE_CONFIG; 115 | 116 | co_lss_rx (&net, 0x7E5, command[0], 8); 117 | EXPECT_EQ (1u, mock_os_channel_send_calls); 118 | EXPECT_TRUE (CanMatch (0x7E4, expected[0], 8)); 119 | 120 | net.node = 0x42; 121 | co_lss_rx (&net, 0x7E5, command[1], 8); 122 | EXPECT_EQ (2u, mock_os_channel_send_calls); 123 | EXPECT_TRUE (CanMatch (0x7E4, expected[1], 8)); 124 | 125 | net.lss.state = LSS_STATE_WAITING; 126 | 127 | // Wrong state, should not respond 128 | co_lss_rx (&net, 0x7E5, command[1], 8); 129 | EXPECT_EQ (2u, mock_os_channel_send_calls); 130 | } 131 | 132 | TEST_F (LssTest, IdentifyRemote) 133 | { 134 | uint8_t command[][8] = { 135 | {0x46, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 136 | {0x47, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 137 | {0x48, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 138 | {0x49, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 139 | {0x4A, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 140 | {0x4B, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 141 | }; 142 | 143 | for (int ix = 0; ix < 6; ix++) 144 | { 145 | co_lss_rx (&net, 0x7E5, command[ix], 8); 146 | } 147 | EXPECT_EQ (1u, mock_os_channel_send_calls); 148 | } 149 | 150 | TEST_F (LssTest, IdentifyRemoteFail) 151 | { 152 | uint8_t command[][8] = { 153 | {0x46, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 154 | {0x47, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 155 | {0x48, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 156 | {0x49, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // Out of range 157 | {0x4A, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 158 | {0x4B, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 159 | }; 160 | 161 | for (unsigned int ix = 0; ix < NELEMENTS (command); ix++) 162 | { 163 | co_lss_rx (&net, 0x7E5, command[ix], 8); 164 | } 165 | EXPECT_EQ (0u, mock_os_channel_send_calls); 166 | } 167 | 168 | TEST_F (LssTest, ConfigureNodeId) 169 | { 170 | uint8_t command[][8] = { 171 | {0x11, 0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 172 | {0x11, 0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // Bad node id 173 | }; 174 | uint8_t expected[][8] = { 175 | {0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 176 | {0x11, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 177 | }; 178 | unsigned int ix; 179 | 180 | net.lss.state = LSS_STATE_CONFIG; 181 | 182 | for (ix = 0; ix < NELEMENTS (command); ix++) 183 | { 184 | co_lss_rx (&net, 0x7E5, command[ix], 8); 185 | EXPECT_TRUE (CanMatch (0x7E4, expected[ix], 8)); 186 | } 187 | 188 | net.lss.state = LSS_STATE_WAITING; 189 | 190 | // Bad state - should ignore 191 | EXPECT_EQ (ix, mock_os_channel_send_calls); 192 | co_lss_rx (&net, 0x7E5, command[0], 8); 193 | EXPECT_EQ (ix, mock_os_channel_send_calls); 194 | } 195 | 196 | TEST_F (LssTest, ConfigureBitrate) 197 | { 198 | uint8_t command[][8] = { 199 | {0x13, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}, 200 | {0x13, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}, // Bad ix 201 | {0x13, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}, // Bad table 202 | }; 203 | uint8_t expected[][8] = { 204 | {0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 205 | {0x13, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 206 | {0x13, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 207 | }; 208 | unsigned int ix; 209 | 210 | net.lss.state = LSS_STATE_CONFIG; 211 | 212 | for (ix = 0; ix < NELEMENTS (command); ix++) 213 | { 214 | co_lss_rx (&net, 0x7E5, command[ix], 8); 215 | EXPECT_TRUE (CanMatch (0x7E4, expected[ix], 8)); 216 | } 217 | 218 | net.lss.state = LSS_STATE_WAITING; 219 | 220 | // Bad state - should ignore 221 | EXPECT_EQ (ix, mock_os_channel_send_calls); 222 | co_lss_rx (&net, 0x7E5, command[0], 8); 223 | EXPECT_EQ (ix, mock_os_channel_send_calls); 224 | } 225 | 226 | TEST_F (LssTest, ActivateBitrate) 227 | { 228 | uint8_t command[][8] = { 229 | {0x13, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00}, // Configure 230 | {0x15, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // Activate 231 | }; 232 | 233 | net.lss.state = LSS_STATE_CONFIG; 234 | 235 | co_lss_rx (&net, 0x7E5, command[0], 8); 236 | co_lss_rx (&net, 0x7E5, command[1], 8); 237 | 238 | EXPECT_EQ (1u, mock_os_channel_set_bitrate_calls); 239 | EXPECT_EQ (125 * 1000, mock_os_channel_set_bitrate_bitrate); 240 | } 241 | 242 | TEST_F (LssTest, Persistence) 243 | { 244 | uint8_t command[][8] = { 245 | {0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // Store 246 | }; 247 | uint8_t expected[][8] = { 248 | {0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 249 | }; 250 | 251 | net.lss.state = LSS_STATE_CONFIG; 252 | net.lss.node = 0x42; 253 | net.lss.bitrate = 125 * 1000; 254 | 255 | co_lss_rx (&net, 0x7E5, command[0], 8); 256 | 257 | EXPECT_EQ (1u, mock_os_channel_send_calls); 258 | EXPECT_TRUE (CanMatch (0x7E4, expected[0], 8)); 259 | 260 | EXPECT_EQ (0x42u, co_lss_get_persistent_node_id (&net)); 261 | EXPECT_EQ (125 * 1000, co_lss_get_persistent_bitrate (&net)); 262 | } 263 | -------------------------------------------------------------------------------- /test/test_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "options.h" 17 | #include 18 | 19 | extern "C" { 20 | #pragma warning(disable : 4100) 21 | void mock_os_usleep (uint32_t usec){}; 22 | os_thread_t * mock_os_thread_create ( 23 | const char * name, 24 | int priority, 25 | int stacksize, 26 | void (*entry) (void * arg), 27 | void * arg) 28 | { 29 | return NULL; 30 | } 31 | 32 | os_channel_t * mock_os_channel_open (const char * name) 33 | { 34 | return NULL; 35 | } 36 | void mock_os_channel_send (os_channel_t * channel, const uint8_t * data, size_t dlc) 37 | { 38 | } 39 | void mock_os_channel_receive ( 40 | os_channel_t * channel, 41 | uint8_t * data, 42 | size_t dlc, 43 | int tmo) 44 | { 45 | } 46 | void mock_os_channel_set_bitrate (os_channel_t * channel, int bitrate) 47 | { 48 | } 49 | void mock_os_channel_set_filter (os_channel_t * channel, uint8_t * filter, size_t size) 50 | { 51 | } 52 | void mock_os_channel_bus_on (os_channel_t * channel) 53 | { 54 | } 55 | void mock_os_channel_bus_off (os_channel_t * channel) 56 | { 57 | } 58 | } 59 | 60 | TEST (alloc, string) 61 | { 62 | ASSERT_STREQ ("hello", "world"); 63 | } 64 | -------------------------------------------------------------------------------- /test/test_nmt.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_nmt.h" 17 | #include "test_util.h" 18 | 19 | // Test fixture 20 | 21 | class NmtTest : public TestBase 22 | { 23 | protected: 24 | virtual void SetUp() 25 | { 26 | TestBase::SetUp(); 27 | } 28 | }; 29 | 30 | // Tests 31 | 32 | TEST_F (NmtTest, Fsm) 33 | { 34 | uint8_t command[][8] = { 35 | {CO_NMT_OPERATIONAL, 0x01}, 36 | {CO_NMT_STOPPED, 0x01}, 37 | {CO_NMT_OPERATIONAL, 0x01}, 38 | {CO_NMT_PRE_OPERATIONAL, 0x01}, 39 | {CO_NMT_RESET_NODE, 0x01}, 40 | {CO_NMT_RESET_COMMUNICATION, 0x01}, 41 | {CO_NMT_OPERATIONAL, 0x00}, 42 | }; 43 | 44 | // Poweron, should go to PREOP 45 | co_nmt_init (&net); 46 | EXPECT_EQ (STATE_PREOP, net.state); 47 | EXPECT_EQ (4u, cb_nmt_calls); 48 | EXPECT_EQ (3u, mock_co_od_reset_calls); 49 | EXPECT_EQ (1u, mock_os_channel_send_calls); 50 | 51 | // Operational, should go to OP 52 | co_nmt_rx (&net, 0, command[0], 2); 53 | EXPECT_EQ (STATE_OP, net.state); 54 | EXPECT_EQ (5u, cb_nmt_calls); 55 | 56 | // Stopped, should go to STOP 57 | co_nmt_rx (&net, 0, command[1], 2); 58 | EXPECT_EQ (STATE_STOP, net.state); 59 | EXPECT_EQ (6u, cb_nmt_calls); 60 | 61 | // Operational, should go to OP 62 | co_nmt_rx (&net, 0, command[2], 2); 63 | EXPECT_EQ (STATE_OP, net.state); 64 | EXPECT_EQ (7u, cb_nmt_calls); 65 | 66 | // Pre operational, should go to PREOP 67 | co_nmt_rx (&net, 0, command[3], 2); 68 | EXPECT_EQ (STATE_PREOP, net.state); 69 | EXPECT_EQ (8u, cb_nmt_calls); 70 | 71 | // Reset node, should go to PREOP 72 | co_nmt_rx (&net, 0, command[4], 2); 73 | EXPECT_EQ (STATE_PREOP, net.state); 74 | EXPECT_EQ (11u, cb_nmt_calls); 75 | EXPECT_EQ (1u, cb_reset_calls); 76 | EXPECT_EQ (6u, mock_co_od_reset_calls); 77 | EXPECT_EQ (2u, mock_os_channel_send_calls); 78 | 79 | // Reset communication, should go to PREOP 80 | co_nmt_rx (&net, 0, command[5], 2); 81 | EXPECT_EQ (STATE_PREOP, net.state); 82 | EXPECT_EQ (13u, cb_nmt_calls); 83 | EXPECT_EQ (1u, cb_reset_calls); 84 | EXPECT_EQ (7u, mock_co_od_reset_calls); 85 | EXPECT_EQ (3u, mock_os_channel_send_calls); 86 | 87 | // Broadcast operational, should go to OP 88 | co_nmt_rx (&net, 0, command[6], 2); 89 | EXPECT_EQ (STATE_OP, net.state); 90 | EXPECT_EQ (14u, cb_nmt_calls); 91 | } 92 | 93 | TEST_F (NmtTest, BadNMT) 94 | { 95 | uint8_t command[][8] = { 96 | {0x42, 0x01}, 97 | {CO_NMT_OPERATIONAL, 0x03}, 98 | }; 99 | 100 | // Bad NMT command, should stay in PREOP 101 | co_nmt_rx (&net, 0, command[0], 2); 102 | EXPECT_EQ (STATE_PREOP, net.state); 103 | EXPECT_EQ (0u, cb_nmt_calls); 104 | 105 | // Bad node id 106 | co_nmt_rx (&net, 0, command[1], 2); 107 | EXPECT_EQ (STATE_PREOP, net.state); 108 | EXPECT_EQ (0u, cb_nmt_calls); 109 | 110 | // Bad dlc 111 | co_nmt_rx (&net, 0, command[0], 1); 112 | EXPECT_EQ (STATE_PREOP, net.state); 113 | EXPECT_EQ (0u, cb_nmt_calls); 114 | 115 | // Bad id 116 | co_nmt_rx (&net, 1, command[0], 1); 117 | EXPECT_EQ (STATE_PREOP, net.state); 118 | EXPECT_EQ (0u, cb_nmt_calls); 119 | } 120 | 121 | TEST_F (NmtTest, LssFsm) 122 | { 123 | net.lss.node = 0xFF; 124 | 125 | // Stay in STATE_INIT_COMM if lss node id is invalid 126 | co_nmt_event (&net, EVENT_RESETCOMM); 127 | EXPECT_EQ (STATE_INIT_COMM, net.state); 128 | } 129 | -------------------------------------------------------------------------------- /test/test_node_guard.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_node_guard.h" 17 | #include "test_util.h" 18 | 19 | #define NODE_GUARD_BASE (CO_RTR_MASK | CO_FUNCTION_NMT_ERR) 20 | 21 | // Test fixture 22 | 23 | class NodeGuardTest : public TestBase 24 | { 25 | }; 26 | 27 | // Tests 28 | 29 | TEST_F (NodeGuardTest, od100c_fn) 30 | { 31 | uint32_t result; 32 | uint32_t value; 33 | 34 | net.node_guard.guard_time = 1000; 35 | 36 | result = co_od100C_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 37 | EXPECT_EQ (0u, result); 38 | EXPECT_EQ (net.node_guard.guard_time, value); 39 | 40 | value = 500; 41 | result = co_od100C_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 42 | EXPECT_EQ (0u, result); 43 | EXPECT_EQ (net.node_guard.guard_time, value); 44 | 45 | result = co_od100C_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 46 | EXPECT_EQ (0u, result); 47 | EXPECT_EQ (net.node_guard.guard_time, value); 48 | } 49 | 50 | TEST_F (NodeGuardTest, od100d_fn) 51 | { 52 | uint32_t result; 53 | uint32_t value; 54 | 55 | net.node_guard.life_time_factor = 10; 56 | 57 | result = co_od100D_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 58 | EXPECT_EQ (0u, result); 59 | EXPECT_EQ (net.node_guard.life_time_factor, value); 60 | 61 | value = 5; 62 | result = co_od100D_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 63 | EXPECT_EQ (0u, result); 64 | EXPECT_EQ (net.node_guard.life_time_factor, value); 65 | 66 | result = co_od100D_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 67 | EXPECT_EQ (0u, result); 68 | EXPECT_EQ (net.node_guard.life_time_factor, value); 69 | } 70 | 71 | TEST_F (NodeGuardTest, NmtRtr) 72 | { 73 | uint8_t expected[] = {0x04, 0x84, 0x05, 0x85, 0x7f, 0xff, 0x00, 0x80}; 74 | uint8_t rtr = 0x00; 75 | 76 | net.state = STATE_STOP; 77 | 78 | mock_os_channel_send_id = 0; 79 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 80 | EXPECT_TRUE (CanMatch (0x701, &expected[0], 1)); 81 | 82 | mock_os_channel_send_id = 0; 83 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 84 | EXPECT_TRUE (CanMatch (0x701, &expected[1], 1)); 85 | 86 | net.state = STATE_OP; 87 | 88 | mock_os_channel_send_id = 0; 89 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 90 | EXPECT_TRUE (CanMatch (0x701, &expected[2], 1)); 91 | 92 | mock_os_channel_send_id = 0; 93 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 94 | EXPECT_TRUE (CanMatch (0x701, &expected[3], 1)); 95 | 96 | net.state = STATE_PREOP; 97 | 98 | mock_os_channel_send_id = 0; 99 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 100 | EXPECT_TRUE (CanMatch (0x701, &expected[4], 1)); 101 | 102 | mock_os_channel_send_id = 0; 103 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 104 | EXPECT_TRUE (CanMatch (0x701, &expected[5], 1)); 105 | 106 | /* faulty state should return state 0 */ 107 | net.state = (co_state_t)42; 108 | 109 | mock_os_channel_send_id = 0; 110 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 111 | EXPECT_TRUE (CanMatch (0x701, &expected[6], 1)); 112 | 113 | mock_os_channel_send_id = 0; 114 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 115 | EXPECT_TRUE (CanMatch (0x701, &expected[7], 1)); 116 | } 117 | 118 | TEST_F (NodeGuardTest, HeartBeatOverNmtRtr) 119 | { 120 | uint8_t rtr = 0x00; 121 | 122 | /* Activate heart beat timer, shall disable nmtrtr */ 123 | net.hb_time = 1000; 124 | 125 | mock_os_channel_send_id = 0; 126 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 127 | EXPECT_NE (0x701u, mock_os_channel_send_id); 128 | } 129 | 130 | TEST_F (NodeGuardTest, Expire) 131 | { 132 | uint8_t expected[] = {0x05, 0x85, 0x7f, 0xff, 0x30}; 133 | uint8_t emcy[] = {0x30, 0x81, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00}; 134 | uint8_t rtr = 0x00; 135 | 136 | net.state = STATE_OP; 137 | net.error_behavior = 0; 138 | net.emcy.cobid = 0x81u; 139 | net.node_guard.is_alive = false; 140 | 141 | net.node_guard.guard_time = 100; 142 | net.node_guard.life_time_factor = 2; 143 | 144 | // Receive node guard request, reset timestamp 145 | mock_os_tick_current_result = 10 * 1000; 146 | mock_os_channel_send_id = 0; 147 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 148 | EXPECT_TRUE (CanMatch (0x701, &expected[0], 1)); 149 | EXPECT_TRUE (net.node_guard.is_alive); 150 | EXPECT_EQ (STATE_OP, net.state); 151 | 152 | // Receive node guard request, reset timestamp 153 | mock_os_tick_current_result = 20 * 1000; 154 | mock_os_channel_send_id = 0; 155 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 156 | EXPECT_TRUE (CanMatch (0x701, &expected[1], 1)); 157 | EXPECT_TRUE (net.node_guard.is_alive); 158 | EXPECT_EQ (STATE_OP, net.state); 159 | 160 | // Should be alive, has not expired 161 | mock_os_channel_send_id = 0; 162 | co_node_guard_timer (&net, 50 * 1000); 163 | EXPECT_NE (0x81u, mock_os_channel_send_id); 164 | EXPECT_TRUE (net.node_guard.is_alive); 165 | EXPECT_EQ (STATE_OP, net.state); 166 | 167 | // Should be alive, has not expired 168 | mock_os_channel_send_id = 0; 169 | co_node_guard_timer (&net, 100 * 1000); 170 | EXPECT_NE (0x81u, mock_os_channel_send_id); 171 | EXPECT_TRUE (net.node_guard.is_alive); 172 | EXPECT_EQ (STATE_OP, net.state); 173 | 174 | // Should not be alive, has expired 175 | mock_os_channel_send_id = 0; 176 | co_node_guard_timer (&net, 220 * 1000); 177 | EXPECT_TRUE (CanMatch (0x81, emcy, 8)); 178 | EXPECT_FALSE (net.node_guard.is_alive); 179 | EXPECT_EQ (STATE_PREOP, net.state); 180 | 181 | // Receive node guard request, reset timestamp 182 | mock_os_tick_current_result = 300 * 1000; 183 | mock_os_channel_send_id = 0; 184 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 185 | EXPECT_TRUE (CanMatch (0x701, &expected[2], 1)); 186 | EXPECT_TRUE (net.node_guard.is_alive); 187 | EXPECT_EQ (STATE_PREOP, net.state); 188 | 189 | // Receive node guard request, reset timestamp 190 | mock_os_tick_current_result = 350 * 1000; 191 | mock_os_channel_send_id = 0; 192 | co_node_guard_rx (&net, NODE_GUARD_BASE | 1, &rtr, 1); 193 | EXPECT_TRUE (CanMatch (0x701, &expected[3], 1)); 194 | EXPECT_TRUE (net.node_guard.is_alive); 195 | EXPECT_EQ (STATE_PREOP, net.state); 196 | } 197 | -------------------------------------------------------------------------------- /test/test_sdo_client.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_sdo.h" 17 | #include "test_util.h" 18 | 19 | // Test fixture 20 | 21 | class SdoClientTest : public TestBase 22 | { 23 | }; 24 | 25 | // Tests 26 | 27 | TEST_F (SdoClientTest, ExpeditedUpload) 28 | { 29 | co_job_t job; 30 | uint32_t value; 31 | 32 | uint8_t expected[][8] = { 33 | {0x40, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 34 | }; 35 | uint8_t response[][8] = { 36 | {0x43, 0x00, 0x10, 0x00, 0x92, 0x01, 0x42, 0x00}, 37 | }; 38 | 39 | job.type = CO_JOB_SDO_READ; 40 | job.sdo.node = 1; 41 | job.sdo.index = 0x1000; 42 | job.sdo.subindex = 0; 43 | job.sdo.data = (uint8_t *)&value; 44 | job.sdo.remain = sizeof (value); 45 | job.callback = NULL; 46 | 47 | mock_os_channel_send_calls = 0; 48 | 49 | co_sdo_issue (&net, &job); 50 | EXPECT_TRUE (CanMatch (0x601, expected[0], 8)); 51 | 52 | co_sdo_tx (&net, 1, response[0], 8); 53 | EXPECT_EQ (0x00420192u, value); 54 | EXPECT_EQ (NELEMENTS (expected), mock_os_channel_send_calls); 55 | 56 | EXPECT_EQ (4u, job.sdo.total); 57 | } 58 | 59 | TEST_F (SdoClientTest, ExpeditedDownload) 60 | { 61 | co_job_t job{}; 62 | uint16_t value = 0; 63 | 64 | uint8_t expected[][8] = { 65 | {0x2b, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}, 66 | }; 67 | uint8_t response[][8] = { 68 | {0x60, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}, 69 | }; 70 | 71 | job.type = CO_JOB_SDO_WRITE; 72 | job.sdo.node = 1; 73 | job.sdo.index = 0x6040; 74 | job.sdo.subindex = 0; 75 | job.sdo.data = (uint8_t *)&value; 76 | job.sdo.remain = sizeof (value); 77 | job.callback = NULL; 78 | 79 | mock_os_channel_send_calls = 0; 80 | 81 | co_sdo_issue (&net, &job); 82 | EXPECT_TRUE (CanMatch (0x601, expected[0], 8)); 83 | 84 | co_sdo_tx (&net, 1, response[0], 8); 85 | EXPECT_EQ (NELEMENTS (expected), mock_os_channel_send_calls); 86 | 87 | EXPECT_EQ (2u, job.sdo.total); 88 | } 89 | 90 | TEST_F (SdoClientTest, SegmentedUpload) 91 | { 92 | co_job_t job; 93 | uint8_t value[16]; 94 | 95 | uint8_t expected[][8] = { 96 | {0x40, 0x0a, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 97 | {0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 98 | {0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 99 | {0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 100 | {0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 101 | }; 102 | uint8_t response[][8] = { 103 | {0x41, 0x0a, 0x10, 0x00, 0x1c, 0x00, 0x00, 0x00}, 104 | {0x00, 0x57, 0x68, 0x69, 0x73, 0x74, 0x6c, 0x65}, 105 | {0x10, 0x20, 0x32, 0x2e, 0x30, 0x32, 0x2e, 0x30}, 106 | {0x00, 0x37, 0x2e, 0x30, 0x30, 0x20, 0x31, 0x30}, 107 | {0x11, 0x4d, 0x61, 0x79, 0x32, 0x30, 0x30, 0x36}, 108 | }; 109 | 110 | job.type = CO_JOB_SDO_READ; 111 | job.sdo.node = 1; 112 | job.sdo.index = 0x100a; 113 | job.sdo.subindex = 0; 114 | job.sdo.data = value; 115 | job.sdo.remain = sizeof (value); 116 | job.callback = NULL; 117 | 118 | mock_os_channel_send_calls = 0; 119 | 120 | co_sdo_issue (&net, &job); 121 | EXPECT_TRUE (CanMatch (0x601, expected[0], 8)); 122 | 123 | for (size_t i = 0; i < NELEMENTS (response); i++) 124 | { 125 | co_sdo_tx (&net, 1, response[i], 8); 126 | EXPECT_EQ (0x601u, mock_os_channel_send_id); 127 | if (i != NELEMENTS (response) - 1) 128 | { 129 | EXPECT_TRUE (CanMatch (0x601, expected[i + 1], 8)); 130 | } 131 | } 132 | EXPECT_EQ (NELEMENTS (expected), mock_os_channel_send_calls); 133 | 134 | EXPECT_EQ (16u, job.sdo.total); 135 | } 136 | 137 | TEST_F (SdoClientTest, SegmentedDownload) 138 | { 139 | co_job_t job{}; 140 | const char * s = "hello world"; 141 | 142 | uint8_t expected[][8] = { 143 | {0x21, 0x99, 0x69, 0x00, 0x0b, 0x00, 0x00, 0x00}, 144 | {0x00, 0x68, 0x65, 0x6c, 0x6c, 0x6f, 0x20, 0x77}, 145 | {0x17, 0x6f, 0x72, 0x6c, 0x64, 0x00, 0x00, 0x00}, 146 | }; 147 | uint8_t response[][8] = { 148 | {0x60, 0x99, 0x69, 0x00, 0x00, 0x00, 0x00, 0x00}, 149 | {0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 150 | {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 151 | }; 152 | 153 | job.type = CO_JOB_SDO_WRITE; 154 | job.sdo.node = 1; 155 | job.sdo.index = 0x6999; 156 | job.sdo.subindex = 0; 157 | job.sdo.data = (uint8_t *)s; 158 | job.sdo.remain = strlen (s); 159 | job.callback = NULL; 160 | 161 | mock_os_channel_send_calls = 0; 162 | 163 | co_sdo_issue (&net, &job); 164 | EXPECT_TRUE (CanMatch (0x601, expected[0], 8)); 165 | 166 | for (size_t i = 0; i < NELEMENTS (response); i++) 167 | { 168 | co_sdo_tx (&net, 1, response[i], 8); 169 | EXPECT_EQ (0x601u, mock_os_channel_send_id); 170 | if (i != NELEMENTS (response) - 1) 171 | { 172 | EXPECT_TRUE (CanMatch (0x601, expected[i + 1], 8)); 173 | } 174 | } 175 | EXPECT_EQ (NELEMENTS (expected), mock_os_channel_send_calls); 176 | 177 | EXPECT_EQ (strlen (s), job.sdo.total); 178 | } 179 | -------------------------------------------------------------------------------- /test/test_sdo_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_sdo.h" 17 | #include "co_od.h" 18 | 19 | #include "options.h" 20 | #include 21 | 22 | #include "mocks.h" 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "test_util.h" 30 | 31 | // Test fixture 32 | 33 | class SdoServerTest : public TestBase 34 | { 35 | }; 36 | 37 | // Tests 38 | 39 | TEST_F (SdoServerTest, ExpeditedUpload) 40 | { 41 | uint8_t expected[][8] = { 42 | {0x43, 0x00, 0x10, 0x00, 0x92, 0x01, 0x42, 0x00}, 43 | }; 44 | uint8_t command[][8] = { 45 | {0x40, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 46 | }; 47 | 48 | mock_co_obj_find_result = find_obj (0x1000); 49 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0); 50 | 51 | co_sdo_rx (&net, 1, command[0], 8); 52 | EXPECT_TRUE (CanMatch (0x581, expected[0], 8)); 53 | } 54 | 55 | TEST_F (SdoServerTest, ExpeditedUploadString) 56 | { 57 | uint8_t expected[][8] = { 58 | {0x47, 0x0a, 0x10, 0x00, 0x31, 0x32, 0x33, 0x00}, 59 | }; 60 | uint8_t command[][8] = { 61 | {0x40, 0x0a, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 62 | }; 63 | 64 | mock_co_obj_find_result = find_obj (0x100A); 65 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0); 66 | 67 | co_sdo_rx (&net, 1, command[0], 8); 68 | EXPECT_TRUE (CanMatch (0x581, expected[0], 8)); 69 | } 70 | 71 | TEST_F (SdoServerTest, ExpeditedDownload) 72 | { 73 | uint8_t expected[][8] = { 74 | {0x60, 0x03, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00}, 75 | }; 76 | uint8_t command[][8] = { 77 | {0x23, 0x03, 0x60, 0x00, 0x78, 0x56, 0x34, 0x12}, 78 | }; 79 | 80 | mock_co_obj_find_result = find_obj (0x6003); 81 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 9); 82 | 83 | co_sdo_rx (&net, 1, command[0], 8); 84 | EXPECT_TRUE (CanMatch (0x581, expected[0], 8)); 85 | 86 | EXPECT_EQ (0x12345678u, value6003_09); 87 | } 88 | 89 | TEST_F (SdoServerTest, SegmentedUpload) 90 | { 91 | uint8_t expected[][8] = { 92 | {0x41, 0x08, 0x10, 0x00, 0x09, 0x00, 0x00, 0x00}, 93 | {0x00, 0x6e, 0x65, 0x77, 0x20, 0x73, 0x6c, 0x61}, 94 | {0x1b, 0x76, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00}, 95 | }; 96 | uint8_t command[][8] = { 97 | {0x40, 0x08, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 98 | {0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 99 | {0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 100 | }; 101 | 102 | mock_co_obj_find_result = find_obj (0x1008); 103 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0); 104 | 105 | for (size_t i = 0; i < NELEMENTS (command); i++) 106 | { 107 | co_sdo_rx (&net, 1, command[i], 8); 108 | EXPECT_TRUE (CanMatch (0x581, expected[i], 8)); 109 | } 110 | } 111 | 112 | TEST_F (SdoServerTest, SegmentedDownload) 113 | { 114 | uint8_t expected[][8] = { 115 | {0x60, 0x09, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 116 | {0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 117 | {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 118 | }; 119 | uint8_t command[][8] = { 120 | {0x21, 0x09, 0x10, 0x00, 0x0e, 0x00, 0x00, 0x00}, 121 | {0x00, 0x6e, 0x65, 0x77, 0x20, 0x73, 0x6c, 0x61}, 122 | {0x11, 0x76, 0x65, 0x20, 0x6e, 0x61, 0x6d, 0x65}, 123 | }; 124 | 125 | mock_co_obj_find_result = find_obj (0x1009); 126 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0); 127 | 128 | for (size_t i = 0; i < NELEMENTS (command); i++) 129 | { 130 | co_sdo_rx (&net, 1, command[i], 8); 131 | EXPECT_TRUE (CanMatch (0x581, expected[i], 8)); 132 | } 133 | 134 | EXPECT_STREQ ("new slave name", name1009); 135 | EXPECT_EQ (1u, cb_notify_calls); 136 | } 137 | 138 | TEST_F (SdoServerTest, SegmentedDownloadCached) 139 | { 140 | uint8_t expected[][8] = { 141 | {0x60, 0x03, 0x60, 0x0A, 0x00, 0x00, 0x00, 0x00}, 142 | {0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 143 | {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 144 | }; 145 | uint8_t command[][8] = { 146 | {0x21, 0x03, 0x60, 0x0A, 0x08, 0x00, 0x00, 0x00}, 147 | {0x00, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66}, 148 | {0x11, 0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 149 | }; 150 | 151 | mock_co_obj_find_result = find_obj (0x6003); 152 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 11); 153 | 154 | for (size_t i = 0; i < NELEMENTS (command); i++) 155 | { 156 | co_sdo_rx (&net, 1, command[i], 8); 157 | EXPECT_TRUE (CanMatch (0x581, expected[i], 8)); 158 | } 159 | } 160 | 161 | TEST_F (SdoServerTest, SegmentedTimeout) 162 | { 163 | uint8_t expected[][8] = { 164 | {0x41, 0x08, 0x10, 0x00, 0x09, 0x00, 0x00, 0x00}, 165 | {0x00, 0x6e, 0x65, 0x77, 0x20, 0x73, 0x6c, 0x61}, 166 | {0x80, 0x08, 0x10, 0x00, 0x00, 0x00, 0x04, 0x05}, 167 | }; 168 | uint8_t command[][8] = { 169 | {0x40, 0x08, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 170 | {0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 171 | }; 172 | os_tick_t now; 173 | 174 | mock_co_obj_find_result = find_obj (0x1008); 175 | mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0); 176 | 177 | co_sdo_rx (&net, 1, command[0], 8); 178 | 179 | // Should not time out 180 | now = 1000 * SDO_TIMEOUT - 1; 181 | co_sdo_server_timer (&net, now); 182 | EXPECT_TRUE (CanMatch (0x581, expected[0], 8)); 183 | 184 | mock_os_tick_current_result = now; 185 | co_sdo_rx (&net, 1, command[1], 8); 186 | 187 | // Should not time out 188 | now += 1000 * SDO_TIMEOUT - 1; 189 | co_sdo_server_timer (&net, now); 190 | EXPECT_TRUE (CanMatch (0x581, expected[1], 8)); 191 | 192 | // Should abort with timeout 193 | now += 1; 194 | co_sdo_server_timer (&net, now); 195 | EXPECT_TRUE (CanMatch (0x581, expected[2], 8)); 196 | } 197 | 198 | TEST_F (SdoServerTest, BadSubIndex) 199 | { 200 | static const co_obj_t obj = {0, OTYPE_NULL, 0, NULL, NULL}; 201 | uint8_t expected[][8] = { 202 | {0x80, 0x00, 0x10, 0x01, 0x11, 0x00, 0x09, 0x06}, 203 | }; 204 | uint8_t command[][8] = { 205 | {0x40, 0x00, 0x10, 0x01, 0x00, 0x00, 0x00, 0x00}, 206 | }; 207 | 208 | mock_co_obj_find_result = &obj; 209 | mock_co_entry_find_result = NULL; 210 | 211 | for (size_t i = 0; i < NELEMENTS (command); i++) 212 | { 213 | co_sdo_rx (&net, 1, command[i], 8); 214 | EXPECT_TRUE (CanMatch (0x581, expected[i], 8)); 215 | } 216 | } 217 | 218 | TEST_F (SdoServerTest, SubIndexFF) 219 | { 220 | uint8_t expected[][8] = { 221 | {0x43, 0x00, 0x10, 0xFF, 0x07, 0x07, 0x00, 0x00}, 222 | {0x43, 0x03, 0x10, 0xFF, 0x08, 0x07, 0x00, 0x00}, 223 | }; 224 | uint8_t command[][8] = { 225 | {0x40, 0x00, 0x10, 0xFF, 0x00, 0x00, 0x00, 0x00}, 226 | {0x40, 0x03, 0x10, 0xFF, 0x00, 0x00, 0x00, 0x00}, 227 | }; 228 | 229 | mock_co_obj_find_result = find_obj (0x1000); 230 | mock_co_entry_find_result = NULL; 231 | co_sdo_rx (&net, 1, command[0], 8); 232 | EXPECT_TRUE (CanMatch (0x581, expected[0], 8)); 233 | 234 | mock_co_obj_find_result = find_obj (0x1003); 235 | mock_co_entry_find_result = NULL; 236 | co_sdo_rx (&net, 1, command[1], 8); 237 | EXPECT_TRUE (CanMatch (0x581, expected[1], 8)); 238 | } 239 | -------------------------------------------------------------------------------- /test/test_sync.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #include "co_sync.h" 17 | #include "test_util.h" 18 | 19 | // Test fixture 20 | 21 | class SyncTest : public TestBase 22 | { 23 | }; 24 | 25 | // Tests 26 | 27 | TEST_F (SyncTest, CobIdSyncMessageConfiguration) 28 | { 29 | uint32_t value; 30 | uint32_t result; 31 | 32 | value = 0x40000000; 33 | result = co_od1005_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 34 | EXPECT_EQ (0x06090030u, result); 35 | 36 | value = 0x40000086; 37 | result = co_od1005_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 38 | EXPECT_EQ (0u, result); 39 | 40 | value = 0x40000085; 41 | result = co_od1005_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 42 | EXPECT_EQ (0x08000000u, result); 43 | 44 | value = 0x00000085; 45 | result = co_od1005_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 46 | EXPECT_EQ (0x0u, result); 47 | 48 | value = 0x00000086; 49 | result = co_od1005_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 50 | EXPECT_EQ (0u, result); 51 | 52 | value = 0x40000086; 53 | result = co_od1005_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 54 | EXPECT_EQ (0u, result); 55 | 56 | value = 0; 57 | result = co_od1005_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 58 | EXPECT_EQ (0u, result); 59 | EXPECT_EQ (0x40000086u, value); 60 | } 61 | 62 | TEST_F (SyncTest, OD1006) 63 | { 64 | uint32_t value; 65 | uint32_t result; 66 | 67 | value = 1000; 68 | result = co_od1006_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 69 | EXPECT_EQ (0u, result); 70 | EXPECT_EQ (net.sync.period, value); 71 | 72 | net.sync.period = 2000; 73 | result = co_od1006_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 74 | EXPECT_EQ (0u, result); 75 | EXPECT_EQ (value, net.sync.period); 76 | } 77 | 78 | TEST_F (SyncTest, OD1019) 79 | { 80 | uint32_t value; 81 | uint32_t result; 82 | 83 | value = 240; 84 | result = co_od1019_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 85 | EXPECT_EQ (0u, result); 86 | EXPECT_EQ (net.sync.overflow, value); 87 | 88 | net.sync.overflow = 10; 89 | result = co_od1019_fn (&net, OD_EVENT_READ, NULL, NULL, 0, &value); 90 | EXPECT_EQ (0u, result); 91 | EXPECT_EQ (value, net.sync.overflow); 92 | 93 | // Reserved values, should fail 94 | value = 1; 95 | result = co_od1019_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 96 | EXPECT_EQ (CO_SDO_ABORT_VALUE, result); 97 | EXPECT_EQ (10u, net.sync.overflow); 98 | 99 | // Reserved values, should fail 100 | value = 241; 101 | result = co_od1019_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 102 | EXPECT_EQ (CO_SDO_ABORT_VALUE, result); 103 | EXPECT_EQ (10u, net.sync.overflow); 104 | 105 | // Should fail if sync producer is active 106 | net.sync.period = 1000; 107 | value = 2; 108 | result = co_od1019_fn (&net, OD_EVENT_WRITE, NULL, NULL, 0, &value); 109 | EXPECT_EQ (CO_SDO_ABORT_WRITE_STATE_DENIED, result); 110 | EXPECT_EQ (10u, net.sync.overflow); 111 | } 112 | 113 | TEST_F (SyncTest, SyncNoCounter) 114 | { 115 | co_sync_t * sync = &net.sync; 116 | 117 | sync->cobid = 0x40000080; 118 | sync->period = 100; 119 | sync->counter = 0; 120 | sync->timestamp = 0; 121 | 122 | co_sync_timer (&net, 100); 123 | EXPECT_EQ (1u, mock_os_channel_send_calls); 124 | EXPECT_TRUE (CanMatch (0x80, NULL, 0)); 125 | EXPECT_EQ (1u, cb_sync_calls); 126 | 127 | co_sync_timer (&net, 199); 128 | EXPECT_EQ (1u, mock_os_channel_send_calls); 129 | 130 | co_sync_timer (&net, 200); 131 | EXPECT_EQ (2u, mock_os_channel_send_calls); 132 | EXPECT_TRUE (CanMatch (0x80, NULL, 0)); 133 | EXPECT_EQ (2u, cb_sync_calls); 134 | } 135 | 136 | TEST_F (SyncTest, SyncCounter) 137 | { 138 | co_sync_t * sync = &net.sync; 139 | uint8_t expected[] = {0x01, 0x02, 0x03, 0x01}; 140 | 141 | sync->cobid = 0x40000080; 142 | sync->period = 100; 143 | sync->overflow = 3; 144 | sync->counter = 1; 145 | sync->timestamp = 0; 146 | 147 | co_sync_timer (&net, 100); 148 | EXPECT_EQ (1u, mock_os_channel_send_calls); 149 | EXPECT_TRUE (CanMatch (0x80, &expected[0], 1)); 150 | 151 | co_sync_timer (&net, 199); 152 | EXPECT_EQ (1u, mock_os_channel_send_calls); 153 | 154 | co_sync_timer (&net, 200); 155 | EXPECT_EQ (2u, mock_os_channel_send_calls); 156 | EXPECT_TRUE (CanMatch (0x80, &expected[1], 1)); 157 | 158 | co_sync_timer (&net, 300); 159 | EXPECT_EQ (3u, mock_os_channel_send_calls); 160 | EXPECT_TRUE (CanMatch (0x80, &expected[2], 1)); 161 | 162 | co_sync_timer (&net, 400); 163 | EXPECT_EQ (4u, mock_os_channel_send_calls); 164 | EXPECT_TRUE (CanMatch (0x80, &expected[3], 1)); 165 | } 166 | -------------------------------------------------------------------------------- /test/test_util.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef TEST_UTIL_H 17 | #define TEST_UTIL_H 18 | 19 | #include 20 | #include "mocks.h" 21 | #include "co_obj.h" 22 | #include "co_nmt.h" 23 | #include "options.h" 24 | #include "co_od.h" 25 | #include "co_pdo.h" 26 | 27 | extern "C" uint32_t cb2001 ( 28 | co_net_t * net, 29 | od_event_t event, 30 | const co_obj_t * obj, 31 | const co_entry_t * entry, 32 | uint8_t subindex, 33 | uint32_t * value); 34 | 35 | class TestBase : public ::testing::Test 36 | { 37 | protected: 38 | virtual void SetUp() 39 | { 40 | memset (&net, 0, sizeof (net)); 41 | store_init(); 42 | 43 | net.node = 1; 44 | net.od = test_od; 45 | net.cb_emcy = cb_emcy; 46 | net.cb_reset = cb_reset; 47 | net.cb_nmt = cb_nmt; 48 | net.cb_sync = cb_sync; 49 | net.cb_notify = cb_notify; 50 | net.cb_heartbeat_state = cb_heartbeat_state; 51 | net.open = store_open; 52 | net.read = store_read; 53 | net.write = store_write; 54 | net.close = store_close; 55 | 56 | co_pdo_init (&net); 57 | co_nmt_init (&net); 58 | co_od_reset (&net, CO_STORE_COMM, 0x1000, 0x1FFF); 59 | 60 | cb_emcy_calls = 0; 61 | cb_reset_calls = 0; 62 | cb_nmt_calls = 0; 63 | cb_sync_calls = 0; 64 | cb_notify_calls = 0; 65 | cb_heartbeat_state_calls = 0; 66 | 67 | mock_os_tick_current_result = 0; 68 | mock_os_channel_send_calls = 0; 69 | mock_os_channel_send_id = 0; 70 | mock_os_channel_receive_calls = 0; 71 | mock_os_channel_bus_off_calls = 0; 72 | mock_os_channel_bus_on_calls = 0; 73 | mock_os_channel_set_bitrate_calls = 0; 74 | mock_os_channel_set_filter_calls = 0; 75 | mock_os_channel_get_state_calls = 0; 76 | mock_co_od_reset_calls = 0; 77 | mock_co_emcy_tx_calls = 0; 78 | store_open_calls = 0; 79 | 80 | strcpy (name1008, "new slave"); 81 | OD1008[0].bitlength = 8 * strlen (name1008); 82 | 83 | strcpy (name100A, "123"); 84 | OD100A[0].bitlength = 8 * strlen (name100A); 85 | } 86 | 87 | co_net_t net; 88 | 89 | const co_entry_t OD1000[1] = { 90 | {0, OD_RO, DTYPE_UNSIGNED32, 32, 0x00420192, NULL}, 91 | }; 92 | 93 | char name1008[20]; 94 | co_entry_t OD1008[1] = { 95 | {0, OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name1008), 0, name1008}, 96 | }; 97 | 98 | char name1009[20] = {0}; 99 | co_entry_t OD1009[1] = { 100 | {0, OD_NOTIFY | OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name1009), 0, name1009}, 101 | }; 102 | 103 | char name100A[20]; 104 | co_entry_t OD100A[1] = { 105 | {0, OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name100A), 0, name100A}, 106 | }; 107 | 108 | const co_entry_t OD1018[5] = { 109 | {0, OD_RO, DTYPE_UNSIGNED8, 8, 4, NULL}, 110 | {1, OD_RO, DTYPE_UNSIGNED32, 32, 1, NULL}, 111 | {2, OD_RO, DTYPE_UNSIGNED32, 32, 2, NULL}, 112 | {3, OD_RO, DTYPE_UNSIGNED32, 32, 3, NULL}, 113 | {4, OD_RO, DTYPE_UNSIGNED32, 32, 4, NULL}, 114 | }; 115 | 116 | uint32_t arr2000[8] = {1, 2, 3, 4, 5, 6, 7, 8}; 117 | const co_entry_t OD2000[2] = { 118 | {0x00, OD_RO, DTYPE_UNSIGNED8, 8, 8, NULL}, 119 | {0x01, OD_RW | OD_ARRAY, DTYPE_UNSIGNED32, 32, 0, arr2000}, 120 | }; 121 | 122 | const co_entry_t OD2001[3] = { 123 | {0x00, OD_RW, DTYPE_UNSIGNED8, 8, 2, NULL}, 124 | {0x01, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 125 | {0x02, OD_RW, DTYPE_UNSIGNED32, 32, 0, NULL}, 126 | }; 127 | 128 | uint32_t value6000 = 0x12345678; 129 | const co_entry_t OD6000[1] = { 130 | {0, OD_NOTIFY | OD_RO | OD_TPDO, DTYPE_UNSIGNED32, 32, 0, &value6000}, 131 | }; 132 | 133 | const co_entry_t OD6001[1] = { 134 | {0, OD_NOTIFY | OD_RO | OD_TPDO, DTYPE_UNSIGNED32, 32, 1, NULL}, 135 | }; 136 | 137 | uint16_t value6003_07; 138 | uint8_t value6003_08; 139 | uint32_t value6003_09; 140 | uint8_t value6003_0A; 141 | uint64_t value6003_0B __attribute__ ((aligned (8))); 142 | const co_entry_t OD6003[12] = { 143 | {0x00, OD_RW, DTYPE_UNSIGNED8, 8, 0x0B, NULL}, 144 | // Pack 145 | {0x01, OD_RW, DTYPE_UNSIGNED16, 16, 4, NULL}, 146 | {0x02, OD_RW, DTYPE_UNSIGNED8, 8, 99, NULL}, 147 | // PackLarge 148 | {0x03, OD_RW, DTYPE_UNSIGNED32, 32, 4, NULL}, 149 | {0x04, OD_RW, DTYPE_UNSIGNED8, 8, 99, NULL}, 150 | // PackSmall 151 | {0x05, OD_RW, DTYPE_BOOLEAN, 1, 0, NULL}, 152 | {0x06, OD_RW, DTYPE_BOOLEAN, 1, 1, NULL}, 153 | // Unpack 154 | {0x07, OD_RW, DTYPE_UNSIGNED16, 16, 0, &value6003_07}, 155 | {0x08, OD_RW, DTYPE_UNSIGNED8, 8, 0, &value6003_08}, 156 | // UnpackLarge 157 | {0x09, OD_RW, DTYPE_UNSIGNED32, 32, 0, &value6003_09}, 158 | {0x0A, OD_RW, DTYPE_UNSIGNED8, 8, 0, &value6003_0A}, 159 | // Largest 160 | {0x0B, OD_RW, DTYPE_UNSIGNED64, 64, 0, &value6003_0B}, 161 | }; 162 | 163 | uint32_t value7000; 164 | const co_entry_t OD7000[1] = { 165 | {0, OD_RW | OD_RPDO, DTYPE_UNSIGNED32, 32, 0, &value7000}, 166 | }; 167 | 168 | const co_obj_t test_od[37] = { 169 | // clang-format off 170 | {0x1000, OTYPE_VAR, 0, OD1000, NULL}, 171 | {0x1001, OTYPE_VAR, 0, OD1001, co_od1001_fn}, 172 | {0x1003, OTYPE_ARRAY, MAX_ERRORS, OD1003, co_od1003_fn}, 173 | {0x1005, OTYPE_VAR, 0, OD1005, co_od1005_fn}, 174 | {0x1006, OTYPE_VAR, 0, OD1006, co_od1006_fn}, 175 | {0x1007, OTYPE_VAR, 0, OD1007, co_od1007_fn}, 176 | {0x1008, OTYPE_VAR, 0, OD1008, NULL}, 177 | {0x1009, OTYPE_VAR, 0, OD1009, NULL}, 178 | {0x100A, OTYPE_VAR, 0, OD100A, NULL}, 179 | {0x100C, OTYPE_VAR, 0, OD100C, co_od100C_fn}, 180 | {0x100D, OTYPE_VAR, 0, OD100D, co_od100D_fn}, 181 | {0x1010, OTYPE_ARRAY, 4, OD1010, co_od1010_fn}, 182 | {0x1011, OTYPE_ARRAY, 4, OD1011, co_od1011_fn}, 183 | {0x1014, OTYPE_VAR, 0, OD1014, co_od1014_fn}, 184 | {0x1015, OTYPE_VAR, 0, OD1015, co_od1015_fn}, 185 | {0x1016, OTYPE_ARRAY, MAX_HEARTBEATS, OD1016, co_od1016_fn}, 186 | {0x1018, OTYPE_RECORD, 4, OD1018, NULL}, 187 | {0x1017, OTYPE_VAR, 0, OD1017, co_od1017_fn}, 188 | {0x1019, OTYPE_VAR, 0, OD1019, co_od1019_fn}, 189 | {0x1020, OTYPE_ARRAY, 2, OD1020, co_od1020_fn}, 190 | {0x1028, OTYPE_ARRAY, MAX_EMCY_COBIDS, OD1028, co_od1028_fn}, 191 | {0x1029, OTYPE_ARRAY, 1, OD1029, co_od1029_fn}, 192 | {0x1400, OTYPE_RECORD, 5, OD1400, co_od1400_fn}, 193 | {0x1533, OTYPE_RECORD, 5, OD1400, co_od1400_fn}, 194 | {0x1600, OTYPE_RECORD, MAX_PDO_ENTRIES, OD1600, co_od1600_fn}, 195 | {0x1733, OTYPE_RECORD, MAX_PDO_ENTRIES, OD1600, co_od1600_fn}, 196 | {0x1800, OTYPE_RECORD, 6, OD1800, co_od1800_fn}, 197 | {0x1899, OTYPE_RECORD, 6, OD1800, co_od1800_fn}, 198 | {0x1A00, OTYPE_RECORD, MAX_PDO_ENTRIES, OD1A00, co_od1A00_fn}, 199 | {0x1A99, OTYPE_RECORD, MAX_PDO_ENTRIES, OD1A00, co_od1A00_fn}, 200 | {0x2000, OTYPE_ARRAY, 8, OD2000, NULL}, 201 | {0x2001, OTYPE_RECORD, 2, OD2001, cb2001}, 202 | {0x6000, OTYPE_VAR, 0, OD6000, NULL}, 203 | {0x6001, OTYPE_VAR, 0, OD6001, NULL}, 204 | {0x6003, OTYPE_RECORD, 12, OD6003, NULL}, 205 | {0x7000, OTYPE_VAR, 0, OD7000, NULL}, 206 | {0, OTYPE_NULL, 0, NULL, NULL}, 207 | // clang-format on 208 | }; 209 | 210 | const co_obj_t * find_obj (uint16_t index) 211 | { 212 | return co_obj_find (&net, index); 213 | } 214 | 215 | const co_entry_t * find_entry (const co_obj_t * obj, uint8_t subindex) 216 | { 217 | return co_entry_find (&net, obj, subindex); 218 | } 219 | }; 220 | 221 | inline std::string FormatHexInt (int value) 222 | { 223 | std::stringstream ss; 224 | ss << std::hex << std::showbase << value; 225 | return ss.str(); 226 | } 227 | 228 | inline std::string FormatByte (uint8_t value) 229 | { 230 | std::stringstream ss; 231 | ss << std::setfill ('0') << std::setw (2) << std::hex << std::showbase 232 | << static_cast (value); 233 | return ss.str(); 234 | } 235 | 236 | template 237 | ::testing::AssertionResult ArraysMatch ( 238 | const T (&expected)[size], 239 | const T (&actual)[size]) 240 | { 241 | for (size_t i (0); i < size; ++i) 242 | { 243 | if (expected[i] != actual[i]) 244 | { 245 | return ::testing::AssertionFailure() 246 | << "actual[" << i << "] (" 247 | << FormatByte (static_cast (actual[i])) << ") != expected[" 248 | << i << "] (" << FormatByte (static_cast (expected[i])) 249 | << ")"; 250 | } 251 | } 252 | 253 | return ::testing::AssertionSuccess(); 254 | } 255 | 256 | inline ::testing::AssertionResult CanMatch (uint32_t id, void * expected, size_t dlc) 257 | { 258 | uint8_t * data = (uint8_t *)expected; 259 | 260 | if (id != mock_os_channel_send_id) 261 | return ::testing::AssertionFailure() 262 | << "id (" << FormatHexInt (mock_os_channel_send_id) 263 | << ") != expected (" << FormatHexInt (id) << ")"; 264 | 265 | if (dlc != mock_os_channel_send_dlc) 266 | return ::testing::AssertionFailure() << "dlc (" << mock_os_channel_send_dlc 267 | << ") != expected (" << dlc << ")"; 268 | 269 | for (size_t i (0); i < dlc; ++i) 270 | { 271 | if (data[i] != mock_os_channel_send_data[i]) 272 | { 273 | return ::testing::AssertionFailure() 274 | << "actual[" << i << "] (" 275 | << FormatByte (mock_os_channel_send_data[i]) << ") != expected[" 276 | << i << "] (" << FormatByte (data[i]) << ")"; 277 | } 278 | } 279 | 280 | return ::testing::AssertionSuccess(); 281 | } 282 | 283 | #endif /* TEST_UTIL_H */ 284 | -------------------------------------------------------------------------------- /version.h.in: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * _ _ _ 3 | * _ __ | |_ _ | | __ _ | |__ ___ 4 | * | '__|| __|(_)| | / _` || '_ \ / __| 5 | * | | | |_ _ | || (_| || |_) |\__ \ 6 | * |_| \__|(_)|_| \__,_||_.__/ |___/ 7 | * 8 | * www.rt-labs.com 9 | * Copyright 2017 rt-labs AB, Sweden. 10 | * 11 | * This software is dual-licensed under GPLv3 and a commercial 12 | * license. See the file LICENSE.md distributed with this software for 13 | * full license information. 14 | ********************************************************************/ 15 | 16 | #ifndef VERSION_H 17 | #define VERSION_H 18 | 19 | #cmakedefine COPEN_GIT_REVISION "@COPEN_GIT_REVISION@" 20 | 21 | #if !defined(CO_VERSION_BUILD) && defined(COPEN_GIT_REVISION) 22 | #define CO_VERSION_BUILD COPEN_GIT_REVISION 23 | #endif 24 | 25 | /* clang-format-off */ 26 | 27 | #define CO_VERSION_MAJOR @COPEN_VERSION_MAJOR@ 28 | #define CO_VERSION_MINOR @COPEN_VERSION_MINOR@ 29 | #define CO_VERSION_PATCH @COPEN_VERSION_PATCH@ 30 | 31 | #if defined(CO_VERSION_BUILD) 32 | #define CO_VERSION \ 33 | "@COPEN_VERSION_MAJOR@.@COPEN_VERSION_MINOR@.@COPEN_VERSION_PATCH@+"CO_VERSION_BUILD 34 | #else 35 | #define CO_VERSION \ 36 | "@COPEN_VERSION_MAJOR@.@COPEN_VERSION_MINOR@.@COPEN_VERSION_PATCH@" 37 | #endif 38 | 39 | /* clang-format-on */ 40 | 41 | #endif /* VERSION_H */ 42 | --------------------------------------------------------------------------------