├── .clang-format ├── .clang-tidy ├── .github └── workflows │ └── main.yml ├── .gitignore ├── .idea └── dictionaries │ └── pavel.xml ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── docs ├── state_machine.dia └── state_machine.svg ├── kocherga ├── kocherga.hpp ├── kocherga_can.hpp └── kocherga_serial.hpp ├── tests ├── .clang-tidy ├── 3rd_party │ └── catch.hpp ├── CMakeLists.txt ├── integration │ ├── .gitignore │ ├── CMakeLists.txt │ ├── bootloader │ │ └── main.cpp │ └── validator │ │ ├── can.orc.yaml │ │ ├── com.zubax.kocherga.test.integration-10-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin │ │ ├── manual_v0.orc.yaml │ │ ├── serial.orc.yaml │ │ └── validate.sh ├── tools │ ├── .gitignore │ ├── demo-be.bin │ ├── demo-le.bin │ ├── invalid.bin │ └── test.sh ├── unit │ ├── CMakeLists.txt │ ├── can │ │ ├── test_misc.cpp │ │ └── test_node.cpp │ ├── images │ │ ├── bad-le-crc-x3.bin │ │ ├── bad-le-short.bin │ │ ├── com.zubax.telega-1-0.3.68620b82.application.bin │ │ ├── good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin │ │ └── good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin │ ├── mock.hpp │ ├── serial │ │ ├── test_node.cpp │ │ └── test_stream.cpp │ ├── test_app_locator.cpp │ ├── test_core.cpp │ ├── test_main.cpp │ ├── test_misc.cpp │ ├── test_presenter.cpp │ └── test_util.cpp └── util │ └── util.hpp └── tools ├── kocherga_image.py └── pyproject.toml /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -4 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: true 7 | AlignConsecutiveDeclarations: true 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: false 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: Inline 15 | AllowShortIfStatementsOnASingleLine: Never 16 | AllowShortLoopsOnASingleLine: false 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: false 20 | AlwaysBreakTemplateDeclarations: Yes 21 | BinPackArguments: false 22 | BinPackParameters: false 23 | BraceWrapping: 24 | AfterCaseLabel: true 25 | AfterClass: true 26 | AfterControlStatement: true 27 | AfterEnum: true 28 | AfterFunction: true 29 | AfterNamespace: true 30 | AfterStruct: true 31 | AfterUnion: true 32 | BeforeCatch: true 33 | BeforeElse: true 34 | IndentBraces: false 35 | SplitEmptyFunction: false 36 | SplitEmptyRecord: false 37 | SplitEmptyNamespace: false 38 | AfterExternBlock: false # Keeps the contents un-indented. 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Custom 41 | BreakBeforeTernaryOperators: true 42 | BreakConstructorInitializers: AfterColon 43 | # BreakInheritanceList: AfterColon 44 | BreakStringLiterals: true 45 | ColumnLimit: 120 46 | CommentPragmas: '^ (coverity|pragma:)' 47 | CompactNamespaces: false 48 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 49 | ConstructorInitializerIndentWidth: 4 50 | ContinuationIndentWidth: 4 51 | Cpp11BracedListStyle: true 52 | DerivePointerAlignment: false 53 | DisableFormat: false 54 | ExperimentalAutoDetectBinPacking: false 55 | FixNamespaceComments: true 56 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 57 | IncludeBlocks: Preserve 58 | IndentCaseLabels: false 59 | IndentPPDirectives: AfterHash 60 | IndentWidth: 4 61 | IndentWrappedFunctionNames: false 62 | KeepEmptyLinesAtTheStartOfBlocks: false 63 | MacroBlockBegin: '' 64 | MacroBlockEnd: '' 65 | MaxEmptyLinesToKeep: 1 66 | NamespaceIndentation: None 67 | PenaltyBreakAssignment: 2 68 | PenaltyBreakBeforeFirstCallParameter: 10000 # Raised intentionally; prefer breaking all 69 | PenaltyBreakComment: 300 70 | PenaltyBreakFirstLessLess: 120 71 | PenaltyBreakString: 1000 72 | PenaltyExcessCharacter: 1000000 73 | PenaltyReturnTypeOnItsOwnLine: 10000 # Raised intentionally because it hurts readability 74 | PointerAlignment: Left 75 | ReflowComments: true 76 | SortIncludes: false 77 | SortUsingDeclarations: false 78 | SpaceAfterCStyleCast: true 79 | SpaceAfterTemplateKeyword: true 80 | SpaceBeforeAssignmentOperators: true 81 | SpaceBeforeCpp11BracedList: false 82 | SpaceBeforeInheritanceColon: true 83 | SpaceBeforeParens: ControlStatements 84 | SpaceBeforeCtorInitializerColon: true 85 | SpaceBeforeRangeBasedForLoopColon: true 86 | SpaceInEmptyParentheses: false 87 | SpacesBeforeTrailingComments: 2 88 | SpacesInAngles: false 89 | SpacesInCStyleCastParentheses: false 90 | SpacesInContainerLiterals: false 91 | SpacesInParentheses: false 92 | SpacesInSquareBrackets: false 93 | Standard: Cpp11 94 | TabWidth: 8 95 | UseTab: Never 96 | ... 97 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | InheritParentConfig: false 2 | Checks: >- 3 | boost-*, 4 | bugprone-*, 5 | cert-*, 6 | clang-analyzer-*, 7 | cppcoreguidelines-*, 8 | google-*, 9 | hicpp-*, 10 | llvm-*, 11 | misc-*, 12 | modernize-*, 13 | performance-*, 14 | portability-*, 15 | readability-*, 16 | -google-readability-todo, 17 | -readability-avoid-const-params-in-decls, 18 | -readability-function-cognitive-complexity, 19 | -llvm-header-guard, 20 | -google-runtime-references, 21 | -misc-non-private-member-variables-in-classes, 22 | -cppcoreguidelines-non-private-member-variables-in-classes, 23 | -cert-msc30-c, 24 | -cert-msc50-cpp, 25 | -readability-identifier-length, 26 | -*-easily-swappable-parameters, 27 | -*-owning-memory, 28 | -*-malloc, 29 | CheckOptions: 30 | - key: readability-magic-numbers.IgnoredIntegerValues 31 | value: '1;2;3;4;5;10;20;60;64;100;128;256;500;512;1000' 32 | WarningsAsErrors: '*' 33 | HeaderFilterRegex: '.*' 34 | AnalyzeTemporaryDtors: false 35 | FormatStyle: file 36 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: Main Workflow 2 | 3 | on: [push, pull_request] 4 | 5 | env: 6 | LLVM_VERSION: 14 7 | YAKUT_COMPILE_OUTPUT: "${{ github.workspace }}" 8 | YAKUT_PATH: "${{ github.workspace }}" 9 | 10 | jobs: 11 | debug: 12 | runs-on: ubuntu-22.04 13 | strategy: 14 | matrix: 15 | toolchain: ['clang', 'gcc'] 16 | include: 17 | - toolchain: gcc 18 | c-compiler: gcc 19 | cxx-compiler: g++ 20 | - toolchain: clang 21 | c-compiler: clang 22 | cxx-compiler: clang++ 23 | steps: 24 | - uses: actions/checkout@v2 25 | - run: | 26 | sudo apt update 27 | sudo apt install gcc-multilib g++-multilib clang-tidy-$LLVM_VERSION ncat 28 | sudo update-alternatives --install /usr/bin/clang-format clang-format /usr/bin/clang-format-$LLVM_VERSION 50 29 | sudo update-alternatives --install /usr/bin/clang-tidy clang-tidy /usr/bin/clang-tidy-$LLVM_VERSION 50 30 | sudo apt install "linux-*-extra-$(uname -r)" 31 | python -m pip --disable-pip-version-check install yakut~=0.7 32 | - run: > 33 | cmake 34 | -B ${{ github.workspace }}/build 35 | -DCMAKE_BUILD_TYPE=Debug 36 | -DCMAKE_C_COMPILER=${{ matrix.c-compiler }} 37 | -DCMAKE_CXX_COMPILER=${{ matrix.cxx-compiler }} 38 | tests 39 | - working-directory: ${{github.workspace}}/build 40 | run: | 41 | yakut compile https://github.com/OpenCyphal/public_regulated_data_types/archive/refs/heads/master.zip 42 | make VERBOSE=1 -j2 43 | make test 44 | - uses: actions/upload-artifact@v2 45 | if: always() 46 | with: 47 | name: ${{github.job}} 48 | path: | 49 | ${{github.workspace}}/**/* 50 | 51 | optimizations: 52 | runs-on: ubuntu-latest 53 | strategy: 54 | matrix: 55 | toolchain: ['clang', 'gcc'] 56 | build_type: [Release, MinSizeRel] 57 | include: 58 | - toolchain: gcc 59 | c-compiler: gcc 60 | cxx-compiler: g++ 61 | - toolchain: clang 62 | c-compiler: clang 63 | cxx-compiler: clang++ 64 | steps: 65 | - uses: actions/checkout@v2 66 | - run: | 67 | sudo apt update 68 | sudo apt install gcc-multilib g++-multilib ncat 69 | sudo apt install "linux-*-extra-$(uname -r)" 70 | python -m pip --disable-pip-version-check install yakut~=0.7 71 | - run: > 72 | cmake 73 | -B ${{ github.workspace }}/build 74 | -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} 75 | -DCMAKE_C_COMPILER=${{ matrix.c-compiler }} 76 | -DCMAKE_CXX_COMPILER=${{ matrix.cxx-compiler }} 77 | -DNO_STATIC_ANALYSIS=1 78 | tests 79 | - working-directory: ${{github.workspace}}/build 80 | run: | 81 | make VERBOSE=1 -j2 82 | yakut compile https://github.com/OpenCyphal/public_regulated_data_types/archive/refs/heads/master.zip 83 | make test 84 | - uses: actions/upload-artifact@v2 85 | if: always() 86 | with: 87 | name: ${{github.job}} 88 | path: | 89 | ${{github.workspace}}/**/* 90 | 91 | tools: 92 | runs-on: ubuntu-latest 93 | env: 94 | python-version: 3.9 95 | steps: 96 | - uses: actions/checkout@v2 97 | - uses: actions/setup-python@v2 98 | with: 99 | python-version: ${{ env.python-version }} 100 | architecture: x64 101 | - run: ./tests/tools/test.sh 102 | 103 | style: 104 | runs-on: ubuntu-latest 105 | steps: 106 | - uses: actions/checkout@v2 107 | - uses: DoozyX/clang-format-lint-action@v0.14 108 | with: 109 | source: ./kocherga ./tests 110 | exclude: ./tests/3rd_party 111 | extensions: cpp,hpp 112 | clangFormatVersion: ${{env.LLVM_VERSION}} 113 | - run: | 114 | pip install black 115 | black --check ./tools/ 116 | 117 | sonarcloud: 118 | runs-on: ubuntu-latest 119 | env: 120 | SONAR_SCANNER_VERSION: 5.0.1.3006 121 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 122 | SONAR_TOKEN: ${{ secrets.SONAR_TOKEN }} 123 | steps: 124 | - uses: actions/checkout@v2 125 | with: 126 | fetch-depth: 0 127 | 128 | - run: | 129 | echo "GITHUB_REF: '$GITHUB_REF'" 130 | echo "GITHUB_HEAD_REF: '$GITHUB_HEAD_REF'" 131 | echo "GITHUB_BASE_REF: '$GITHUB_BASE_REF'" 132 | 133 | - name: Install Dependencies 134 | run: | 135 | sudo apt update 136 | sudo apt install gcc-multilib g++-multilib ncat 137 | sudo apt install "linux-*-extra-$(uname -r)" 138 | python -m pip --disable-pip-version-check install yakut~=0.7 139 | 140 | - name: Set up JDK 141 | uses: actions/setup-java@v3 142 | with: 143 | java-version: 17 144 | distribution: 'zulu' 145 | 146 | - name: Cache SonarCloud packages 147 | uses: actions/cache@v1 148 | with: 149 | path: ~/.sonar/cache 150 | key: ${{ runner.os }}-sonar 151 | restore-keys: ${{ runner.os }}-sonar 152 | 153 | - name: Download and set up sonar-scanner 154 | env: 155 | SONAR_SCANNER_DOWNLOAD_URL: https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-${{ env.SONAR_SCANNER_VERSION }}-linux.zip 156 | run: | 157 | mkdir -p $HOME/.sonar 158 | curl -sSLo $HOME/.sonar/sonar-scanner.zip ${{ env.SONAR_SCANNER_DOWNLOAD_URL }} 159 | unzip -o $HOME/.sonar/sonar-scanner.zip -d $HOME/.sonar/ 160 | echo "$HOME/.sonar/sonar-scanner-${{ env.SONAR_SCANNER_VERSION }}-linux/bin" >> $GITHUB_PATH 161 | 162 | - name: Download and set up build-wrapper 163 | env: 164 | BUILD_WRAPPER_DOWNLOAD_URL: https://sonarcloud.io/static/cpp/build-wrapper-linux-x86.zip 165 | run: | 166 | curl -sSLo $HOME/.sonar/build-wrapper-linux-x86.zip ${{ env.BUILD_WRAPPER_DOWNLOAD_URL }} 167 | unzip -o $HOME/.sonar/build-wrapper-linux-x86.zip -d $HOME/.sonar/ 168 | echo "$HOME/.sonar/build-wrapper-linux-x86" >> $GITHUB_PATH 169 | 170 | - name: Build and test 171 | run: | 172 | cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 -DCMAKE_CXX_FLAGS='-DNDEBUG=1' 173 | build-wrapper-linux-x86-64 --out-dir . make all VERBOSE=1 174 | yakut compile https://github.com/OpenCyphal/public_regulated_data_types/archive/refs/heads/master.zip 175 | make test 176 | gcov --preserve-paths --long-file-names $(find integration/CMakeFiles/bootloader.dir -name '*.gcno') 177 | gcov --preserve-paths --long-file-names $(find unit/CMakeFiles/test_cov.dir -name '*.gcno') 178 | 179 | # https://community.sonarsource.com/t/analyzing-a-header-only-c-library/51468 180 | - name: Run sonar-scanner 181 | if: env.SONAR_TOKEN != '' 182 | run: > 183 | sonar-scanner 184 | --define sonar.host.url="https://sonarcloud.io" 185 | --define sonar.projectName=kocherga 186 | --define sonar.organization=zubax 187 | --define sonar.projectKey=Zubax_kocherga 188 | --define sonar.sources=kocherga,tests/unit,tests/integration 189 | --define sonar.issue.ignore.allfile=a1,a2 190 | --define sonar.issue.ignore.allfile.a1.fileRegexp='^#include.*catch\.hpp[>"]$' 191 | --define sonar.issue.ignore.allfile.a2.fileRegexp='^auto main\([^)]*\) -> int$' 192 | --define sonar.coverage.exclusions="tests/**/*" 193 | --define sonar.cpd.exclusions="tests/**/*" 194 | --define sonar.cfamily.gcov.reportsPath=. 195 | --define sonar.cfamily.cache.enabled=false 196 | --define sonar.cfamily.threads=2 197 | --define sonar.cfamily.build-wrapper-output=. 198 | $([ -z "$GITHUB_BASE_REF" ] && echo "--define sonar.branch.name=${GITHUB_REF##*/}" || true) 199 | 200 | - uses: actions/upload-artifact@v2 201 | if: always() 202 | with: 203 | name: ${{github.job}} 204 | path: ${{github.workspace}}/**/* 205 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build outputs 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | *.so 7 | *.lai 8 | *.la 9 | *.a 10 | *.elf 11 | *.hex 12 | lib*.so 13 | lib*.so.* 14 | *.d 15 | build 16 | *.cout 17 | *.pbd 18 | *.pbi 19 | *.browse 20 | cmake-build-* 21 | 22 | # Temp files 23 | *.swp 24 | *~ 25 | *.bak 26 | *.tmp 27 | *.log 28 | 29 | # IDE 30 | .metadata 31 | .settings 32 | .project 33 | .cproject 34 | .pydevproject 35 | *.kdev4 36 | **/.idea/* 37 | !**/.idea/dictionaries 38 | !**/.idea/dictionaries/* 39 | -------------------------------------------------------------------------------- /.idea/dictionaries/pavel.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | aaaaaaaa 5 | abcdef 6 | automati 7 | badc 8 | bootloaders 9 | brickproof 10 | castagnoli 11 | cccccccc 12 | cfamily 13 | deadbeef 14 | deja 15 | devirtualization 16 | dsdl 17 | dsonar 18 | encobs 19 | ffee 20 | ffff 21 | gcov 22 | kirienko 23 | kocherga 24 | kochergá 25 | mmmmmmmm 26 | multiframe 27 | nosonar 28 | patchee 29 | prog 30 | pyuavcan 31 | sidrane 32 | telega 33 | tttt 34 | uavcan 35 | unicast 36 | usec 37 | vcan 38 | vssc 39 | vvvvvvvv 40 | zubax 41 | 42 | 43 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Kocherga contribution guide 2 | 3 | ## Directory layout 4 | 5 | The production sources are located under `/kocherga/`. 6 | Do not put anything else in there. 7 | 8 | The tests are located under `/tests/`. 9 | This directory also contains the top `CMakeLists.txt` needed to build and run the tests on the local machine. 10 | 11 | The high-level documentation is written in the main README, API documentation is given directly in the header files. 12 | 13 | ## Standards 14 | 15 | The library shall be implemented in ISO C++17 with partial adherence to MISRA C++. 16 | The MISRA compliance is enforced by Clang-Tidy and SonarQube. 17 | Deviations are documented directly in the source code as follows: 18 | 19 | ```c 20 | // Intentional violation of MISRA: 21 | <... deviant construct ...> // NOLINT NOSONAR 22 | ``` 23 | 24 | The full list of deviations with the accompanying explanation can be found by grepping the sources. 25 | 26 | Do not suppress compliance warnings using the means provided by static analysis tools because such deviations 27 | are impossible to track at the source code level. 28 | An exception applies for the case of false-positive (invalid) warnings -- those should not be mentioned in the codebase. 29 | 30 | [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah) shall be followed. 31 | Formatting is enforced by Clang-Format; it is used also to fail the CI/CD build if violations are detected. 32 | 33 | Unfortunately, some rules are hard or impractical to enforce automatically, 34 | so code reviewers shall be aware of MISRA and general high-reliability coding practices 35 | to prevent non-compliant code from being accepted into upstream. 36 | 37 | ## Tools 38 | 39 | For the full list of the tools please refer to the CI scripts. 40 | 41 | ### Clang-Tidy 42 | 43 | Clang-Tidy is used to enforce compliance with MISRA and Zubax Coding Conventions. 44 | 45 | Clang-Tidy is invoked automatically on each translation unit before it is compiled; 46 | the build will fail if the tool is not available locally. 47 | To disable this behavior, pass `NO_STATIC_ANALYSIS=1` to CMake at the generation time. 48 | 49 | ### Clang-Format 50 | 51 | Clang-Format is used to enforce compliance with MISRA and Zubax Coding Conventions. 52 | 53 | To reformat the sources, generate the project and build the target `format`; e.g., for Make: `make format`. 54 | 55 | ### SonarQube 56 | 57 | SonarQube is a cloud solution so its use is delegated to the CI/CD pipeline. 58 | If you need access, please get in touch with the maintainers. 59 | 60 | If you have access, you can run the `sonar-scanner` agent locally for testing and debugging purposes; 61 | please refer to the CI configs for details. 62 | You may need to manually `--define sonar.branch.name` as explained in 63 | . 64 | 65 | ### IDE 66 | 67 | The recommended development environment is JetBrains CLion. The root project file can be found under `tests/`. 68 | The repository contains the spelling dictionaries for CLion located under `.idea/`, be sure to use them. 69 | 70 | ## Testing 71 | 72 | Generate the CMake project, build all, and then build the target `test` (e.g., `make test`). 73 | 74 | Some of the tests are intended to be run manually due to lack of adequate automation solutions in the v0 ecosystem. 75 | Please navigate to `/tests/integration/validator/` for details. 76 | 77 | ## Releasing 78 | 79 | 1. Bump the version numbers (`KOCHERGA_VERSION_MAJOR`, `KOCHERGA_VERSION_MINOR`) in `kocherga.hpp`. Push the change. 80 | 2. Create a new release on GitHub: 81 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Zubax Robotics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kochergá 2 | 3 | [![CI](https://github.com/Zubax/kocherga/actions/workflows/main.yml/badge.svg)](https://github.com/Zubax/kocherga/actions/workflows/main.yml) 4 | [![Forum](https://img.shields.io/discourse/https/forum.zubax.com/users.svg?color=e00000)](https://forum.zubax.com) 5 | [![Forum](https://img.shields.io/discourse/https/forum.opencyphal.org/users.svg?color=1700b3)](https://forum.opencyphal.org) 6 | 7 | **Kochergá is a robust platform-agnostic [Cyphal](https://opencyphal.org) bootloader for deeply embedded systems.** 8 | 9 | Technical support is provided on the [OpenCyphal Forum](https://forum.opencyphal.org/). 10 | 11 | A standard-compliant implementation of the software update server is provided in 12 | [Yakut](https://github.com/OpenCyphal/yakut#updating-node-software). 13 | 14 | ## Features 15 | 16 | - **Portability** -- Kochergá is written in standard C++17 and is distributed as a header-only library with no external 17 | dependencies. 18 | 19 | - **Robustness** -- Kochergá is brick-proof. The application (i.e., firmware) update process can be interrupted at any 20 | point (e.g., by turning off the power supply or by disconnecting the interface), and it is guaranteed that the device 21 | will always end up in a known valid state. If a dysfunctional application image is uploaded, Kochergá can regain 22 | control after a watchdog reset. 23 | 24 | - **Safety** -- Kochergá verifies the correctness of the application image with a 64-bit hash before every boot. 25 | Kochergá's own codebase features extensive test coverage. 26 | 27 | - **Multiple supported transports:** 28 | - **Cyphal/CAN** + **DroneCAN** -- the protocol version is auto-detected at runtime. 29 | - **Cyphal/serial** 30 | - More may appear in the future -- new transports are easy to add. 31 | 32 | ## Usage 33 | 34 | ### Integration 35 | 36 | The entire library is contained in the header file `kocherga.hpp`; protocol implementations are provided each in a 37 | separate header file named `kocherga_*.hpp`. Kochergá does not have any compilation units of its own. 38 | 39 | To integrate Kochergá into your application, just include this repository as a git subtree/submodule, or simply 40 | copy-paste the required header files into your source tree. 41 | 42 | For reference, a typical implementation on an ARM Cortex M4 MCU supporting 43 | Cyphal/serial (USB+UART), Cyphal/CAN, and DroneCAN (autodetection) would set you back by about ~32K of flash. 44 | 45 | ### Application signature 46 | 47 | The bootloader looks for an instance of the `AppInfo` structure located in the ROM image of the application at every 48 | boot. Only if a valid `AppInfo` structure is found the application will be launched. It is recommended to allocate the 49 | structure closer to the beginning of the image in order to speed up its verification. The structure is defined as 50 | follows: 51 | 52 | | Offset | Type | Description | 53 | |--------|------------|--------------------------------------------------------------------------------------------------| 54 | | -16 | `uint64` | Constant value 0x5E4415146FC0C4C7 used for locating the descriptor and detecting the byte order. | 55 | | -8 | `uint8[8]` | Set to `APDesc00`; used for compatibility with legacy deployments. | 56 | | 0 | `uint64` | CRC-64-WE of the entire application image when this field itself is set to zero. | 57 | | 8 | `uint32` | Size of the application image, in bytes. Note that the image must be padded to eight bytes. | 58 | | 12 | `void32` | Reserved. Used to contain the 32-bit version control system revision ID; see replacement below. | 59 | | 16 | `uint8[2]` | Major and minor semantic version numbers. | 60 | | 18 | `uint8` | Flags: 1 - this is a release build; 2 - this is a dirty build (uncommitted changes present). | 61 | | 19 | `void8` | Reserved; set to 0. | 62 | | 20 | `uint32` | UNIX UTC build timestamp; i.e., the number of seconds since 1970-01-01T00:00:00Z. | 63 | | 24 | `uint64` | Version control system (VCS) revision ID (e.g., the git commit hash). | 64 | | 32 | `void64` | Reserved. | 65 | | 40 | `void64` | Reserved. | 66 | 67 | When computing the application image CRC, the process will eventually encounter the location where the CRC itself is 68 | stored. In order to avoid recursive dependency, the CRC storage location must be replaced with zero bytes when 69 | computing/verifying the CRC. The parameters of the CRC-64 algorithm are the following: 70 | 71 | * Initial value: 0xFFFF'FFFF'FFFF'FFFF 72 | * Polynomial: 0x42F0'E1EB'A9EA'3693 73 | * Reverse: no 74 | * Output xor: 0xFFFF'FFFF'FFFF'FFFF 75 | * Check: 0x62EC'59E3'F1A4'F00A 76 | 77 | The CRC and size fields cannot be populated until after the application binary is compiled and linked. One possible way 78 | to populate these fields is to initialize them with zeroes in the source code, and then use the 79 | script `tools/kocherga_image.py` after the binary is generated to update the fields with their actual values. The script 80 | can be invoked from the build system (e.g., from a Makefile rule) trivially as follows: 81 | 82 | ```sh 83 | kocherga_image.py application-name-goes-here.bin 84 | ``` 85 | 86 | The output will be stored in a file whose name follows the pattern expected by the firmware update server implemented in 87 | the [Yakut CLI tool](https://github.com/OpenCyphal/yakut#updating-node-software). 88 | 89 | ### State machine 90 | 91 | The following diagram documents the state machine of the bootloader: 92 | 93 | ![Kochergá State Machine Diagram](docs/state_machine.svg "Kochergá State Machine Diagram") 94 | 95 | The bootloader states are mapped onto Cyphal node states as follows: 96 | 97 | | Bootloader state | Node mode | Node health | Vendor-specific status code | 98 | |---------------------|-------------------|-------------|------------------------------------| 99 | | NoAppToBoot | `SOFTWARE_UPDATE` | `WARNING` | 0 | 100 | | BootDelay | `SOFTWARE_UPDATE` | `NOMINAL` | 0 | 101 | | BootCancelled | `SOFTWARE_UPDATE` | `ADVISORY` | 0 | 102 | | AppUpdateInProgress | `SOFTWARE_UPDATE` | `NOMINAL` | number of read requests, always >0 | 103 | 104 | ### API usage 105 | 106 | The following snippet demonstrates how to integrate Kochergá into your bootloader executable. 107 | User-provided functions are shown in `SCREAMING_SNAKE_CASE()`. 108 | This is a stripped-down example; the full API documentation is available in the header files. 109 | 110 | The integration test application available under `/tests/integration/bootloader/` may also be a good reference. 111 | 112 | #### Configuring Kochergá 113 | 114 | ##### Random number generation 115 | 116 | Kochergá needs a source of random numbers regardless of the transport used. 117 | You need to provide a definition of `kocherga::getRandomByte() -> std::uint8_t` for the library to build successfully. 118 | You can use this implementation based on `std::rand()`: 119 | 120 | ```c++ 121 | #include 122 | 123 | auto kocherga::getRandomByte() -> std::uint8_t 124 | { 125 | const auto product = 126 | static_cast(std::rand()) * static_cast(std::numeric_limits::max()); 127 | return static_cast(product / RAND_MAX); 128 | } 129 | 130 | int main() 131 | { 132 | std::srand(GET_ENTROPY()); 133 | // bootloader implementation below 134 | return 0; 135 | } 136 | ``` 137 | 138 | An alternative is to use a generator from C++ standard library: 139 | 140 | ```c++ 141 | #include 142 | 143 | auto kocherga::getRandomByte() -> std::uint8_t 144 | { 145 | static std::mt19937 rd{GET_ENTROPY()}; 146 | return static_cast(rd() * std::numeric_limits::max() / std::mt19937::max()); 147 | } 148 | ``` 149 | 150 | In both cases beware that you need to initialize the psudorandom sequence with `GET_ENTROPY()`. 151 | This function should retrieve a sufficiently random or unique value (such as the number of seconds since epoch). 152 | Look for more information in the respective documentation of both `std::srand` and `std::mt19937`. 153 | 154 | ##### Providing custom assert macros 155 | 156 | Kochergá uses the `assert` macro from the stadard C library to check its invariants. 157 | If this is undesireable in your project, you can redefine the following macros. 158 | You can do this before including Kochergá or globally in your build system. 159 | 160 | ```c++ 161 | #define KOCHERGA_ASSERT(x) some_other_assert(x, ...); 162 | #include 163 | ``` 164 | 165 | You can disable all internal assertions like this: 166 | 167 | ```c++ 168 | #define KOCHERGA_ASSERT(x) (void)(x); 169 | #include 170 | ``` 171 | 172 | ##### Compatibility with environments with missing operator delete 173 | 174 | Kocherga does not require heap but some toolchains may refuse to link the code if operator delete is not available. 175 | If your environment does not define `operator delete`, you can provide a custom definition in your code like this: 176 | 177 | ```c++ 178 | void operator delete(void*) noexcept { std::abort(); } 179 | ``` 180 | 181 | This is needed as Kochergá uses virtual destructors, code generation for which includes 182 | an `operator delete` even if deleting an object through pointer to its base class is 183 | not used in your entire application. 184 | 185 | #### ROM interface 186 | 187 | The ROM backend abstracts the specifics of reading and writing your ROM (usually this is the on-chip flash memory). 188 | Be sure to avoid overwriting the bootloader while modifying the ROM. 189 | 190 | ```c++ 191 | class MyROMBackend final : public kocherga::IROMBackend 192 | { 193 | auto write(const std::size_t offset, const std::byte* const data, const std::size_t size) override 194 | -> std::optional 195 | { 196 | if (WRITE_ROM(offset, data, size)) 197 | { 198 | return size; 199 | } 200 | return {}; // Failure case 201 | } 202 | 203 | auto read(const std::size_t offset, std::byte* const out_data, const std::size_t size) const override 204 | -> std::size_t 205 | { 206 | return READ_ROM(offset, out_data, size); // Return the number of bytes read (may be less than size). 207 | } 208 | }; 209 | ``` 210 | 211 | #### Media layer interfaces 212 | 213 | Transport implementations --- Cyphal/CAN, Cyphal/serial, etc., depending on which transports you need --- 214 | are interfaced with your hardware as follows. 215 | 216 | ```c++ 217 | class MySerialPort final : public kocherga::serial::ISerialPort 218 | { 219 | auto receive() -> std::optional override 220 | { 221 | if (SERIAL_RX_PENDING()) 222 | { 223 | return SERIAL_READ_BYTE(); 224 | } 225 | return {}; 226 | } 227 | 228 | auto send(const std::uint8_t b) -> bool override { return SERIAL_WRITE_BYTE(b); } 229 | }; 230 | ``` 231 | 232 | ```c++ 233 | class MyCANDriver final : public kocherga::can::ICANDriver 234 | { 235 | auto configure(const Bitrate& bitrate, 236 | const bool silent, 237 | const kocherga::can::CANAcceptanceFilterConfig& filter) -> std::optional override 238 | { 239 | tx_queue_.clear(); 240 | CAN_CONFIGURE(bitrate, silent, filter); 241 | return Mode::FD; // Or Mode::Classic if CAN FD is not supported by the CAN controller. 242 | } 243 | 244 | auto push(const bool force_classic_can, 245 | const std::uint32_t extended_can_id, 246 | const std::uint8_t payload_size, 247 | const void* const payload) -> bool override 248 | { 249 | const std::chrono::microseconds now = GET_TIME_SINCE_BOOT(); 250 | // You can use tx_queue_.size() to limit maximum depth of the queue. 251 | const bool ok = tx_queue_.push(now, force_classic_can, extended_can_id, payload_size, payload); 252 | pollTxQueue(now); 253 | return ok; 254 | } 255 | 256 | auto pop(PayloadBuffer& payload_buffer) -> std::optional> override 257 | { 258 | pollTxQueue(); // Check if the HW TX mailboxes are ready to accept the next frame from the SW queue. 259 | return CAN_POP(payload_buffer.data()); // The return value is optional(can_id, payload_size). 260 | } 261 | 262 | void pollTxQueue(const std::chrono::microseconds now) 263 | { 264 | if (const auto* const item = tx_queue_.peek()) // Take the top frame from the prioritized queue. 265 | { 266 | const bool expired = now > (item->timestamp + kocherga::can::SendTimeout); // Drop expired frames. 267 | if (expired || CAN_PUSH(item->force_classic_can, // force_classic_can means no DLE, no BRS. 268 | item->extended_can_id, 269 | item->payload_size, 270 | item->payload)) 271 | { 272 | tx_queue_.pop(); // Enqueued into the HW TX mailbox or expired -- remove from the SW queue. 273 | } 274 | } 275 | } 276 | 277 | // Some CAN drivers come with built-in queue (e.g., SocketCAN), in which case this will not be needed. 278 | // The recommended heap is https://github.com/pavel-kirienko/o1heap. 279 | kocherga::can::TxQueue tx_queue_(&MY_MALLOC, &MY_FREE); 280 | }; 281 | ``` 282 | 283 | #### Passing arguments from the application 284 | 285 | When the application is commanded to upgrade itself, it needs to store relevant context into a struct, 286 | write this struct into a pre-defined memory location, and then reboot. 287 | The bootloader would check that location to see if there is a valid argument struct in it. 288 | Kochergá provides a convenient class for that --- `kocherga::VolatileStorage<>` --- 289 | which checks the presence and validity of the arguments with a strong 64-bit CRC. 290 | 291 | ```c++ 292 | /// The application may pass this structure when rebooting into the bootloader. 293 | /// Feel free to modify the contents to suit your system. 294 | /// It is a good idea to include an explicit version field here for future-proofing. 295 | struct ArgumentsFromApplication 296 | { 297 | std::uint16_t cyphal_serial_node_id; ///< Invalid if unknown. 298 | 299 | std::pair cyphal_can_bitrate; ///< Zeros if unknown. 300 | std::uint8_t cyphal_can_not_dronecan; ///< 0xFF-unknown; 0-DroneCAN; 1-Cyphal/CAN. 301 | std::uint8_t cyphal_can_node_id; ///< Invalid if unknown. 302 | 303 | std::uint8_t trigger_node_index; ///< 0 - serial, 1 - CAN, >1 - none. 304 | std::uint16_t file_server_node_id; ///< Invalid if unknown. 305 | std::array remote_file_path; ///< Null-terminated string. 306 | }; 307 | static_assert(std::is_trivial_v); 308 | ``` 309 | 310 | #### Running the bootloader 311 | 312 | ```c++ 313 | #include // Pick the transports you need. 314 | #include // In this example we are using Cyphal/serial + Cyphal/CAN. 315 | 316 | int main() 317 | { 318 | // Check if the application has passed any arguments to the bootloader via shared RAM. 319 | // The address where the arguments are stored obviously has to be shared with the application. 320 | // If the application uses heap, then it might be a good idea to alias this area with the heap. 321 | std::optional args = 322 | kocherga::VolatileStorage(reinterpret_cast(0x2000'4000U)).take(); 323 | 324 | // Initialize the bootloader core. 325 | MyROMBackend rom_backend; 326 | kocherga::SystemInfo system_info = GET_SYSTEM_INFO(); 327 | kocherga::Bootloader::Params params{.linger = args.has_value()}; // Read the docs on the available params. 328 | kocherga::Bootloader boot(rom_backend, system_info, params); 329 | // It's a good idea to check if the app is valid and safe to boot before adding the nodes. 330 | // This way you can skip the potentially slow or disturbing interface initialization on the happy path. 331 | // You can do it by calling poll() here once. 332 | 333 | // Add a Cyphal/serial node to the bootloader instance. 334 | MySerialPort serial_port; 335 | kocherga::serial::SerialNode serial_node(serial_port, system_info.unique_id); 336 | if (args && (args->cyphal_serial_node_id <= kocherga::serial::MaxNodeID)) 337 | { 338 | serial_node.setLocalNodeID(args->cyphal_serial_node_id); 339 | } 340 | boot.addNode(&serial_node); 341 | 342 | // Add a Cyphal/CAN node to the bootloader instance. 343 | std::optional can_bitrate; 344 | std::optional cyphal_can_not_dronecan; 345 | std::optional cyphal_can_node_id; 346 | if (args) 347 | { 348 | if (args->cyphal_can_bitrate.first > 0) 349 | { 350 | can_bitrate = ICANDriver::Bitrate{args.cyphal_can_bitrate.first, args.cyphal_can_bitrate.second}; 351 | } 352 | cyphal_can_not_dronecan = args->cyphal_can_not_dronecan;// Will be ignored if invalid. 353 | cyphal_can_node_id = args->cyphal_can_node_id; // Will be ignored if invalid. 354 | } 355 | MyCANDriver can_driver; 356 | kocherga::can::CANNode can_node(can_driver, 357 | system_info.unique_id, 358 | can_bitrate, 359 | cyphal_can_not_dronecan, 360 | cyphal_can_node_id); 361 | boot.addNode(&can_node); 362 | 363 | while (true) 364 | { 365 | const auto uptime = GET_TIME_SINCE_BOOT(); 366 | if (const auto fin = boot.poll(std::chrono::duration_cast(uptime))) 367 | { 368 | if (*fin == kocherga::Final::BootApp) 369 | { 370 | BOOT_THE_APPLICATION(); 371 | } 372 | if (*fin == kocherga::Final::Restart) 373 | { 374 | RESTART_THE_BOOTLOADER(); 375 | } 376 | assert(false); 377 | } 378 | // Trigger the update process internally if the required arguments are provided by the application. 379 | // The trigger method cannot be called before the first poll(). 380 | if (args && (args->trigger_node_index < 2)) 381 | { 382 | (void) boot.trigger(args->trigger_node_index, // Use serial or CAN? 383 | args->file_server_node_id, // Which node to download the file from? 384 | std::strlen(args->remote_file_path.data()), // Remote file path length. 385 | args->remote_file_path.data()); 386 | args.reset(); 387 | } 388 | // Sleep until the next hardware event (like reception of CAN frame or UART byte) but no longer than 389 | // 1 second. A fixed sleep is also acceptable but the resulting polling interval should be adequate 390 | // to avoid data loss (about 100 microseconds is usually ok). 391 | WAIT_FOR_EVENT(); 392 | } 393 | } 394 | ``` 395 | 396 | #### Building a compliant application image 397 | 398 | Define the following application signature structure somewhere in your application: 399 | 400 | ```c++ 401 | struct AppDescriptor 402 | { 403 | std::uint64_t magic = 0x5E44'1514'6FC0'C4C7ULL; 404 | std::array signature{{'A', 'P', 'D', 'e', 's', 'c', '0', '0'}}; 405 | 406 | std::uint64_t image_crc = 0; // Populated after build 407 | std::uint32_t image_size = 0; // Populated after build 408 | [[maybe_unused]] std::array _reserved_a{}; 409 | std::uint8_t version_major = SOFTWARE_VERSION_MAJOR; 410 | std::uint8_t version_minor = SOFTWARE_VERSION_MINOR; 411 | std::uint8_t flags = 412 | #if RELEASE_BUILD 413 | Flags::ReleaseBuild 414 | #else 415 | 0 416 | #endif 417 | ; 418 | [[maybe_unused]] std::array _reserved_b{}; 419 | std::uint32_t build_timestamp_utc = TIMESTAMP_UTC; 420 | std::uint64_t vcs_revision_id = GIT_HASH; 421 | [[maybe_unused]] std::array _reserved_c{}; 422 | 423 | struct Flags 424 | { 425 | static constexpr std::uint8_t ReleaseBuild = 1U; 426 | static constexpr std::uint8_t DirtyBuild = 2U; 427 | }; 428 | }; 429 | 430 | static const volatile AppDescriptor g_app_descriptor; 431 | // Optionally, use explicit placement near the beginning of the binary: 432 | // __attribute__((used, section(".app_descriptor"))); 433 | // and define the .app_descriptor section in the linker file. 434 | ``` 435 | 436 | Then modify your build script to invoke `kocherga_image.py` as explained earlier. 437 | 438 | #### Rebooting into the bootloader from the application 439 | 440 | If the application needs to pass arguments to the bootloader, it can be done with the help of 441 | `kocherga::VolatileStorage<>`. 442 | Ensure that the definition of `ArgumentsFromApplication` used by the application and by the bootloader use 443 | the same binary layout. 444 | 445 | Note that the application does not need to depend on the Kochergá library. 446 | It is recommended to copy-paste relevant pieces from Kochergá instead; specifically: 447 | 448 | - `kocherga::VolatileStorage<>` 449 | - `kocherga::CRC64` 450 | 451 | ## Revisions 452 | 453 | ### v2.0 454 | 455 | - Provide dedicated parameter struct to minimize usage errors. 456 | - Retry timed out requests up to a configurable number of times (https://github.com/Zubax/kocherga/issues/17). 457 | 458 | ### v1.0 459 | 460 | The first stable revision is virtually identical to v0.2. 461 | 462 | ### v0.2 463 | 464 | - Add helper `kocherga::can::TxQueue`. 465 | - Promote `kocherga::CRC64` to public API. 466 | - Add optional support for legacy app descriptors to simplify integration into existing projects. 467 | - Minor doc and API enhancements. 468 | 469 | ### v0.1 470 | 471 | The first revision to go public. 472 | -------------------------------------------------------------------------------- /docs/state_machine.dia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/docs/state_machine.dia -------------------------------------------------------------------------------- /docs/state_machine.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | NoAppToBoot 9 | 10 | 11 | 12 | 13 | 14 | 15 | BootDelay 16 | 17 | 18 | 19 | 20 | 21 | 22 | BootCanceled 23 | 24 | 25 | 26 | 27 | 28 | 29 | AppUpdateInProgress 30 | 31 | 32 | 33 | 34 | 35 | 36 | Bootloader started 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | Boot cancelled 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | Boot the 57 | application 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | Valid application found 72 | 73 | 74 | No valid application found 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | Update successful, received image is valid 98 | 99 | 100 | Boot delay expired 101 | (zero by default) 102 | 103 | 104 | Update requested 105 | 106 | 107 | Update failed, 108 | no valid image 109 | is now 110 | available 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | Update failed, 119 | but the existing valid image was not altered 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /kocherga/kocherga_serial.hpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #pragma once 6 | 7 | #include "kocherga.hpp" 8 | #include 9 | #include 10 | 11 | namespace kocherga::serial 12 | { 13 | namespace detail 14 | { 15 | using kocherga::detail::BitsPerByte; // NOSONAR 16 | using kocherga::detail::CRC16CCITT; // NOSONAR 17 | using kocherga::detail::CRC32C; // NOSONAR 18 | 19 | constexpr std::uint8_t FrameDelimiter = 0x00; ///< Zeros cannot occur inside frames thanks to COBS encoding. 20 | 21 | /// Reference values to check the header against. 22 | static constexpr std::uint8_t FrameFormatVersion = 1; 23 | static constexpr std::array FrameIndexEOTReference{0, 0, 0, 0x80}; 24 | static constexpr std::array UserData{0, 0}; 25 | 26 | /// New instance shall be created per encoded frame. 27 | /// ByteWriter is of type (std::uint8_t) -> bool, returns true on success. 28 | /// This is an original implementation of the algorithm. 29 | template 30 | class COBSEncoder 31 | { 32 | public: 33 | explicit COBSEncoder(ByteWriter byte_writer) : byte_writer_(byte_writer) {} 34 | 35 | /// Invoke this function once per byte to transmit COBS-encoded bytestream. 36 | /// The leading frame delimiter will be added automatically. 37 | /// The instance shall be discarded immediately if this method returns false. 38 | [[nodiscard]] auto push(const std::uint8_t b) -> bool 39 | { 40 | if (byte_count_ == 0) 41 | { 42 | if (!output(FrameDelimiter)) // NOSONAR merging this if with the outer one impedes comprehension. 43 | { 44 | return false; 45 | } 46 | } 47 | byte_count_++; 48 | if (b != FrameDelimiter) 49 | { 50 | lookahead_.push(b); 51 | } 52 | if ((b == FrameDelimiter) || (lookahead_.size() >= (std::numeric_limits::max() - 1))) 53 | { 54 | return flush(); 55 | } 56 | return true; 57 | } 58 | 59 | /// This function shall be invoked at the end once. 60 | /// The trailing frame delimiter will be added automatically. 61 | /// The instance shall be discarded immediately if this method returns false. 62 | [[nodiscard]] auto end() -> bool { return flush() && output(FrameDelimiter); } 63 | 64 | private: 65 | [[nodiscard]] auto output(const std::uint8_t b) const -> bool { return byte_writer_(b); } 66 | 67 | [[nodiscard]] auto flush() -> bool 68 | { 69 | const auto sz = lookahead_.size(); 70 | KOCHERGA_ASSERT(sz < std::numeric_limits::max()); 71 | if (!output(static_cast(sz + 1U))) 72 | { 73 | return false; 74 | } 75 | while (auto b = lookahead_.pop()) 76 | { 77 | if (!output(*b)) 78 | { 79 | return false; 80 | } 81 | } 82 | KOCHERGA_ASSERT(lookahead_.size() == 0); 83 | return true; 84 | } 85 | 86 | class LookaheadFIFO final // NOLINT 87 | { 88 | public: 89 | /// Check size() before calling, otherwise the write pointer will wrap around to the beginning of the buffer. 90 | [[nodiscard]] auto push(const std::uint8_t val) 91 | { 92 | buf_.at(in_) = val; 93 | in_++; 94 | } 95 | 96 | /// Returns empty if empty, meaning that the current state is PUSHING. 97 | [[nodiscard]] auto pop() -> std::optional 98 | { 99 | if (out_ < in_) 100 | { 101 | const auto pos = out_; 102 | out_++; 103 | return buf_.at(pos); 104 | } 105 | out_ = 0; 106 | in_ = 0; 107 | return {}; 108 | } 109 | 110 | /// The number of bytes that currently reside in the queue. 111 | [[nodiscard]] auto size() const noexcept { return in_; } 112 | 113 | private: 114 | std::array buf_; 115 | std::uint8_t in_ = 0; 116 | std::uint8_t out_ = 0; 117 | }; 118 | 119 | LookaheadFIFO lookahead_; 120 | std::size_t byte_count_ = 0; 121 | ByteWriter byte_writer_; 122 | }; 123 | 124 | /// Constant-complexity COBS stream decoder extracts useful payload from a COBS-encoded stream of bytes in real time. 125 | /// It does not perform any error checking; the outer logic is responsible for that (e.g., using CRC). 126 | /// The implementation is derived from "Consistent Overhead Byte Stuffing" by Stuart Cheshire and Mary Baker, 1999. 127 | class COBSDecoder 128 | { 129 | public: 130 | struct Delimiter ///< Indicates that the previous packet is finished and the next one is started. 131 | {}; 132 | struct Nothing ///< Indicates that there is no output byte to provide in this update cycle. 133 | {}; 134 | using Result = std::variant; 135 | 136 | [[nodiscard]] auto feed(const std::uint8_t bt) -> Result 137 | { 138 | if (bt == 0) 139 | { 140 | code_ = Top; 141 | copy_ = 0; 142 | return Delimiter{}; 143 | } 144 | const auto old_copy = copy_; 145 | copy_--; 146 | if (old_copy != 0) 147 | { 148 | return bt; 149 | } 150 | const auto old_code = code_; 151 | KOCHERGA_ASSERT(bt >= 1); 152 | copy_ = static_cast(bt - 1); 153 | code_ = bt; 154 | if (old_code != Top) 155 | { 156 | return std::uint8_t{0}; 157 | } 158 | return Nothing{}; 159 | } 160 | 161 | /// Alias for feed(0). 162 | void reset() { (void) feed(0); } 163 | 164 | private: 165 | static constexpr std::uint8_t Top = std::numeric_limits::max(); 166 | std::uint8_t code_ = Top; 167 | std::uint8_t copy_ = 0; 168 | }; 169 | 170 | struct Transfer 171 | { 172 | struct Metadata 173 | { 174 | static constexpr std::uint8_t DefaultPriority = 6U; // Second to lowest. 175 | static constexpr NodeID AnonymousNodeID = 0xFFFFU; 176 | static constexpr PortID DataSpecServiceFlag = 0x8000U; 177 | static constexpr PortID DataSpecRequestFlag = 0x4000U; 178 | static constexpr auto DataSpecServiceIDMask = 179 | static_cast(~static_cast(DataSpecServiceFlag | DataSpecRequestFlag)); 180 | 181 | std::uint8_t priority = DefaultPriority; 182 | NodeID source = AnonymousNodeID; 183 | NodeID destination = AnonymousNodeID; 184 | std::uint16_t data_spec = std::numeric_limits::max(); 185 | TransferID transfer_id = std::numeric_limits::max(); 186 | 187 | [[nodiscard]] auto isRequest() const noexcept -> std::optional 188 | { 189 | if (((data_spec & DataSpecServiceFlag) != 0) && ((data_spec & DataSpecRequestFlag) != 0)) 190 | { 191 | return data_spec & DataSpecServiceIDMask; 192 | } 193 | return {}; 194 | } 195 | 196 | [[nodiscard]] auto isResponse() const noexcept -> std::optional 197 | { 198 | if (((data_spec & DataSpecServiceFlag) != 0) && ((data_spec & DataSpecRequestFlag) == 0)) 199 | { 200 | return data_spec & DataSpecServiceIDMask; 201 | } 202 | return {}; 203 | } 204 | }; 205 | Metadata meta{}; 206 | std::size_t payload_len = 0; 207 | const std::uint8_t* payload = nullptr; 208 | }; 209 | 210 | /// Cyphal/serial stream parser. Extracts Cyphal/serial frames from raw stream of bytes in constant time. 211 | /// Frames that contain more than MaxPayloadSize bytes of payload are rejected as invalid. 212 | template 213 | class StreamParser 214 | { 215 | public: 216 | /// If the byte completed a transfer, it will be returned. 217 | /// The returned object contains a pointer to the payload buffer memory allocated inside this instance. 218 | /// The buffer is invalidated on the 32-nd call to update() after reception. 219 | [[nodiscard]] auto update(const std::uint8_t stream_byte) -> std::optional 220 | { 221 | std::optional out; 222 | const auto dec = decoder_.feed(stream_byte); 223 | if (std::holds_alternative(dec)) 224 | { 225 | if (inside_ && (offset_ >= TotalOverheadSize) && transfer_crc_.isResidueCorrect() && isMetaValid()) 226 | { 227 | out = Transfer{ 228 | meta_, 229 | offset_ - TotalOverheadSize, 230 | buf_.data(), 231 | }; 232 | } 233 | reset(); 234 | inside_ = true; 235 | } 236 | else if (const std::uint8_t* const decoded_byte = std::get_if(&dec)) 237 | { 238 | if (offset_ < HeaderSize) 239 | { 240 | header_crc_.update(*decoded_byte); 241 | acceptHeader(*decoded_byte); 242 | } 243 | else 244 | { 245 | transfer_crc_.update(*decoded_byte); 246 | const auto buf_offset = offset_ - HeaderSize; 247 | if (buf_offset < buf_.size()) 248 | { 249 | buf_.at(buf_offset) = *decoded_byte; 250 | } 251 | else 252 | { 253 | reset(); 254 | } 255 | } 256 | ++offset_; 257 | } 258 | else 259 | { 260 | KOCHERGA_ASSERT(std::holds_alternative(dec)); // Stuff byte, skip silently. 261 | } 262 | return out; 263 | } 264 | 265 | /// Reset the decoder state machine, drop the current incomplete frame if any. 266 | void reset() noexcept 267 | { 268 | decoder_.reset(); 269 | offset_ = 0; 270 | inside_ = false; 271 | header_crc_ = {}; 272 | transfer_crc_ = {}; 273 | meta_ = {}; 274 | } 275 | 276 | private: 277 | void acceptHeader(const std::uint8_t bt) 278 | { 279 | if ((OffsetVersion == offset_) && (bt != FrameFormatVersion)) 280 | { 281 | reset(); 282 | } 283 | if (OffsetPriority == offset_) 284 | { 285 | meta_.priority = bt; 286 | } 287 | acceptHeaderField(OffsetSource, meta_.source, bt); 288 | acceptHeaderField(OffsetDestination, meta_.destination, bt); 289 | acceptHeaderField(OffsetDataSpec, meta_.data_spec, bt); 290 | acceptHeaderField(OffsetTransferID, meta_.transfer_id, bt); 291 | if ((OffsetFrameIndexEOT.first <= offset_) && (offset_ <= OffsetFrameIndexEOT.second) && 292 | (FrameIndexEOTReference.at(offset_ - OffsetFrameIndexEOT.first) != bt)) 293 | { 294 | reset(); 295 | } 296 | if (offset_ == (HeaderSize - 1U)) 297 | { 298 | if (!header_crc_.isResidueCorrect()) 299 | { 300 | reset(); // Header CRC error. 301 | } 302 | // At this point the header has been received and proven to be correct. Here, a generic implementation 303 | // would normally query the subscription list to see if the frame is interesting or it should be dropped; 304 | // also, the amount of dynamic memory that needs to be allocated for the payload would also be determined 305 | // at this moment. The main purpose of the header CRC is to permit such early-stage frame processing. 306 | // This specialized implementation requires none of that. 307 | } 308 | } 309 | 310 | template 311 | void acceptHeaderField(const std::pair range, Field& fld, const std::uint8_t bt) const 312 | { 313 | if ((range.first <= offset_) && (offset_ <= range.second)) 314 | { 315 | if (const auto byte_index = offset_ - range.first; byte_index > 0) 316 | { 317 | fld |= static_cast(static_cast(bt) << (BitsPerByte * byte_index)); 318 | } 319 | else 320 | { 321 | fld = static_cast(bt); 322 | } 323 | } 324 | } 325 | 326 | [[nodiscard]] auto isMetaValid() const -> bool 327 | { 328 | if (meta_.isResponse() || meta_.isRequest()) 329 | { 330 | return (meta_.source != Transfer::Metadata::AnonymousNodeID) && 331 | (meta_.destination != Transfer::Metadata::AnonymousNodeID); 332 | } 333 | return meta_.destination == Transfer::Metadata::AnonymousNodeID; 334 | } 335 | 336 | static constexpr std::size_t HeaderSize = 24; 337 | static constexpr std::size_t TotalOverheadSize = HeaderSize + CRC32C::Size; 338 | // Header field offsets. 339 | static constexpr std::size_t OffsetVersion = 0; 340 | static constexpr std::size_t OffsetPriority = 1; 341 | static constexpr std::pair OffsetSource{2, 3}; 342 | static constexpr std::pair OffsetDestination{4, 5}; 343 | static constexpr std::pair OffsetDataSpec{6, 7}; 344 | static constexpr std::pair OffsetTransferID{8, 15}; 345 | static constexpr std::pair OffsetFrameIndexEOT{16, 19}; 346 | 347 | COBSDecoder decoder_; 348 | std::size_t offset_ = 0; 349 | bool inside_ = false; 350 | CRC16CCITT header_crc_; 351 | CRC32C transfer_crc_; 352 | Transfer::Metadata meta_; 353 | std::array buf_{}; 354 | }; 355 | 356 | /// Sends a transfer with minimal buffering (some buffering is required by COBS) to save memory and reduce latency. 357 | /// Callback is of type (std::uint8_t) -> bool whose semantics reflects ISerialPort::send(). 358 | /// Callback shall not be an std::function<> to avoid heap allocation. 359 | template 360 | [[nodiscard]] inline auto transmit(const Callback& send_byte, const Transfer& tr) -> bool 361 | { 362 | COBSEncoder encoder(send_byte); 363 | CRC16CCITT header_crc; 364 | const auto header_out = [&header_crc, &encoder](const std::uint8_t b) { 365 | header_crc.update(b); 366 | return encoder.push(b); 367 | }; 368 | const auto header_out2 = [&header_out](const std::uint16_t bb) { 369 | return header_out(static_cast(bb)) && header_out(static_cast(bb >> BitsPerByte)); 370 | }; 371 | 372 | bool ok = header_out(FrameFormatVersion) && header_out(tr.meta.priority) && // 373 | header_out2(tr.meta.source) && header_out2(tr.meta.destination) && header_out2(tr.meta.data_spec); 374 | auto tmp_transfer_id = tr.meta.transfer_id; 375 | for (auto i = 0U; i < sizeof(std::uint64_t); i++) 376 | { 377 | ok = ok && header_out(static_cast(tmp_transfer_id)); 378 | tmp_transfer_id >>= BitsPerByte; 379 | } 380 | for (const auto x : FrameIndexEOTReference) 381 | { 382 | ok = ok && header_out(x); 383 | } 384 | for (const auto x : UserData) 385 | { 386 | ok = ok && header_out(x); 387 | } 388 | for (const auto x : header_crc.getBytes()) 389 | { 390 | ok = ok && header_out(x); 391 | } 392 | 393 | CRC32C transfer_crc; 394 | { 395 | const auto* ptr = tr.payload; 396 | for (std::size_t i = 0U; i < tr.payload_len; i++) 397 | { 398 | transfer_crc.update(*ptr); 399 | ok = ok && encoder.push(*ptr); 400 | ++ptr; 401 | if (!ok) 402 | { 403 | break; 404 | } 405 | } 406 | } 407 | for (const auto x : transfer_crc.getBytes()) 408 | { 409 | ok = ok && encoder.push(x); 410 | } 411 | return ok && encoder.end(); 412 | } 413 | 414 | } // namespace detail 415 | 416 | static constexpr NodeID MaxNodeID = 0xFFFEU; 417 | 418 | /// Bridges Kocherga/serial with the platform-specific serial port implementation. 419 | /// Implement this and pass a reference to SerialNode. 420 | class ISerialPort 421 | { 422 | public: 423 | /// Receive a single byte from the RX queue without blocking, if available. Otherwise, return an empty option. 424 | [[nodiscard]] virtual auto receive() -> std::optional = 0; 425 | 426 | /// Send a single byte into the TX queue without blocking if there is free space available. 427 | /// The queue shall be at least 1 KiB deep. 428 | /// Return true if enqueued or sent successfully; return false if no space available. 429 | [[nodiscard]] virtual auto send(const std::uint8_t b) -> bool = 0; 430 | 431 | virtual ~ISerialPort() = default; 432 | ISerialPort() = default; 433 | ISerialPort(const ISerialPort&) = delete; 434 | ISerialPort(ISerialPort&&) = delete; 435 | auto operator=(const ISerialPort&) -> ISerialPort& = delete; 436 | auto operator=(ISerialPort&&) -> ISerialPort& = delete; 437 | }; 438 | 439 | /// Kocherga node implementing the Cyphal/serial transport. 440 | class SerialNode : public kocherga::INode 441 | { 442 | public: 443 | /// The local UID shall be the same that is passed to the bootloader. It is used for PnP node-ID allocation. 444 | SerialNode(ISerialPort& port, const SystemInfo::UniqueID& local_unique_id) : 445 | unique_id_(local_unique_id), port_(port) 446 | {} 447 | 448 | /// Set up the local node-ID manually instead of running PnP allocation. 449 | /// If a manual update is triggered, this shall be done beforehand. 450 | /// Do not assign the local node-ID more than once. Invalid values will be ignored. 451 | void setLocalNodeID(const NodeID node_id) noexcept 452 | { 453 | if (node_id <= MaxNodeID) 454 | { 455 | local_node_id_ = node_id; 456 | } 457 | } 458 | 459 | /// Resets the state of the frame parser. Call it when the communication channel is reinitialized. 460 | void reset() noexcept { stream_parser_.reset(); } 461 | 462 | private: 463 | void poll(IReactor& reactor, const std::chrono::microseconds uptime) override 464 | { 465 | for (auto i = 0U; i < MaxBytesToProcessPerPoll; i++) 466 | { 467 | if (auto bt = port_.receive()) 468 | { 469 | if (const auto tr = stream_parser_.update(*bt)) 470 | { 471 | processReceivedTransfer(reactor, *tr, uptime); 472 | } 473 | } 474 | else 475 | { 476 | break; 477 | } 478 | } 479 | if ((!local_node_id_) && (uptime >= pnp_next_request_at_)) 480 | { 481 | using kocherga::detail::dsdl::PnPNodeIDAllocation; 482 | constexpr std::int64_t interval_usec = 483 | std::chrono::duration_cast(PnPNodeIDAllocation::MaxRequestInterval).count(); 484 | const std::chrono::microseconds delay{(getRandomByte() * interval_usec) / 485 | std::numeric_limits::max()}; 486 | pnp_next_request_at_ = uptime + delay; 487 | std::array buf{}; 488 | std::uint8_t* ptr = buf.data(); 489 | *ptr++ = std::numeric_limits::max(); 490 | *ptr++ = std::numeric_limits::max(); 491 | (void) std::memcpy(ptr, unique_id_.data(), unique_id_.size()); 492 | (void) publishMessageImpl(SubjectID::PnPNodeIDAllocationData_v2, pnp_transfer_id_, buf.size(), buf.data()); 493 | ++pnp_transfer_id_; 494 | } 495 | } 496 | 497 | void processReceivedTransfer(IReactor& reactor, const detail::Transfer& tr, const std::chrono::microseconds uptime) 498 | { 499 | if (const auto resp_id = tr.meta.isResponse()) 500 | { 501 | if (pending_request_meta_ && local_node_id_) 502 | { 503 | // Observe that we don't need to perform deduplication explicitly because it is naturally addressed by 504 | // the pending metadata struct: as soon as the response is received, it is invalidated immediately. 505 | const bool match = (resp_id == pending_request_meta_->service_id) && 506 | (tr.meta.source == pending_request_meta_->server_node_id) && 507 | (tr.meta.destination == *local_node_id_) && 508 | (tr.meta.transfer_id == pending_request_meta_->transfer_id); 509 | if (match) 510 | { 511 | pending_request_meta_.reset(); // Reset first in case if the reactor initiates another request. 512 | reactor.processResponse(tr.payload_len, tr.payload); 513 | } 514 | } 515 | } 516 | else if (const auto req_id = tr.meta.isRequest()) 517 | { 518 | if (local_node_id_ && (tr.meta.destination == *local_node_id_)) 519 | { 520 | // This implementation of deduplication is grossly oversimplified. It is considered to be acceptable 521 | // for this specific library because double acceptance is not expected to cause adverse effects at the 522 | // application layer. The difference compared to a fully conformant implementation is that it will 523 | // accept duplicate request if there was another accepted request between the duplicates: 524 | // - The normal case where duplicates removed is like: (A, A, A, B, B) --> (A, B) 525 | // - The edge case where they are not removed is like: (A, A, B, B, A) --> (A, B, A) 526 | // Full-fledged implementations are obviously immune to this because they keep separate state per 527 | // session specifier, which does come with certain complexity (e.g., see libserard). 528 | const auto [last_meta, last_ts] = last_received_request_meta_; 529 | const bool duplicate = // 530 | ((last_ts + ::kocherga::detail::DefaultTransferIDTimeout) >= uptime) && // 531 | (last_meta.data_spec == tr.meta.data_spec) && // 532 | (last_meta.source == tr.meta.source) && // 533 | (last_meta.transfer_id == tr.meta.transfer_id); 534 | if (!duplicate) 535 | { 536 | last_received_request_meta_ = {tr.meta, uptime}; 537 | std::array buf{}; 538 | if (const auto size = 539 | reactor.processRequest(*req_id, tr.meta.source, tr.payload_len, tr.payload, buf.data())) 540 | { 541 | detail::Transfer::Metadata meta{}; 542 | meta.priority = tr.meta.priority; 543 | meta.source = *local_node_id_; 544 | meta.destination = tr.meta.source; 545 | meta.data_spec = static_cast(*req_id) | 546 | static_cast(detail::Transfer::Metadata::DataSpecServiceFlag); 547 | meta.transfer_id = tr.meta.transfer_id; 548 | for (auto i = 0U; i < service_multiplication_factor_; i++) 549 | { 550 | (void) transmit({meta, *size, buf.data()}); 551 | } 552 | } 553 | } 554 | } 555 | } 556 | else 557 | { 558 | if ((!local_node_id_) && // If node-ID is not yet allocated, check if this is an allocation response. 559 | (tr.meta.data_spec == static_cast(SubjectID::PnPNodeIDAllocationData_v2)) && 560 | (tr.meta.source != detail::Transfer::Metadata::AnonymousNodeID) && 561 | (tr.meta.destination == detail::Transfer::Metadata::AnonymousNodeID)) 562 | { 563 | const std::uint8_t* ptr = tr.payload; 564 | NodeID node_id = *ptr; 565 | ++ptr; 566 | node_id |= static_cast(static_cast(*ptr) << detail::BitsPerByte); 567 | ++ptr; 568 | const bool uid_match = std::equal(std::begin(unique_id_), std::end(unique_id_), ptr); 569 | if (uid_match) 570 | { 571 | local_node_id_ = node_id; 572 | } 573 | } 574 | } 575 | } 576 | 577 | [[nodiscard]] auto sendRequest(const ServiceID service_id, 578 | const NodeID server_node_id, 579 | const TransferID transfer_id, 580 | const std::size_t payload_length, 581 | const std::uint8_t* const payload) -> bool override 582 | { 583 | if (local_node_id_) 584 | { 585 | detail::Transfer::Metadata meta{}; 586 | meta.source = *local_node_id_; 587 | meta.destination = server_node_id; 588 | meta.data_spec = 589 | static_cast(service_id) | static_cast(detail::Transfer::Metadata::DataSpecServiceFlag | 590 | detail::Transfer::Metadata::DataSpecRequestFlag); 591 | meta.transfer_id = transfer_id; 592 | bool transmit_ok = false; // Optimistic aggregation: one successful transmission is considered a success. 593 | for (auto i = 0U; i < service_multiplication_factor_; i++) 594 | { 595 | transmit_ok = transmit({meta, payload_length, payload}) || transmit_ok; 596 | } 597 | if (transmit_ok) 598 | { 599 | pending_request_meta_ = PendingRequestMetadata{ 600 | server_node_id, 601 | static_cast(service_id), 602 | transfer_id, 603 | }; 604 | return true; 605 | } 606 | } 607 | return false; 608 | } 609 | 610 | void cancelRequest() override { pending_request_meta_.reset(); } 611 | 612 | auto publishMessage(const SubjectID subject_id, 613 | const TransferID transfer_id, 614 | const std::size_t payload_length, 615 | const std::uint8_t* const payload) -> bool override 616 | { 617 | if (local_node_id_) 618 | { 619 | return publishMessageImpl(subject_id, transfer_id, payload_length, payload); 620 | } 621 | return false; 622 | } 623 | 624 | auto publishMessageImpl(const SubjectID subject_id, 625 | const TransferID transfer_id, 626 | const std::size_t payload_length, 627 | const std::uint8_t* const payload) -> bool 628 | { 629 | detail::Transfer::Metadata meta{}; 630 | meta.source = local_node_id_ ? *local_node_id_ : detail::Transfer::Metadata::AnonymousNodeID; 631 | meta.data_spec = static_cast(subject_id); 632 | meta.transfer_id = transfer_id; 633 | return transmit({meta, payload_length, payload}); 634 | } 635 | 636 | [[nodiscard]] auto transmit(const detail::Transfer& tr) -> bool 637 | { 638 | return detail::transmit([this](const std::uint8_t b) { return port_.send(b); }, tr); 639 | } 640 | 641 | struct PendingRequestMetadata 642 | { 643 | NodeID server_node_id{}; 644 | PortID service_id{}; 645 | TransferID transfer_id{}; 646 | }; 647 | 648 | static constexpr std::size_t MaxBytesToProcessPerPoll = 1024; 649 | 650 | const SystemInfo::UniqueID unique_id_; 651 | 652 | ISerialPort& port_; 653 | detail::StreamParser stream_parser_; 654 | std::optional local_node_id_; 655 | std::optional pending_request_meta_; 656 | std::pair last_received_request_meta_{}; 657 | 658 | std::chrono::microseconds pnp_next_request_at_{0}; 659 | std::uint64_t pnp_transfer_id_ = 0; 660 | 661 | /// Controls deterministic data loss mitigation for outgoing service transfers. Messages are never duplicated. 662 | const std::uint8_t service_multiplication_factor_ = 1; 663 | }; 664 | 665 | } // namespace kocherga::serial 666 | -------------------------------------------------------------------------------- /tests/.clang-tidy: -------------------------------------------------------------------------------- 1 | InheritParentConfig: true 2 | Checks: >- 3 | boost-*, 4 | bugprone-*, 5 | cert-*, 6 | clang-analyzer-*, 7 | cppcoreguidelines-*, 8 | google-*, 9 | hicpp-*, 10 | llvm-*, 11 | misc-*, 12 | modernize-*, 13 | performance-*, 14 | portability-*, 15 | readability-*, 16 | -google-readability-todo, 17 | -readability-identifier-length, 18 | -readability-avoid-const-params-in-decls, 19 | -readability-magic-numbers, 20 | -readability-function-cognitive-complexity, 21 | -llvm-header-guard, 22 | -google-runtime-references, 23 | -misc-non-private-member-variables-in-classes, 24 | -cppcoreguidelines-non-private-member-variables-in-classes, 25 | -cppcoreguidelines-avoid-magic-numbers, 26 | -cppcoreguidelines-pro-bounds-array-to-pointer-decay, 27 | -cppcoreguidelines-pro-bounds-pointer-arithmetic, 28 | -cppcoreguidelines-pro-type-reinterpret-cast, 29 | -modernize-pass-by-value, 30 | -hicpp-no-array-decay, 31 | -cert-msc30-c, 32 | -cert-msc50-cpp, 33 | -*-function-size, 34 | -*-easily-swappable-parameters, 35 | -*-owning-memory, 36 | -*-malloc, 37 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This software is distributed under the terms of the MIT License. 2 | # Copyright (c) 2020 Zubax Robotics. 3 | # Author: Pavel Kirienko 4 | 5 | cmake_minimum_required(VERSION 3.19) 6 | enable_testing() 7 | 8 | project(kocherga CXX) 9 | 10 | set(CXX_EXTENSIONS OFF) 11 | set(CMAKE_CXX_STANDARD 17) 12 | list(APPEND CMAKE_CTEST_ARGUMENTS "--output-on-failure") 13 | 14 | if(NOT CMAKE_BUILD_TYPE) 15 | set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING 16 | "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." 17 | FORCE) 18 | endif() 19 | 20 | set(LIBRARY_DIR "${CMAKE_SOURCE_DIR}/../kocherga") 21 | 22 | # Use -DNO_STATIC_ANALYSIS=1 to suppress static analysis if the tools are not available. 23 | if (NOT NO_STATIC_ANALYSIS) 24 | find_program(clang_tidy NAMES clang-tidy) 25 | if (NOT clang_tidy) 26 | message(FATAL_ERROR "Could not locate clang-tidy") 27 | endif () 28 | message(STATUS "Using clang-tidy: ${clang_tidy}") 29 | set(CMAKE_CXX_CLANG_TIDY ${clang_tidy}) 30 | endif () 31 | 32 | # clang-format 33 | find_program(clang_format NAMES clang-format) 34 | if (NOT clang_format) 35 | message(STATUS "Could not locate clang-format") 36 | else () 37 | file(GLOB_RECURSE FORMAT_FILES 38 | ${LIBRARY_DIR}/*.hpp 39 | ${CMAKE_SOURCE_DIR}/unit/*.[ch]pp 40 | ${CMAKE_SOURCE_DIR}/integration/*.[ch]pp 41 | ${CMAKE_SOURCE_DIR}/util/*.[ch]pp) 42 | message(STATUS "Using clang-format: ${clang_format}; files: ${FORMAT_FILES}") 43 | add_custom_target(format COMMAND ${clang_format} -i -fallback-style=none -style=file --verbose ${FORMAT_FILES}) 44 | endif () 45 | 46 | add_subdirectory(unit) 47 | add_subdirectory(integration) 48 | -------------------------------------------------------------------------------- /tests/integration/.gitignore: -------------------------------------------------------------------------------- 1 | # Test outputs 2 | *.json 3 | *.rom 4 | *.db 5 | -------------------------------------------------------------------------------- /tests/integration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This software is distributed under the terms of the MIT License. 2 | # Copyright (c) 2021 Zubax Robotics. 3 | # Author: Pavel Kirienko 4 | 5 | cmake_minimum_required(VERSION 3.19) 6 | enable_testing() 7 | 8 | project(kocherga_integration_test CXX) 9 | 10 | set(library_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../kocherga") 11 | 12 | # C++ options 13 | set(CXX_EXTENSIONS OFF) 14 | set(CMAKE_CXX_STANDARD 17) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wconversion -Wsign-promo") 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations") 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wtype-limits -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") 21 | # Attribute warning is useless in GCC: https://stackoverflow.com/questions/50646334 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-attributes") 23 | 24 | # Enable coverage if makes sense. 25 | if ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_BUILD_TYPE STREQUAL "Debug")) 26 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 --coverage") 27 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") 28 | endif () 29 | 30 | include_directories(${library_dir}) 31 | include_directories(../util) 32 | 33 | file(GLOB bootloader_sources ${CMAKE_CURRENT_SOURCE_DIR}/bootloader/*.cpp) 34 | add_executable(bootloader ${bootloader_sources}) 35 | 36 | # To run just this test specifically, go to the binary directory and run: ctest -R run_validator 37 | add_test(run_validator 38 | "${CMAKE_CURRENT_SOURCE_DIR}/validator/validate.sh" 39 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) 40 | set_tests_properties(run_validator PROPERTIES TIMEOUT 30) 41 | -------------------------------------------------------------------------------- /tests/integration/bootloader/main.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2021 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "kocherga_serial.hpp" 6 | #include "kocherga_can.hpp" 7 | #include "util.hpp" 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace 23 | { 24 | /// Tunnels serial port connection via TCP socket as-is without any wrapping. 25 | /// Per Kocherga's API contracts, the API is fully non-blocking. 26 | class TCPSerialPort : public kocherga::serial::ISerialPort 27 | { 28 | public: 29 | explicit TCPSerialPort(const int sock_fd) : fd_(sock_fd) {} 30 | 31 | ~TCPSerialPort() override { (void) ::close(fd_); } 32 | 33 | TCPSerialPort(const TCPSerialPort&) = delete; 34 | TCPSerialPort(TCPSerialPort&&) = delete; 35 | auto operator=(const TCPSerialPort&) -> TCPSerialPort& = delete; 36 | auto operator=(TCPSerialPort&&) -> TCPSerialPort& = delete; 37 | 38 | static auto connect(const char* const remote_host, const std::uint16_t remote_port) 39 | -> std::shared_ptr 40 | { 41 | const ::hostent* const he = gethostbyname(remote_host); 42 | if (he == nullptr) 43 | { 44 | throw std::runtime_error(std::string("Could not resolve host: ") + remote_host); 45 | } 46 | ::sockaddr_in sa{}; 47 | sa.sin_family = AF_INET; 48 | sa.sin_port = ::htons(remote_port); 49 | sa.sin_addr = *static_cast(static_cast(he->h_addr)); 50 | 51 | const auto fd = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 52 | if (fd < 0) 53 | { 54 | throw std::runtime_error("Could not open socket"); 55 | } 56 | if (::connect(fd, reinterpret_cast(&sa), sizeof(sockaddr)) < 0) 57 | { 58 | (void) ::close(fd); 59 | throw std::runtime_error("Could not connect to remote endpoint at: " + std::string(remote_host) + ":" + 60 | std::to_string(static_cast(remote_port))); 61 | } 62 | return std::make_shared(fd); 63 | } 64 | 65 | [[nodiscard]] auto receive() -> std::optional override 66 | { 67 | std::uint8_t out{}; 68 | if (::recv(fd_, &out, 1, MSG_DONTWAIT) > 0) 69 | { 70 | return out; 71 | } 72 | return {}; 73 | } 74 | 75 | [[nodiscard]] auto send(const std::uint8_t b) -> bool override { return ::send(fd_, &b, 1, MSG_DONTWAIT) > 0; } 76 | 77 | private: 78 | const int fd_; 79 | }; 80 | 81 | class SocketCANDriver : public kocherga::can::ICANDriver 82 | { 83 | public: 84 | explicit SocketCANDriver(const std::string& iface_name) : fd_(setup(iface_name)) {} 85 | 86 | private: 87 | [[nodiscard]] auto configure(const Bitrate& bitrate, 88 | const bool silent, 89 | const kocherga::can::CANAcceptanceFilterConfig& filter) -> std::optional override 90 | { 91 | // Simplified implementation. The bit rate is configured externally. 92 | std::clog << "SocketCANDriver::configure(" // 93 | << "bitrate={" << bitrate.arbitration << "," << bitrate.data << "}, " // 94 | << "silent=" << silent << ", " // 95 | << "filter={" << filter.extended_can_id << "," << filter.mask << "})" // 96 | << std::endl; 97 | return Mode::FD; 98 | } 99 | 100 | [[nodiscard]] auto push(const bool force_classic_can, 101 | const std::uint32_t extended_can_id, 102 | const std::uint8_t payload_size, 103 | const void* const payload) -> bool override 104 | { 105 | ::canfd_frame frame{}; 106 | frame.can_id = extended_can_id | CAN_EFF_FLAG; 107 | frame.len = payload_size; 108 | frame.flags = force_classic_can ? 0 : CANFD_BRS; 109 | (void) std::memcpy(frame.data, payload, payload_size); 110 | return ::send(fd_, &frame, force_classic_can ? CAN_MTU : CANFD_MTU, MSG_DONTWAIT) > 0; 111 | } 112 | 113 | [[nodiscard]] auto pop(PayloadBuffer& payload_buffer) 114 | -> std::optional> override 115 | { 116 | ::canfd_frame frame{}; 117 | const auto rx_out = ::recv(fd_, &frame, sizeof(::canfd_frame), MSG_DONTWAIT); 118 | if (rx_out > 0) 119 | { 120 | if (((frame.can_id & CAN_EFF_FLAG) != 0) && // 121 | ((frame.can_id & CAN_ERR_FLAG) == 0) && // 122 | ((frame.can_id & CAN_RTR_FLAG) == 0)) 123 | { 124 | (void) std::memcpy(payload_buffer.data(), 125 | frame.data, 126 | std::min(frame.len, payload_buffer.max_size())); 127 | return std::pair{frame.can_id & CAN_EFF_MASK, frame.len}; 128 | } 129 | } 130 | return {}; 131 | } 132 | 133 | [[nodiscard]] static auto setup(const std::string& iface_name) -> int 134 | { 135 | const int fd = ::socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW); 136 | if (fd < 0) 137 | { 138 | throw std::runtime_error("Could not open CAN socket"); 139 | } 140 | ::ifreq ifr{}; 141 | (void) std::memcpy(ifr.ifr_name, iface_name.data(), iface_name.length() + 1); // NOLINT union 142 | if (0 != ::ioctl(fd, SIOCGIFINDEX, &ifr)) // NOLINT vararg 143 | { 144 | (void) ::close(fd); 145 | throw std::runtime_error("No such CAN interface: " + iface_name); 146 | } 147 | ::sockaddr_can adr{}; 148 | adr.can_family = AF_CAN; 149 | adr.can_ifindex = ifr.ifr_ifindex; // NOLINT union 150 | if (0 != ::bind(fd, reinterpret_cast<::sockaddr*>(&adr), sizeof(adr))) 151 | { 152 | (void) ::close(fd); 153 | throw std::runtime_error("Could not bind CAN socket"); 154 | } 155 | const int en = 1; 156 | if (0 != ::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &en, sizeof(en))) 157 | { 158 | (void) ::close(fd); 159 | throw std::runtime_error("Could not enable FD mode on the CAN socket -- is CAN FD supported?"); 160 | } 161 | return fd; 162 | } 163 | 164 | const int fd_; 165 | }; 166 | 167 | auto initSerialPort() -> std::shared_ptr 168 | { 169 | const auto iface_env = util::getEnvironmentVariableMaybe("UAVCAN__SERIAL__IFACE"); 170 | if (!iface_env) 171 | { 172 | return nullptr; 173 | } 174 | static const std::string Prefix = "socket://"; 175 | if (iface_env->find(Prefix) != 0) 176 | { 177 | throw std::invalid_argument("Expected serial port prefix: " + Prefix); 178 | } 179 | const auto endpoint = iface_env->substr(Prefix.size()); 180 | const auto colon_pos = endpoint.find(':'); 181 | if ((colon_pos == std::string::npos) || ((colon_pos + 1) >= endpoint.size())) 182 | { 183 | throw std::invalid_argument("Invalid serial port name format: " + endpoint); 184 | } 185 | const auto host = endpoint.substr(0, colon_pos); 186 | const auto port_str = endpoint.substr(colon_pos + 1); 187 | const auto port_raw = std::stoull(port_str, nullptr, 0); 188 | const auto port = static_cast(port_raw); 189 | if (port != port_raw) 190 | { 191 | throw std::invalid_argument("Port number invalid: " + port_str); 192 | } 193 | return TCPSerialPort::connect(host.c_str(), port); 194 | } 195 | 196 | auto initCANDriver() -> std::shared_ptr 197 | { 198 | const auto iface_env = util::getEnvironmentVariableMaybe("UAVCAN__CAN__IFACE"); 199 | if (!iface_env) 200 | { 201 | return nullptr; 202 | } 203 | static const std::string Prefix = "socketcan:"; 204 | if (iface_env->find(Prefix) != 0) 205 | { 206 | throw std::invalid_argument("Unsupported iface name prefix: " + Prefix); 207 | } 208 | const auto socketcan_iface_name = iface_env->substr(Prefix.size()); 209 | if (socketcan_iface_name.empty()) 210 | { 211 | throw std::runtime_error("SocketCAN iface name cannot be empty"); 212 | } 213 | return std::make_shared(socketcan_iface_name); 214 | } 215 | 216 | auto getSystemInfo() -> kocherga::SystemInfo 217 | { 218 | kocherga::SystemInfo system_info{}; 219 | system_info.node_name = "com.zubax.kocherga.test.integration"; 220 | { 221 | auto hw_ver = util::getEnvironmentVariable("UAVCAN__NODE__HARDWARE_VERSION"); 222 | const auto maj = std::stoull(hw_ver.substr(0, hw_ver.find(' '))); 223 | const auto min = std::stoull(hw_ver.substr(hw_ver.find(' ') + 1)); 224 | if (maj > std::numeric_limits::max() || min > std::numeric_limits::max()) 225 | { 226 | throw std::invalid_argument("Hardware version numbers out of range"); 227 | } 228 | system_info.hardware_version = {static_cast(maj), static_cast(min)}; 229 | } 230 | { 231 | const auto uid = util::getEnvironmentVariable("UAVCAN__NODE__UNIQUE_ID"); 232 | if (uid.length() > system_info.unique_id.size()) 233 | { 234 | throw std::runtime_error("Invalid value length of register uavcan.node.unique_id"); 235 | } 236 | std::copy(uid.begin(), uid.end(), system_info.unique_id.begin()); 237 | } 238 | { 239 | static const auto coa = util::getEnvironmentVariableMaybe("UAVCAN__NODE__CERTIFICATE_OF_AUTHENTICITY"); 240 | if (coa) 241 | { 242 | system_info.certificate_of_authenticity_len = static_cast(coa->size()); 243 | system_info.certificate_of_authenticity = reinterpret_cast(coa->data()); 244 | } 245 | } 246 | return system_info; 247 | } 248 | 249 | } // namespace 250 | 251 | auto kocherga::getRandomByte() -> std::uint8_t 252 | { 253 | return util::getRandomInteger(); 254 | } 255 | 256 | auto main(const int argc, char* const argv[]) -> int 257 | { 258 | (void) argc; 259 | try 260 | { 261 | const bool linger = util::getEnvironmentVariable("BOOTLOADER__LINGER") != "0"; 262 | const auto rom_file = util::getEnvironmentVariable("BOOTLOADER__ROM_FILE"); 263 | const auto rom_size = std::stoul(util::getEnvironmentVariable("BOOTLOADER__ROM_SIZE")); 264 | const auto max_app_size = std::stoul(util::getEnvironmentVariable("BOOTLOADER__MAX_APP_SIZE")); 265 | const auto boot_delay = 266 | std::chrono::seconds(std::stoul(util::getEnvironmentVariableMaybe("BOOTLOADER__BOOT_DELAY").value_or("0"))); 267 | std::clog << "Bootloader configuration:" // 268 | << " linger=" << linger // 269 | << " rom_file=" << rom_file // 270 | << " rom_size=" << rom_size // 271 | << " max_app_size=" << max_app_size // 272 | << " boot_delay=" << boot_delay.count() // 273 | << std::endl; 274 | 275 | util::FileROMBackend rom(rom_file, rom_size); 276 | 277 | const auto system_info = getSystemInfo(); 278 | kocherga::Bootloader::Params params; 279 | params.max_app_size = max_app_size; 280 | params.linger = linger; 281 | params.boot_delay = boot_delay; 282 | kocherga::Bootloader boot(rom, system_info, params); 283 | 284 | // Configure the serial port node. 285 | auto serial_port = initSerialPort(); 286 | if (serial_port) 287 | { 288 | std::clog << "Using Cyphal/serial" << std::endl; 289 | (void) boot.addNode(new kocherga::serial::SerialNode(*serial_port, system_info.unique_id)); // NOLINT owner 290 | } 291 | 292 | // Configure the CAN node. 293 | auto can_driver = initCANDriver(); 294 | if (can_driver) 295 | { 296 | std::clog << "Using Cyphal/CAN" << std::endl; 297 | (void) boot.addNode(new kocherga::can::CANNode(*can_driver, system_info.unique_id)); // NOLINT owner 298 | } 299 | 300 | const auto started_at = std::chrono::steady_clock::now(); 301 | std::clog << "Bootloader started" << std::endl; 302 | while (true) 303 | { 304 | const auto uptime = std::chrono::steady_clock::now() - started_at; 305 | if (const auto fin = boot.poll(std::chrono::duration_cast(uptime))) 306 | { 307 | std::clog << "Final state reached: " << static_cast(*fin) << std::endl; 308 | if (*fin == kocherga::Final::BootApp) 309 | { 310 | std::clog << "Booting the application" << std::endl; 311 | break; 312 | } 313 | if (*fin == kocherga::Final::Restart) 314 | { 315 | std::clog << "Restarting the bootloader; using executable " << argv[0] << std::endl; 316 | return -::execve(argv[0], argv, ::environ); 317 | } 318 | assert(false); 319 | } 320 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 321 | } 322 | } 323 | catch (std::exception& ex) 324 | { 325 | std::cerr << "Unhandled exception: " << ex.what() << std::endl; 326 | return 1; 327 | } 328 | return 0; 329 | } 330 | -------------------------------------------------------------------------------- /tests/integration/validator/can.orc.yaml: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S yakut -v orchestrate 2 | 3 | uavcan: 4 | can: 5 | iface: socketcan:vcan0 6 | diagnostic: 7 | severity: 3 8 | 9 | $=: 10 | - ?=: rm -f *.rom *.json 11 | - ?=: sudo modprobe can ; sudo modprobe can_raw ; sudo modprobe vcan 12 | - 13 | - ?=: sudo ip link add dev vcan0 type vcan && sudo ip link set vcan0 mtu 72 && sudo ip link set up vcan0 14 | - 15 | - $=: yakut --verbose file-server --plug-and-play alloc.db $SOFTWARE_PACKAGE_DIR --update-software 16 | uavcan.node.id: 32 17 | - $=: yakut --json sub --with-metadata uavcan.node.Heartbeat > heartbeats.json 18 | - $=: yakut --json sub --with-metadata uavcan.diagnostic.Record > diagnostics.json 19 | - $=: 20 | - ./bootloader 21 | - # Wait for the bootloader to exit. It will only do that after it has successfully loaded and validated the app. 22 | - exit 223 # A non-zero exit code will bring down the entire composition, which signifies the end of the test. 23 | uavcan: 24 | node: 25 | hardware_version: [10, 30] 26 | unique_id: aaaaaaaaaaaaaaaa 27 | certificate_of_authenticity: this is a certificate 28 | bootloader: 29 | rom_file: b.rom 30 | rom_size: 1024 31 | max_app_size: 512 32 | linger: false 33 | -------------------------------------------------------------------------------- /tests/integration/validator/com.zubax.kocherga.test.integration-10-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin: -------------------------------------------------------------------------------- 1 | ../../unit/images/good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin -------------------------------------------------------------------------------- /tests/integration/validator/manual_v0.orc.yaml: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S yakut -v orchestrate 2 | # This composition is mostly intended for manual testing due to lack of adequate automation tools for DroneCAN. 3 | 4 | uavcan: 5 | can: 6 | iface: socketcan:vcan0 7 | node: 8 | hardware_version: [20, 21] 9 | unique_id: cccccccccccccccc 10 | certificate_of_authenticity: this is a certificate 11 | bootloader: 12 | rom_file: c.rom 13 | rom_size: 10485760 14 | max_app_size: 1048576 15 | linger: true 16 | boot_delay: 30 17 | 18 | 19 | $=: 20 | - ?=: rm -f *.rom *.json 21 | - ?=: sudo modprobe can ; sudo modprobe can_raw ; sudo modprobe vcan 22 | - 23 | - ?=: sudo ip link add dev vcan0 type vcan && sudo ip link set vcan0 mtu 72 && sudo ip link set up vcan0 24 | - 25 | - dronecan_gui_tool # Start the tool for manual testing. 26 | - ./bootloader 27 | -------------------------------------------------------------------------------- /tests/integration/validator/serial.orc.yaml: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S yakut -v orchestrate 2 | 3 | uavcan: 4 | serial: 5 | iface: socket://localhost:50905 6 | duplicate_service_transfers: true 7 | diagnostic: 8 | severity: 3 9 | 10 | $=: 11 | - ?=: rm -f *.rom *.json # Clean up the outputs from the previous run. 12 | - # Do not proceed until the cleanup is done. 13 | - ?=: ncat --broker --listen -p 50905 -v # Ignore error in case it is already launched externally. 14 | - $=: 15 | - sleep 1 16 | - # Wait for the broker to start and get ready. 17 | 18 | - $=: yakut --verbose file-server --plug-and-play alloc.db $SOFTWARE_PACKAGE_DIR --update-software 19 | uavcan.node.id: 32 20 | 21 | - $=: yakut --json sub --with-metadata uavcan.node.Heartbeat > heartbeats.json 22 | - $=: yakut --json sub --with-metadata uavcan.diagnostic.Record > diagnostics.json 23 | 24 | - $=: 25 | - sleep 1 26 | - # Let the subscribers initialize to ensure we don't lose any diagnostic messages. 27 | - ./bootloader 28 | - # Wait for the bootloader to exit. It will only do that after it has successfully loaded and validated the app. 29 | - exit 222 # A non-zero exit code will bring down the entire composition, which signifies the end of the test. 30 | uavcan: 31 | node: 32 | hardware_version: [10, 30] 33 | unique_id: aaaaaaaaaaaaaaaa 34 | certificate_of_authenticity: this is a certificate 35 | bootloader: 36 | rom_file: a.rom 37 | rom_size: 1024 38 | max_app_size: 512 39 | linger: false 40 | -------------------------------------------------------------------------------- /tests/integration/validator/validate.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -o nounset 4 | 5 | function die() 6 | { 7 | echo >&2 "FAILURE: $*" 8 | exit 1 9 | } 10 | 11 | export SOFTWARE_PACKAGE_DIR=$(dirname "$0") 12 | 13 | echo "TESTING: Cyphal/serial" 14 | yakut -v orchestrate "$SOFTWARE_PACKAGE_DIR/serial.orc.yaml" 15 | result=$? 16 | [[ $result == 222 ]] || die "Unexpected exit code: $result" 17 | echo "Exit code $result is valid, test passed" 18 | 19 | echo "TESTING: Cyphal/CAN" 20 | yakut -v orchestrate "$SOFTWARE_PACKAGE_DIR/can.orc.yaml" 21 | result=$? 22 | [[ $result == 223 ]] || die "Unexpected exit code: $result" 23 | echo "Exit code $result is valid, test passed" 24 | 25 | echo "PLEASE TEST v0 MANUALLY! RUN THIS: $SOFTWARE_PACKAGE_DIR/manual_v0.orc.yaml" 26 | -------------------------------------------------------------------------------- /tests/tools/.gitignore: -------------------------------------------------------------------------------- 1 | # Test output files. 2 | *.app*.bin 3 | -------------------------------------------------------------------------------- /tests/tools/demo-be.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/tools/demo-be.bin -------------------------------------------------------------------------------- /tests/tools/demo-le.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/tools/demo-le.bin -------------------------------------------------------------------------------- /tests/tools/invalid.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/tools/invalid.bin -------------------------------------------------------------------------------- /tests/tools/test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -o nounset 4 | set -o xtrace 5 | 6 | function die() 7 | { 8 | echo >&2 "FAILURE: $*" 9 | exit 1 10 | } 11 | 12 | cd "${0%/*}" || die "Couldn't cd into this script's directory" 13 | 14 | SCRIPT=../../tools/kocherga_image.py 15 | 16 | $SCRIPT self-test || die "Self-test unsuccessful" 17 | echo 18 | 19 | $SCRIPT demo-le.bin -vv --assign-vcs-revision-id=0xbadc_0ffe_e0dd_f00d --assign-flag-dirty=1 --assign-flag-release=0 20 | [ -f demo-le-1.16.badc0ffee0ddf00d.01a43c554cb3de13.app.dirty.bin ] || die "Output file has not been created" 21 | echo 22 | 23 | $SCRIPT demo-le.bin -vv --assign-vcs-revision-id=0xbadc_0ffe_e0dd_f00d --assign-version=2.5 24 | [ -f demo-le-2.5.badc0ffee0ddf00d.ca47fd04aefc15b6.app.release.dirty.bin ] || die "Output file has not been created" 25 | echo 26 | 27 | $SCRIPT nonexistent -vv && die "Expected failure" 28 | echo 29 | 30 | $SCRIPT invalid.bin -vv && die "Expected failure" 31 | echo 32 | 33 | # This image is already processed. 34 | $SCRIPT demo-le-2.5.badc0ffee0ddf00d.ca47fd04aefc15b6.app.release.dirty.bin -vv && die "Expected failure" 35 | $SCRIPT demo-le-2.5.badc0ffee0ddf00d.ca47fd04aefc15b6.app.release.dirty.bin -vv --lazy || die "Expected success" 36 | 37 | rm ./*.app*.bin 38 | -------------------------------------------------------------------------------- /tests/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This software is distributed under the terms of the MIT License. 2 | # Copyright (c) 2020 Zubax Robotics. 3 | # Author: Pavel Kirienko 4 | 5 | cmake_minimum_required(VERSION 3.19) 6 | enable_testing() 7 | 8 | project(kocherga_unit_test CXX) 9 | 10 | set(library_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../kocherga") 11 | 12 | # C++ options 13 | set(CXX_EXTENSIONS OFF) 14 | set(CMAKE_CXX_STANDARD 17) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wconversion -Wsign-promo") 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations") 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wtype-limits -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") 21 | # Attribute warning is useless in GCC: https://stackoverflow.com/questions/50646334 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-attributes") 23 | 24 | include_directories(${library_dir}) 25 | include_directories(../util) 26 | include_directories(SYSTEM ../3rd_party) 27 | add_definitions(-DCATCH_CONFIG_ENABLE_ALL_STRINGMAKERS=1) 28 | 29 | # Test targets 30 | function(gen_test name files compile_definitions compile_flags link_flags) 31 | add_executable(${name} ${files}) 32 | target_compile_definitions(${name} PUBLIC ${compile_definitions}) 33 | target_link_libraries(${name} pthread) 34 | set_target_properties(${name} PROPERTIES COMPILE_FLAGS "${compile_flags}" LINK_FLAGS "${link_flags}") 35 | 36 | add_test("run_${name}" "${name}" --rng-seed time) 37 | set_tests_properties("run_${name}" PROPERTIES ENVIRONMENT SOURCE_DIR=${CMAKE_CURRENT_SOURCE_DIR}) 38 | endfunction() 39 | 40 | file(GLOB test_sources 41 | ${CMAKE_CURRENT_SOURCE_DIR}/test_*.cpp 42 | ${CMAKE_CURRENT_SOURCE_DIR}/serial/test_*.cpp 43 | ${CMAKE_CURRENT_SOURCE_DIR}/can/test_*.cpp) 44 | gen_test("test_x64" "${test_sources}" "" "-m64" "-m64") 45 | gen_test("test_x32" "${test_sources}" "" "-m32" "-m32") 46 | if ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_BUILD_TYPE STREQUAL "Debug")) 47 | gen_test("test_cov" "${test_sources}" "" "-g -O0 --coverage" "--coverage") 48 | endif () 49 | -------------------------------------------------------------------------------- /tests/unit/images/bad-le-crc-x3.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/unit/images/bad-le-crc-x3.bin -------------------------------------------------------------------------------- /tests/unit/images/bad-le-short.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/unit/images/bad-le-short.bin -------------------------------------------------------------------------------- /tests/unit/images/com.zubax.telega-1-0.3.68620b82.application.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/unit/images/com.zubax.telega-1-0.3.68620b82.application.bin -------------------------------------------------------------------------------- /tests/unit/images/good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/unit/images/good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin -------------------------------------------------------------------------------- /tests/unit/images/good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Zubax/kocherga/d806ba1dd6acb550aeeea7e21172fa08535a0213/tests/unit/images/good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin -------------------------------------------------------------------------------- /tests/unit/mock.hpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #pragma once 6 | 7 | #include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. 8 | #include "catch.hpp" 9 | #include "util.hpp" 10 | #include 11 | #include 12 | 13 | namespace mock 14 | { 15 | struct Transfer 16 | { 17 | std::optional remote_node_id; 18 | kocherga::TransferID transfer_id{}; 19 | std::vector payload; 20 | 21 | Transfer() = default; 22 | Transfer(const kocherga::TransferID arg_transfer_id, 23 | const std::vector& arg_payload, 24 | const std::optional arg_remote_node_id = std::optional()) : 25 | remote_node_id(arg_remote_node_id), transfer_id(arg_transfer_id), payload(arg_payload) 26 | {} 27 | Transfer(const kocherga::TransferID arg_transfer_id, 28 | const std::size_t arg_payload_length, 29 | const std::uint8_t* const arg_payload, 30 | const std::optional arg_remote_node_id = std::optional()) : 31 | remote_node_id(arg_remote_node_id), transfer_id(arg_transfer_id) 32 | { 33 | (void) std::copy_n(arg_payload, arg_payload_length, std::back_inserter(payload)); 34 | } 35 | 36 | [[nodiscard]] auto toString() const -> std::string 37 | { 38 | return "remote_nid=" + (remote_node_id ? std::to_string(static_cast(*remote_node_id)) : "?") + 39 | " tid=" + std::to_string(transfer_id) + " payload:\n" + util::makeHexDump(payload) + "\n"; 40 | } 41 | 42 | auto operator==(const Transfer& rhs) const -> bool 43 | { 44 | return (remote_node_id == rhs.remote_node_id) && (transfer_id == rhs.transfer_id) && 45 | std::equal(std::begin(payload), std::end(payload), std::begin(rhs.payload), std::end(rhs.payload)); 46 | } 47 | auto operator!=(const Transfer& rhs) const -> bool { return !operator==(rhs); } 48 | }; 49 | 50 | class Node : public kocherga::INode 51 | { 52 | public: 53 | enum class Output 54 | { 55 | ExecuteCommandResponse, 56 | NodeInfoResponse, 57 | FileReadRequest, 58 | HeartbeatMessage, 59 | LogRecordMessage, 60 | }; 61 | 62 | enum class Input 63 | { 64 | ExecuteCommandRequest, 65 | NodeInfoRequest, 66 | FileReadResponse, 67 | }; 68 | 69 | [[nodiscard]] auto getLastPollTime() const { return last_poll_at_; } 70 | 71 | [[nodiscard]] auto wasRequestCanceled() 72 | { 73 | const auto out = request_canceled_; 74 | request_canceled_ = false; 75 | return out; 76 | } 77 | 78 | void setFileReadResult(const bool value) { file_read_result_ = value; } 79 | 80 | /// Retrieve a previously received transfer under the specified session. 81 | /// Return an empty option if no such transfer was received since the last retrieval. 82 | /// The read is destructive -- the transfer is removed afterwards. 83 | [[nodiscard]] auto popOutput(const Output ses) -> std::optional 84 | { 85 | if (const auto it = outputs_.find(ses); it != std::end(outputs_)) 86 | { 87 | const Transfer ret = it->second; 88 | outputs_.erase(it); 89 | return ret; 90 | } 91 | return {}; 92 | } 93 | 94 | /// Store the transfer for reception when the next poll() is invoked. 95 | /// The invocation fails if such transfer is already pending. 96 | void pushInput(const Input ses, const Transfer& tr) 97 | { 98 | REQUIRE(inputs_.find(ses) == std::end(inputs_)); 99 | inputs_[ses] = tr; 100 | } 101 | 102 | private: 103 | void poll(kocherga::IReactor& reactor, const std::chrono::microseconds uptime) override 104 | { 105 | const auto proc = [&reactor, this](const Output ses, const kocherga::ServiceID service_id, const Transfer& tr) { 106 | std::vector buffer(kocherga::MaxSerializedRepresentationSize); 107 | // 108 | const auto size = reactor.processRequest(static_cast(service_id), 109 | *tr.remote_node_id, 110 | tr.payload.size(), 111 | tr.payload.data(), 112 | buffer.data()); 113 | if (size) 114 | { 115 | buffer.resize(*size); 116 | store(ses, Transfer(tr.transfer_id, buffer, *tr.remote_node_id)); 117 | } 118 | }; 119 | 120 | last_poll_at_ = uptime; 121 | for (auto [key, tr] : inputs_) 122 | { 123 | switch (key) 124 | { 125 | case Input::ExecuteCommandRequest: 126 | { 127 | proc(Output::ExecuteCommandResponse, kocherga::ServiceID::NodeExecuteCommand, tr); 128 | break; 129 | } 130 | case Input::NodeInfoRequest: 131 | { 132 | proc(Output::NodeInfoResponse, kocherga::ServiceID::NodeGetInfo, tr); 133 | break; 134 | } 135 | case Input::FileReadResponse: 136 | { 137 | reactor.processResponse(std::size(tr.payload), tr.payload.data()); 138 | break; 139 | } 140 | default: 141 | { 142 | FAIL("UNHANDLED CASE"); 143 | } 144 | } 145 | } 146 | inputs_.clear(); 147 | } 148 | 149 | [[nodiscard]] auto sendRequest(const kocherga::ServiceID service_id, 150 | const kocherga::NodeID server_node_id, 151 | const kocherga::TransferID transfer_id, 152 | const std::size_t payload_length, 153 | const std::uint8_t* const payload) -> bool override 154 | { 155 | switch (service_id) 156 | { 157 | case kocherga::ServiceID::FileRead: 158 | { 159 | store(Output::FileReadRequest, Transfer(transfer_id, payload_length, payload, server_node_id)); 160 | return file_read_result_; 161 | } 162 | case kocherga::ServiceID::NodeGetInfo: 163 | case kocherga::ServiceID::NodeExecuteCommand: 164 | default: 165 | { 166 | FAIL("UNEXPECTED SERVICE REQUEST"); 167 | return false; 168 | } 169 | } 170 | } 171 | 172 | void cancelRequest() override { request_canceled_ = true; } 173 | 174 | [[nodiscard]] auto publishMessage(const kocherga::SubjectID subject_id, 175 | const kocherga::TransferID transfer_id, 176 | const std::size_t payload_length, 177 | const std::uint8_t* const payload) -> bool override 178 | { 179 | bool out = false; 180 | switch (subject_id) 181 | { 182 | case kocherga::SubjectID::NodeHeartbeat: 183 | { 184 | store(Output::HeartbeatMessage, Transfer(transfer_id, payload_length, payload)); 185 | out = true; 186 | break; 187 | } 188 | case kocherga::SubjectID::DiagnosticRecord: 189 | { 190 | store(Output::LogRecordMessage, Transfer(transfer_id, payload_length, payload)); 191 | out = true; 192 | break; 193 | } 194 | case kocherga::SubjectID::PnPNodeIDAllocationData_v1: 195 | case kocherga::SubjectID::PnPNodeIDAllocationData_v2: 196 | default: 197 | { 198 | FAIL("UNEXPECTED MESSAGE"); 199 | break; 200 | } 201 | } 202 | return out; 203 | } 204 | 205 | void store(const Output ses, const Transfer& tr) 206 | { 207 | INFO("output: " << static_cast(ses)); 208 | INFO("transfer: " << tr.toString()); 209 | REQUIRE(outputs_.find(ses) == std::end(outputs_)); 210 | outputs_[ses] = tr; 211 | } 212 | 213 | std::chrono::microseconds last_poll_at_{}; 214 | bool file_read_result_ = true; 215 | bool request_canceled_ = false; 216 | std::map outputs_; 217 | std::map inputs_; 218 | }; 219 | 220 | } // namespace mock 221 | -------------------------------------------------------------------------------- /tests/unit/serial/test_node.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2021 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "kocherga_serial.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. 6 | #include "catch.hpp" 7 | #include 8 | #include 9 | 10 | namespace 11 | { 12 | class SerialPortMock : public kocherga::serial::ISerialPort 13 | { 14 | public: 15 | void pushRx(const kocherga::serial::detail::Transfer& transfer) 16 | { 17 | REQUIRE(kocherga::serial::detail::transmit( 18 | [this](const std::uint8_t bt) { 19 | rx_.push_back(bt); 20 | return true; 21 | }, 22 | transfer)); 23 | } 24 | 25 | /// The payload pointer of the returned transfer is invalidated on the 32-nd call to ISerialPort::send(). 26 | [[nodiscard]] auto popTx() -> std::optional 27 | { 28 | while (!tx_.empty()) 29 | { 30 | const auto bt = tx_.front(); 31 | tx_.pop_front(); 32 | if (const auto tr = stream_parser_.update(bt)) 33 | { 34 | return tr; 35 | } 36 | } 37 | return {}; 38 | } 39 | 40 | private: 41 | [[nodiscard]] auto receive() -> std::optional override 42 | { 43 | if (!rx_.empty()) 44 | { 45 | const auto out = rx_.front(); 46 | rx_.pop_front(); 47 | return out; 48 | } 49 | return {}; 50 | } 51 | 52 | [[nodiscard]] auto send(const std::uint8_t b) -> bool override 53 | { 54 | tx_.push_back(b); 55 | return true; 56 | } 57 | 58 | std::deque tx_; 59 | std::deque rx_; 60 | kocherga::serial::detail::StreamParser<1024> stream_parser_; 61 | }; 62 | 63 | class ReactorMock : public kocherga::IReactor 64 | { 65 | public: 66 | struct IncomingRequest 67 | { 68 | kocherga::PortID service_id = 0xFFFF; 69 | kocherga::NodeID client_node_id = 0xFFFF; 70 | std::vector data; 71 | }; 72 | 73 | /// Accepts request, returns the serialized response payload or nothing if no response should be sent. 74 | using IncomingRequestHandler = std::function>(IncomingRequest)>; 75 | 76 | void setIncomingRequestHandler(const IncomingRequestHandler& irh) { request_handler_ = irh; } 77 | 78 | [[nodiscard]] auto popPendingResponse() -> std::optional> 79 | { 80 | if (!pending_responses_.empty()) 81 | { 82 | const auto out = pending_responses_.front(); 83 | pending_responses_.pop_front(); 84 | return out; 85 | } 86 | return {}; 87 | } 88 | 89 | private: 90 | [[nodiscard]] auto processRequest(const kocherga::PortID service_id, 91 | const kocherga::NodeID client_node_id, 92 | const std::size_t request_length, 93 | const std::uint8_t* const request, 94 | std::uint8_t* const out_response) -> std::optional override 95 | { 96 | IncomingRequest ir{service_id, client_node_id, {}}; 97 | std::copy_n(request, request_length, std::back_insert_iterator(ir.data)); 98 | REQUIRE(request_handler_); 99 | if (const auto response_payload = request_handler_(ir)) 100 | { 101 | std::copy(response_payload->begin(), response_payload->end(), out_response); 102 | return response_payload->size(); 103 | } 104 | return {}; 105 | } 106 | 107 | void processResponse(const std::size_t response_length, const std::uint8_t* const response) override 108 | { 109 | pending_responses_.emplace_back(); 110 | std::copy_n(response, response_length, std::back_insert_iterator(pending_responses_.back())); 111 | } 112 | 113 | IncomingRequestHandler request_handler_; 114 | std::deque> pending_responses_; 115 | }; 116 | 117 | } // namespace 118 | 119 | TEST_CASE("kocherga_serial::SerialNode service") 120 | { 121 | SerialPortMock port; 122 | kocherga::serial::SerialNode node(port, {}); 123 | node.setLocalNodeID(2222); 124 | ReactorMock reactor; 125 | 126 | // Send a request transfer and then pop a response. 127 | { 128 | REQUIRE(static_cast(node).sendRequest(kocherga::ServiceID::NodeExecuteCommand, 129 | 1111, 130 | 0xCAFE'CAFE, 131 | 3, 132 | reinterpret_cast("\x03\x03\x03"))); 133 | const auto request = port.popTx(); 134 | REQUIRE(request); 135 | REQUIRE(request->meta.source == 2222); 136 | REQUIRE(request->meta.destination == 1111); 137 | REQUIRE(request->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | 138 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | 139 | kocherga::serial::detail::Transfer::Metadata::DataSpecRequestFlag)); 140 | REQUIRE(request->meta.transfer_id == 0xCAFE'CAFE); 141 | REQUIRE(request->payload_len == 3); 142 | REQUIRE(0 == std::memcmp(request->payload, "\x03\x03\x03", 3)); 143 | REQUIRE(!port.popTx()); // Ensure no extra requests are sent. 144 | // Ensure non-matching responses are simply ignored. 145 | { 146 | kocherga::serial::detail::Transfer response; 147 | // BAD SOURCE 148 | response.meta.source = 1110; 149 | response.meta.destination = 2222; 150 | response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | 151 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; 152 | response.meta.transfer_id = 0xCAFE'CAFE; 153 | port.pushRx(response); 154 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 155 | REQUIRE(!reactor.popPendingResponse()); 156 | // BAD DESTINATION 157 | response.meta.source = 1111; 158 | response.meta.destination = 2221; 159 | response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | 160 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; 161 | response.meta.transfer_id = 0xCAFE'CAFE; 162 | port.pushRx(response); 163 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 164 | REQUIRE(!reactor.popPendingResponse()); 165 | // BAD DATA SPEC 166 | response.meta.source = 1111; 167 | response.meta.destination = 2222; 168 | response.meta.data_spec = static_cast(kocherga::ServiceID::NodeGetInfo) | 169 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; 170 | response.meta.transfer_id = 0xCAFE'CAFE; 171 | port.pushRx(response); 172 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 173 | REQUIRE(!reactor.popPendingResponse()); 174 | // BAD TRANSFER ID 175 | response.meta.source = 1111; 176 | response.meta.destination = 2222; 177 | response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | 178 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; 179 | response.meta.transfer_id = 0xCAFE'CAFA; 180 | port.pushRx(response); 181 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 182 | REQUIRE(!reactor.popPendingResponse()); 183 | } 184 | // Receive the matching response transfer. 185 | kocherga::serial::detail::Transfer response; 186 | response.meta.source = 1111; 187 | response.meta.destination = 2222; 188 | response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | 189 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; 190 | response.meta.transfer_id = 0xCAFE'CAFE; 191 | response.payload_len = 6; 192 | response.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); 193 | port.pushRx(response); // First response shall be processed. 194 | port.pushRx(response); // Deterministic data loss mitigation; shall be ignored. 195 | port.pushRx(response); // Same here. 196 | static_cast(node).poll(reactor, std::chrono::microseconds(9'000)); 197 | const auto rp = reactor.popPendingResponse(); 198 | REQUIRE(rp); 199 | REQUIRE(*rp == std::vector{5, 4, 3, 2, 1, 0}); 200 | REQUIRE(!reactor.popPendingResponse()); // Ensure duplicates are removed. 201 | } 202 | 203 | // Send a request multiple times emulating the deterministic data loss mitigation, ensure repetitions are ignored. 204 | { 205 | kocherga::serial::detail::Transfer request; 206 | request.meta.source = 1111; 207 | request.meta.destination = 2222; 208 | request.meta.data_spec = 209 | static_cast(kocherga::ServiceID::NodeExecuteCommand) | 210 | static_cast(kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | 211 | kocherga::serial::detail::Transfer::Metadata::DataSpecRequestFlag); 212 | request.meta.transfer_id = 0xCAFE'BABE; 213 | request.payload_len = 6; 214 | request.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); 215 | port.pushRx(request); // First request shall be processed. 216 | port.pushRx(request); // Deterministic data loss mitigation; shall be ignored. 217 | port.pushRx(request); // Same here. 218 | auto num_requests = 0; 219 | reactor.setIncomingRequestHandler( 220 | [&num_requests](const ReactorMock::IncomingRequest& ir) -> std::optional> { 221 | num_requests++; 222 | REQUIRE(ir.service_id == static_cast(kocherga::ServiceID::NodeExecuteCommand)); 223 | REQUIRE(ir.client_node_id == 1111); 224 | REQUIRE(ir.data == std::vector{5, 4, 3, 2, 1, 0}); 225 | return std::vector{1, 2, 3, 4, 5}; 226 | }); 227 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 228 | REQUIRE(num_requests == 1); // Ensure extras are ignored. 229 | const auto response = port.popTx(); 230 | REQUIRE(response); 231 | REQUIRE(response->meta.source == 2222); 232 | REQUIRE(response->meta.destination == 1111); 233 | REQUIRE(response->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | 234 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag)); 235 | REQUIRE(response->meta.transfer_id == 0xCAFE'BABE); 236 | REQUIRE(response->payload_len == 5); 237 | REQUIRE(0 == std::memcmp(response->payload, "\x01\x02\x03\x04\x05", 5)); 238 | REQUIRE(!port.popTx()); // Ensure no extra responses are sent. 239 | } 240 | 241 | // Ensure transfer-ID timeouts are handled properly -- repetitions separated by a long time interval are accepted. 242 | { 243 | kocherga::serial::detail::Transfer request; 244 | request.meta.source = 3210; 245 | request.meta.destination = 2222; 246 | request.meta.data_spec = 247 | static_cast(kocherga::ServiceID::NodeExecuteCommand) | 248 | static_cast(kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | 249 | kocherga::serial::detail::Transfer::Metadata::DataSpecRequestFlag); 250 | request.meta.transfer_id = 0xBABA'CACA; 251 | request.payload_len = 6; 252 | request.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); 253 | port.pushRx(request); // First request shall be processed. 254 | port.pushRx(request); // Deterministic data loss mitigation; shall be ignored. 255 | port.pushRx(request); // Same here. 256 | auto num_requests = 0; 257 | reactor.setIncomingRequestHandler( 258 | [&num_requests](const ReactorMock::IncomingRequest& ir) -> std::optional> { 259 | num_requests++; 260 | REQUIRE(ir.service_id == static_cast(kocherga::ServiceID::NodeExecuteCommand)); 261 | REQUIRE(ir.client_node_id == 3210); 262 | REQUIRE(ir.data == std::vector{5, 4, 3, 2, 1, 0}); 263 | return std::vector{1, 2, 3, 4, 5}; 264 | }); 265 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 266 | REQUIRE(num_requests == 1); // Ensure extras are ignored. 267 | auto response = port.popTx(); 268 | REQUIRE(response); 269 | REQUIRE(response->meta.source == 2222); 270 | REQUIRE(response->meta.destination == 3210); 271 | REQUIRE(response->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | 272 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag)); 273 | REQUIRE(response->meta.transfer_id == 0xBABA'CACA); 274 | REQUIRE(response->payload_len == 5); 275 | REQUIRE(0 == std::memcmp(response->payload, "\x01\x02\x03\x04\x05", 5)); 276 | REQUIRE(!port.popTx()); // Ensure no extra responses are sent. 277 | // Now, we send the very same transfer but a long time has passed so it is accepted anyway. 278 | port.pushRx(request); 279 | port.pushRx(request); 280 | static_cast(node).poll(reactor, std::chrono::microseconds(100'000'000)); 281 | REQUIRE(num_requests == 2); // Ensure extras are ignored. 282 | response = port.popTx(); 283 | REQUIRE(response); 284 | REQUIRE(response->meta.source == 2222); 285 | REQUIRE(response->meta.destination == 3210); 286 | REQUIRE(response->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | 287 | kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag)); 288 | REQUIRE(response->meta.transfer_id == 0xBABA'CACA); 289 | REQUIRE(response->payload_len == 5); 290 | REQUIRE(0 == std::memcmp(response->payload, "\x01\x02\x03\x04\x05", 5)); 291 | REQUIRE(!port.popTx()); // Ensure no extra responses are sent. 292 | } 293 | 294 | // Send an unknown request transfer, ensure no response is sent back. 295 | { 296 | kocherga::serial::detail::Transfer request; 297 | request.meta.source = 3333; 298 | request.meta.destination = 2222; 299 | request.meta.data_spec = 300 | static_cast(kocherga::ServiceID::NodeExecuteCommand) | 301 | static_cast(kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | 302 | kocherga::serial::detail::Transfer::Metadata::DataSpecRequestFlag); 303 | request.meta.transfer_id = 0xBABA'BABA; 304 | request.payload_len = 6; 305 | request.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); 306 | port.pushRx(request); // First request shall be processed. 307 | port.pushRx(request); // Deterministic data loss mitigation; shall be ignored. 308 | port.pushRx(request); // Same here. 309 | auto num_requests = 0; 310 | reactor.setIncomingRequestHandler( 311 | [&num_requests](const ReactorMock::IncomingRequest& ir) -> std::optional> { 312 | num_requests++; 313 | REQUIRE(ir.service_id == static_cast(kocherga::ServiceID::NodeExecuteCommand)); 314 | REQUIRE(ir.client_node_id == 3333); 315 | REQUIRE(ir.data == std::vector{5, 4, 3, 2, 1, 0}); 316 | return {}; // Return no response. 317 | }); 318 | static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); 319 | REQUIRE(num_requests == 1); // Ensure extras are ignored. 320 | REQUIRE(!port.popTx()); // Ensure no responses are sent. 321 | } 322 | } 323 | -------------------------------------------------------------------------------- /tests/unit/test_app_locator.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. 6 | #include "catch.hpp" 7 | #include "util.hpp" 8 | #include 9 | 10 | TEST_CASE("AppLocator-good-simple") 11 | { 12 | const util::FileROMBackend rom( 13 | util::getImagePath("good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin")); 14 | 15 | const kocherga::detail::AppLocator loc_ok(rom, 1024U); 16 | 17 | auto info = loc_ok.identifyApplication(); 18 | REQUIRE(info); 19 | REQUIRE(info->image_size == 144); 20 | REQUIRE(info->image_crc == 0x452A'4267'971A'3928ULL); 21 | REQUIRE(info->timestamp_utc == 1234567890); 22 | REQUIRE(info->vcs_revision_id == 0xBADC'0FFE'E0DD'F00DULL); 23 | REQUIRE(info->isReleaseBuild()); 24 | REQUIRE(!info->isDirtyBuild()); 25 | REQUIRE(info->version.at(0) == 3); 26 | REQUIRE(info->version.at(1) == 1); 27 | 28 | const kocherga::detail::AppLocator loc_too_small(rom, 64U); 29 | REQUIRE(!loc_too_small.identifyApplication()); 30 | } 31 | 32 | TEST_CASE("AppLocator-good-3rd-entry") 33 | { 34 | const util::FileROMBackend rom( 35 | util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin")); 36 | 37 | const kocherga::detail::AppLocator loc_ok(rom, 1024U); 38 | 39 | auto info = loc_ok.identifyApplication(); 40 | REQUIRE(info); 41 | REQUIRE(info->image_size == 576); 42 | REQUIRE(info->image_crc == 0x8B61'938E'E5F9'0B1FULL); 43 | REQUIRE(info->timestamp_utc == 1234567890); 44 | REQUIRE(info->vcs_revision_id == 0x3333'3333'3333'3333ULL); 45 | REQUIRE(!info->isReleaseBuild()); 46 | REQUIRE(info->isDirtyBuild()); 47 | REQUIRE(info->version.at(0) == 5); 48 | REQUIRE(info->version.at(1) == 6); 49 | 50 | const kocherga::detail::AppLocator loc_too_small(rom, 64U); 51 | REQUIRE(!loc_too_small.identifyApplication()); 52 | } 53 | 54 | TEST_CASE("AppLocator-bad-crc-x3") 55 | { 56 | const util::FileROMBackend rom(util::getImagePath("bad-le-crc-x3.bin")); 57 | REQUIRE(!kocherga::detail::AppLocator(rom, 1024U).identifyApplication()); 58 | REQUIRE(!kocherga::detail::AppLocator(rom, 100U).identifyApplication()); 59 | } 60 | 61 | TEST_CASE("AppLocator-bad-short") 62 | { 63 | const util::FileROMBackend rom(util::getImagePath("bad-le-short.bin")); 64 | REQUIRE(!kocherga::detail::AppLocator(rom, 1024U).identifyApplication()); 65 | REQUIRE(!kocherga::detail::AppLocator(rom, 10U).identifyApplication()); 66 | } 67 | 68 | TEST_CASE("AppLocator-legacy") 69 | { 70 | const util::FileROMBackend rom(util::getImagePath("com.zubax.telega-1-0.3.68620b82.application.bin")); 71 | 72 | const kocherga::detail::AppLocator loc_ok(rom, 512UL * 1024U); 73 | 74 | REQUIRE(!loc_ok.identifyApplication()); 75 | auto info = loc_ok.identifyApplication(true); 76 | REQUIRE(info); 77 | REQUIRE(info->image_size == 271'440U); 78 | REQUIRE(info->image_crc == 0x9EA4'7D98'DCC7'B58AULL); 79 | REQUIRE(info->version.at(0) == 0); 80 | REQUIRE(info->version.at(1) == 3); 81 | 82 | const kocherga::detail::AppLocator loc_too_small(rom, 128UL * 1024U); 83 | REQUIRE(!loc_too_small.identifyApplication()); 84 | } 85 | -------------------------------------------------------------------------------- /tests/unit/test_core.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. 6 | #include "catch.hpp" 7 | #include "mock.hpp" 8 | #include 9 | 10 | namespace 11 | { 12 | inline auto getSysInfo() 13 | { 14 | static constexpr auto coa_capacity = 222U; 15 | std::array coa{}; 16 | for (auto i = 0U; i < coa_capacity; i++) 17 | { 18 | coa.at(i) = static_cast(coa.size() - i); 19 | } 20 | return kocherga::SystemInfo{ 21 | kocherga::SemanticVersion{33, 11}, 22 | {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}, 23 | "com.zubax.kocherga.test.core", 24 | 222, 25 | coa.data(), 26 | }; 27 | } 28 | 29 | template 30 | inline auto checkHeartbeat(std::array& nodes, 31 | const std::optional tid, 32 | const std::uint32_t uptime, 33 | const kocherga::detail::dsdl::Heartbeat::Health health, 34 | const std::uint8_t vssc) -> bool 35 | { 36 | using kocherga::detail::dsdl::Heartbeat; 37 | for (auto& n : nodes) 38 | { 39 | const mock::Transfer tr = *n.popOutput(mock::Node::Output::HeartbeatMessage); 40 | const mock::Transfer ref(tid ? *tid : tr.transfer_id, 41 | { 42 | static_cast(uptime >> 0U), 43 | static_cast(uptime >> 8U), 44 | static_cast(uptime >> 16U), 45 | static_cast(uptime >> 24U), 46 | static_cast(health), 47 | static_cast(Heartbeat::ModeSoftwareUpdate), 48 | static_cast(vssc), 49 | }); 50 | if (tr != ref) 51 | { 52 | std::cout << ref.toString() << tr.toString() << std::endl; 53 | return false; 54 | } 55 | } 56 | return true; 57 | } 58 | 59 | } // namespace 60 | 61 | TEST_CASE("Bootloader-fast-boot") 62 | { 63 | using std::chrono_literals::operator""ms; 64 | 65 | const auto sys = getSysInfo(); 66 | const auto img = util::getImagePath("good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin"); 67 | util::FileROMBackend rom(img); 68 | std::array nodes; 69 | kocherga::Bootloader::Params params; 70 | params.max_app_size = static_cast(std::filesystem::file_size(img)); 71 | params.linger = false; 72 | kocherga::Bootloader bl(rom, sys, params); 73 | REQUIRE(bl.addNode(&nodes.at(0))); 74 | REQUIRE(bl.addNode(&nodes.at(1))); 75 | REQUIRE(bl.addNode(&nodes.at(2))); 76 | REQUIRE(!bl.addNode(&nodes.at(2))); // Double registration has no effect. 77 | REQUIRE(bl.getNumberOfNodes() == 3); 78 | 79 | REQUIRE(bl.poll(500ms) == kocherga::Final::BootApp); 80 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 81 | 82 | auto ai = *bl.getAppInfo(); 83 | REQUIRE(0x452A'4267'971A'3928ULL == ai.image_crc); 84 | REQUIRE(0xBADC'0FFE'E0DD'F00DULL == ai.vcs_revision_id); 85 | REQUIRE(3 == ai.version.at(0)); 86 | REQUIRE(1 == ai.version.at(1)); 87 | REQUIRE(ai.isReleaseBuild()); 88 | REQUIRE(!ai.isDirtyBuild()); 89 | } 90 | 91 | TEST_CASE("Bootloader-boot-delay") 92 | { 93 | using std::chrono_literals::operator""s; 94 | using std::chrono_literals::operator""ms; 95 | 96 | const auto sys = getSysInfo(); 97 | const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); 98 | util::FileROMBackend rom(img); 99 | mock::Node node; 100 | kocherga::Bootloader::Params params; 101 | params.max_app_size = static_cast(std::filesystem::file_size(img)); 102 | params.linger = false; 103 | params.boot_delay = 1s; 104 | kocherga::Bootloader bl(rom, sys, params); 105 | REQUIRE(bl.addNode(&node)); 106 | 107 | REQUIRE(!bl.poll(500ms)); 108 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 109 | 110 | auto ai = *bl.getAppInfo(); 111 | REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); 112 | REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); 113 | REQUIRE(5 == ai.version.at(0)); 114 | REQUIRE(6 == ai.version.at(1)); 115 | REQUIRE(!ai.isReleaseBuild()); 116 | REQUIRE(ai.isDirtyBuild()); 117 | 118 | REQUIRE(!bl.poll(900ms)); 119 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 120 | 121 | REQUIRE(bl.poll(1900ms) == kocherga::Final::BootApp); 122 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 123 | } 124 | 125 | TEST_CASE("Bootloader-linger-reboot") 126 | { 127 | using std::chrono_literals::operator""ms; 128 | using mock::Node; 129 | using mock::Transfer; 130 | 131 | const auto sys = getSysInfo(); 132 | const auto img = util::getImagePath("good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin"); 133 | util::FileROMBackend rom(img); 134 | std::array nodes; 135 | kocherga::Bootloader::Params params; 136 | params.max_app_size = static_cast(std::filesystem::file_size(img)); 137 | params.linger = true; 138 | kocherga::Bootloader bl(rom, sys, params); 139 | REQUIRE(bl.addNode(&nodes.at(0))); 140 | REQUIRE(bl.addNode(&nodes.at(1))); 141 | 142 | REQUIRE(!bl.poll(500ms)); 143 | REQUIRE(bl.getState() == kocherga::State::BootCanceled); // LINGER -- NO BOOT 144 | 145 | auto ai = *bl.getAppInfo(); 146 | REQUIRE(0x452A'4267'971A'3928ULL == ai.image_crc); 147 | REQUIRE(0xBADC'0FFE'E0DD'F00DULL == ai.vcs_revision_id); 148 | REQUIRE(3 == ai.version.at(0)); 149 | REQUIRE(1 == ai.version.at(1)); 150 | REQUIRE(ai.isReleaseBuild()); 151 | REQUIRE(!ai.isDirtyBuild()); 152 | 153 | nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, Transfer(444, {255, 255, 0}, 2222)); 154 | REQUIRE(bl.poll(600ms) == kocherga::Final::Restart); 155 | REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); 156 | REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == 157 | Transfer(444, {0, 0, 0, 0, 0, 0, 0}, 2222)); 158 | } 159 | 160 | TEST_CASE("Bootloader-update-valid") 161 | { 162 | using std::chrono_literals::operator""ms; 163 | using mock::Node; 164 | using mock::Transfer; 165 | using kocherga::detail::dsdl::Heartbeat; 166 | 167 | const auto sys = getSysInfo(); 168 | const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); 169 | REQUIRE(std::filesystem::copy_file(img, "rom.img.tmp", std::filesystem::copy_options::overwrite_existing)); 170 | util::FileROMBackend rom("rom.img.tmp"); 171 | std::array nodes; 172 | kocherga::Bootloader::Params params; 173 | params.max_app_size = static_cast(std::filesystem::file_size(img)); 174 | params.linger = true; 175 | kocherga::Bootloader bl(rom, sys, params); 176 | REQUIRE(bl.addNode(&nodes.at(0))); 177 | REQUIRE(bl.addNode(&nodes.at(1))); 178 | 179 | REQUIRE(!bl.poll(1'500ms)); 180 | REQUIRE(bl.getState() == kocherga::State::BootCanceled); 181 | REQUIRE(checkHeartbeat(nodes, 0, 1, Heartbeat::Health::Advisory, 0)); 182 | 183 | auto ai = *bl.getAppInfo(); 184 | REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); 185 | REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); 186 | REQUIRE(5 == ai.version.at(0)); 187 | REQUIRE(6 == ai.version.at(1)); 188 | REQUIRE(!ai.isReleaseBuild()); 189 | REQUIRE(ai.isDirtyBuild()); 190 | 191 | // REQUEST UPDATE 192 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( 193 | // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_BEGIN_SOFTWARE_UPDATE, 194 | // 'good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin')))) 195 | nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, 196 | Transfer(444, 197 | {253, 255, 69, 103, 111, 111, 100, 45, 108, 101, 45, 51, 114, 100, 45, 198 | 101, 110, 116, 114, 121, 45, 53, 46, 54, 46, 51, 51, 51, 51, 51, 199 | 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, 46, 56, 98, 54, 200 | 49, 57, 51, 56, 101, 101, 53, 102, 57, 48, 98, 49, 102, 46, 97, 201 | 112, 112, 46, 100, 105, 114, 116, 121, 46, 98, 105, 110}, 202 | 0)); 203 | REQUIRE(!bl.poll(2'100ms)); 204 | REQUIRE(checkHeartbeat(nodes, 1, 2, Heartbeat::Health::Nominal, 0)); 205 | REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); 206 | REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == Transfer(444, {0, 0, 0, 0, 0, 0, 0}, 0)); 207 | 208 | // FIRST READ REQUEST 209 | REQUIRE(!bl.poll(2'200ms)); 210 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 211 | REQUIRE(nodes.at(1).popOutput(Node::Output::LogRecordMessage)); 212 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 213 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.file.Read_1_1.Request(0, 214 | // uavcan.file.Path_2_0('good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin'))))) 215 | const auto received = *nodes.at(1).popOutput(Node::Output::FileReadRequest); 216 | const auto reference = 217 | Transfer(1, 218 | {0, 0, 0, 0, 0, 69, 103, 111, 111, 100, 45, 108, 101, 45, 51, 114, 100, 45, 101, 219 | 110, 116, 114, 121, 45, 53, 46, 54, 46, 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, 220 | 51, 51, 51, 51, 51, 51, 46, 56, 98, 54, 49, 57, 51, 56, 101, 101, 53, 102, 57, 221 | 48, 98, 49, 102, 46, 97, 112, 112, 46, 100, 105, 114, 116, 121, 46, 98, 105, 110}, 222 | 0); 223 | std::cout << received.toString() << reference.toString() << std::endl; 224 | REQUIRE(received == reference); 225 | 226 | // READ RESPONSE 227 | // The serialized representation was constructed manually from the binary file 228 | nodes.at(1).pushInput(Node::Input::FileReadResponse, 229 | Transfer(0, 230 | {0, 0, 128, 0, 72, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100, 63, 32, 231 | 32, 32, 32, 199, 196, 192, 111, 20, 21, 68, 94, 65, 80, 68, 101, 115, 99, 232 | 48, 48, 124, 194, 145, 71, 22, 198, 82, 134, 64, 2, 0, 0, 0, 0, 0, 233 | 0, 1, 16, 0, 0, 210, 2, 150, 73, 13, 240, 221, 224, 254, 15, 220, 186, 234 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 235 | 0, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 236 | 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 237 | 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126}, 238 | 0)); 239 | (void) bl.poll(2'300ms); // Results will appear on the SECOND poll. 240 | REQUIRE(kocherga::Final::BootApp == *bl.poll(2'400ms)); 241 | // Completed here. 242 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 243 | REQUIRE(kocherga::Final::BootApp == *bl.poll(2'500ms)); // All subsequent calls yield the same Final. 244 | REQUIRE(kocherga::Final::BootApp == *bl.poll(2'600ms)); // Yep. 245 | 246 | // NEW APPLICATION IS NOW AVAILABLE 247 | ai = *bl.getAppInfo(); 248 | REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); 249 | REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); 250 | REQUIRE(5 == ai.version.at(0)); 251 | REQUIRE(6 == ai.version.at(1)); 252 | REQUIRE(!ai.isReleaseBuild()); 253 | REQUIRE(ai.isDirtyBuild()); 254 | } 255 | 256 | TEST_CASE("Bootloader-update-invalid") // NOLINT NOSONAR complexity threshold 257 | { 258 | using std::chrono_literals::operator""s; 259 | using std::chrono_literals::operator""ms; 260 | using mock::Node; 261 | using mock::Transfer; 262 | using kocherga::detail::dsdl::Heartbeat; 263 | 264 | const auto sys = getSysInfo(); 265 | const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); 266 | REQUIRE(std::filesystem::copy_file(img, "rom.img.tmp", std::filesystem::copy_options::overwrite_existing)); 267 | util::FileROMBackend rom("rom.img.tmp"); 268 | std::array nodes; 269 | kocherga::Bootloader::Params params; 270 | params.max_app_size = static_cast(std::filesystem::file_size(img)); 271 | params.linger = false; 272 | params.boot_delay = 2s; 273 | params.request_retry_limit = 0; 274 | kocherga::Bootloader bl(rom, sys, params); 275 | REQUIRE(bl.addNode(&nodes.at(0))); 276 | 277 | REQUIRE(!bl.poll(1'100ms)); 278 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 279 | REQUIRE(checkHeartbeat(nodes, 0, 1, Heartbeat::Health::Nominal, 0)); 280 | 281 | auto ai = *bl.getAppInfo(); 282 | REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); 283 | REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); 284 | REQUIRE(5 == ai.version.at(0)); 285 | REQUIRE(6 == ai.version.at(1)); 286 | REQUIRE(!ai.isReleaseBuild()); 287 | REQUIRE(ai.isDirtyBuild()); 288 | 289 | // REQUEST UPDATE 290 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( 291 | // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_BEGIN_SOFTWARE_UPDATE, 'bad-le-crc-x3.bin')))) 292 | nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, 293 | Transfer(111, 294 | {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, 295 | 99, 114, 99, 45, 120, 51, 46, 98, 105, 110}, 296 | 1111)); 297 | REQUIRE(!bl.poll(1'600ms)); 298 | REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == 299 | Transfer(111, {0, 0, 0, 0, 0, 0, 0}, 1111)); 300 | 301 | REQUIRE(!bl.poll(2'100ms)); 302 | REQUIRE(checkHeartbeat(nodes, 1, 2, Heartbeat::Health::Nominal, 1)); 303 | 304 | // FIRST READ REQUEST 305 | REQUIRE(!bl.poll(2'200ms)); 306 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 307 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.file.Read_1_1.Request(0, 308 | // uavcan.file.Path_2_0('bad-le-crc-x3.bin'))))) 309 | auto received = *nodes.at(0).popOutput(Node::Output::FileReadRequest); 310 | auto reference = 311 | Transfer(1, 312 | {0, 0, 0, 0, 0, 17, 98, 97, 100, 45, 108, 101, 45, 99, 114, 99, 45, 120, 51, 46, 98, 105, 110}, 313 | 1111); 314 | std::cout << received.toString() << reference.toString() << std::endl; 315 | REQUIRE(received == reference); 316 | 317 | // READ RESPONSE 318 | // The serialized representation was constructed manually from the binary file: bad-le-crc-x3.bin 319 | nodes.at(0).pushInput(Node::Input::FileReadResponse, 320 | Transfer(0, 321 | { 322 | 0x00, 0x00, 0x00, 0x01, 0x48, 0x65, 0x6c, 0x6c, 0x6f, 0x20, 0x77, 0x6f, 0x72, 323 | 0x6c, 0x64, 0x3f, 0x20, 0x20, 0x20, 0x20, 0xc7, 0xc4, 0xc0, 0x6f, 0x14, 0x15, 324 | 0x44, 0x5e, 0x88, 0x7a, 0x2e, 0xd0, 0x7e, 0xb1, 0x8c, 0xbe, 0xff, 0xff, 0xff, 325 | 0xff, 0xff, 0x00, 0x00, 0x00, 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, 326 | 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 327 | 0x00, 0x01, 0x10, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 328 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 329 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 330 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x48, 331 | 0x65, 0x6c, 0x6c, 0x6f, 0x20, 0x77, 0x6f, 0x72, 0x6c, 0x64, 0x3f, 0x20, 0x20, 332 | 0x20, 0x20, 0xc7, 0xc4, 0xc0, 0x6f, 0x14, 0x15, 0x44, 0x5e, 0x88, 0x7a, 0x2e, 333 | 0xd0, 0x7e, 0xb1, 0x8c, 0xbe, 0x68, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 334 | 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, 0x00, 0x00, 0x00, 0x00, 0x01, 335 | 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x00, 336 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 337 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 338 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 339 | 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x48, 0x65, 0x6c, 0x6c, 0x6f, 0x20, 340 | 0x77, 0x6f, 0x72, 0x6c, 0x64, 0x3f, 0x20, 0x20, 0x20, 0x20, 0xc7, 0xc4, 0xc0, 341 | 0x6f, 0x14, 0x15, 0x44, 0x5e, 0x88, 0x7a, 0x2e, 0xd0, 0x7e, 0xb1, 0x8c, 0xbe, 342 | }, 343 | 1111)); 344 | REQUIRE(!bl.poll(3'100ms)); 345 | REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); 346 | REQUIRE(checkHeartbeat(nodes, 2, 3, Heartbeat::Health::Nominal, 1)); 347 | REQUIRE(!bl.poll(3'200ms)); 348 | REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); 349 | 350 | // SECOND READ REQUEST 351 | REQUIRE(!bl.poll(3'300ms)); 352 | received = nodes.at(0).popOutput(Node::Output::FileReadRequest).value(); 353 | reference = Transfer(2, 354 | {0, 1, 0, 0, 0, 17, 98, 97, 100, 45, 108, 101, 45, 99, 114, 99, 45, 120, 51, 46, 98, 105, 110}, 355 | 1111); 356 | std::cout << received.toString() << reference.toString() << std::endl; 357 | REQUIRE(received == reference); 358 | 359 | // READ TIMEOUT, NO VALID APPLICATION TO BOOT 360 | // Due to the large time leap, we will be getting a heartbeat every poll() from now on until the leap is corrected. 361 | REQUIRE(!bl.poll(10'100ms)); 362 | REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); 363 | REQUIRE(checkHeartbeat(nodes, 3, 10, Heartbeat::Health::Warning, 0)); 364 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 365 | 366 | // RESPONSE ERROR CODE 367 | nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, 368 | Transfer(112, 369 | {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, 370 | 99, 114, 99, 45, 120, 51, 46, 105, 109, 103}, 371 | 2222)); 372 | REQUIRE(!bl.poll(10'200ms)); 373 | REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == 374 | Transfer(112, {0, 0, 0, 0, 0, 0, 0}, 2222)); 375 | REQUIRE(checkHeartbeat(nodes, 4, 10, Heartbeat::Health::Nominal, 0)); 376 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 377 | nodes.at(0).pushInput(Node::Input::FileReadResponse, Transfer(2, {0xAD, 0xDE, 0x00, 0x00}, 2222)); 378 | REQUIRE(!bl.poll(10'300ms)); 379 | REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); 380 | REQUIRE(checkHeartbeat(nodes, 5, 10, Heartbeat::Health::Warning, 0)); 381 | REQUIRE(nodes.at(0).popOutput(Node::Output::FileReadRequest)); // Not checked 382 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 383 | 384 | // SEND REQUEST FAILURE 385 | nodes.at(0).setFileReadResult(false); 386 | nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, 387 | Transfer(113, 388 | {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, 389 | 99, 114, 99, 45, 120, 51, 46, 105, 109, 103}, 390 | 3333)); 391 | REQUIRE(!bl.poll(10'400ms)); 392 | REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == 393 | Transfer(113, {0, 0, 0, 0, 0, 0, 0}, 3333)); 394 | REQUIRE(checkHeartbeat(nodes, 6, 10, Heartbeat::Health::Nominal, 0)); 395 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 396 | REQUIRE(!bl.poll(10'500ms)); 397 | REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); 398 | (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); 399 | REQUIRE(nodes.at(0).popOutput(Node::Output::FileReadRequest)); // Not checked 400 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 401 | 402 | // ROM WRITE FAILURE 403 | nodes.at(0).setFileReadResult(true); 404 | rom.enableFailureInjection(true); 405 | nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, 406 | Transfer(114, 407 | {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, 408 | 99, 114, 99, 45, 120, 51, 46, 105, 109, 103}, 409 | 3210)); 410 | REQUIRE(!bl.poll(10'600ms)); 411 | (void) nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse); 412 | (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); 413 | (void) nodes.at(0).popOutput(Node::Output::LogRecordMessage); 414 | REQUIRE(!bl.poll(10'700ms)); 415 | REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); 416 | (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); 417 | (void) nodes.at(0).popOutput(Node::Output::LogRecordMessage); 418 | REQUIRE(nodes.at(0).popOutput(Node::Output::FileReadRequest)); 419 | nodes.at(0).pushInput(Node::Input::FileReadResponse, 420 | Transfer(4, 421 | {0x00, 0x00, 0x09, 0x00, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa}, 422 | 3210)); 423 | REQUIRE(!bl.poll(10'800ms)); 424 | (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); 425 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 426 | REQUIRE(!bl.poll(11'100ms)); 427 | REQUIRE(checkHeartbeat(nodes, {}, 11, Heartbeat::Health::Warning, 0)); 428 | REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); 429 | REQUIRE(!bl.getAppInfo()); 430 | } 431 | 432 | TEST_CASE("Bootloader-trigger") 433 | { 434 | using std::chrono_literals::operator""s; 435 | using std::chrono_literals::operator""ms; 436 | using mock::Node; 437 | using mock::Transfer; 438 | using kocherga::detail::dsdl::Heartbeat; 439 | 440 | const auto sys = getSysInfo(); 441 | const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); 442 | REQUIRE(std::filesystem::copy_file(img, "rom.img.tmp", std::filesystem::copy_options::overwrite_existing)); 443 | util::FileROMBackend rom("rom.img.tmp"); 444 | std::array nodes; 445 | kocherga::Bootloader::Params params; 446 | params.max_app_size = static_cast(std::filesystem::file_size(img)); 447 | params.linger = false; 448 | params.boot_delay = 1s; 449 | kocherga::Bootloader bl(rom, sys, params); 450 | REQUIRE(bl.addNode(&nodes.at(0))); 451 | REQUIRE(bl.addNode(&nodes.at(1))); 452 | REQUIRE(bl.addNode(&nodes.at(2))); 453 | REQUIRE(!bl.addNode(&nodes.at(2))); // Double registration has no effect. 454 | 455 | REQUIRE(!bl.poll(100ms)); 456 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 457 | 458 | auto ai = *bl.getAppInfo(); 459 | REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); 460 | REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); 461 | REQUIRE(5 == ai.version.at(0)); 462 | REQUIRE(6 == ai.version.at(1)); 463 | REQUIRE(!ai.isReleaseBuild()); 464 | REQUIRE(ai.isDirtyBuild()); 465 | 466 | // MANUAL UPDATE TRIGGER 467 | const auto* const path = 468 | reinterpret_cast("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); 469 | REQUIRE(bl.trigger(2, 2222, 69, path)); 470 | REQUIRE(!bl.trigger(222, 2222, 69, path)); // No such node 471 | REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); 472 | REQUIRE(!bl.poll(1'100ms)); 473 | REQUIRE(checkHeartbeat(nodes, 0, 1, Heartbeat::Health::Nominal, 1)); 474 | 475 | // FIRST READ REQUEST, FIRST ATTEMPT 476 | REQUIRE(!bl.poll(1'200ms)); 477 | REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 478 | REQUIRE(nodes.at(1).popOutput(Node::Output::LogRecordMessage)); 479 | REQUIRE(nodes.at(2).popOutput(Node::Output::LogRecordMessage)); 480 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 481 | REQUIRE(!nodes.at(1).popOutput(Node::Output::FileReadRequest)); 482 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.file.Read_1_1.Request(0, 483 | // uavcan.file.Path_2_0('good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin'))))) 484 | auto received = nodes.at(2).popOutput(Node::Output::FileReadRequest).value(); 485 | auto reference = 486 | Transfer(1, 487 | {0, 0, 0, 0, 0, 69, 103, 111, 111, 100, 45, 108, 101, 45, 51, 114, 100, 45, 101, 488 | 110, 116, 114, 121, 45, 53, 46, 54, 46, 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, 489 | 51, 51, 51, 51, 51, 51, 46, 56, 98, 54, 49, 57, 51, 56, 101, 101, 53, 102, 57, 490 | 48, 98, 49, 102, 46, 97, 112, 112, 46, 100, 105, 114, 116, 121, 46, 98, 105, 110}, 491 | 2222); 492 | std::cout << received.toString() << reference.toString() << std::endl; 493 | REQUIRE(received == reference); 494 | 495 | // FIRST READ REQUEST, SECOND ATTEMPT 496 | REQUIRE(!bl.poll(6'000ms)); 497 | REQUIRE(nodes.at(0).popOutput(Node::Output::HeartbeatMessage)); 498 | REQUIRE(nodes.at(1).popOutput(Node::Output::HeartbeatMessage)); 499 | REQUIRE(nodes.at(2).popOutput(Node::Output::HeartbeatMessage)); 500 | REQUIRE(!nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 501 | REQUIRE(!nodes.at(1).popOutput(Node::Output::LogRecordMessage)); 502 | REQUIRE(!nodes.at(2).popOutput(Node::Output::LogRecordMessage)); 503 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 504 | REQUIRE(!nodes.at(1).popOutput(Node::Output::FileReadRequest)); 505 | received = nodes.at(2).popOutput(Node::Output::FileReadRequest).value(); 506 | reference.transfer_id = 2; // transfer-ID incremented, the rest is the same 507 | REQUIRE(received == reference); 508 | 509 | // FIRST READ REQUEST, THIRD ATTEMPT 510 | REQUIRE(!bl.poll(12'000ms)); 511 | REQUIRE(nodes.at(0).popOutput(Node::Output::HeartbeatMessage)); 512 | REQUIRE(nodes.at(1).popOutput(Node::Output::HeartbeatMessage)); 513 | REQUIRE(nodes.at(2).popOutput(Node::Output::HeartbeatMessage)); 514 | REQUIRE(!nodes.at(0).popOutput(Node::Output::LogRecordMessage)); 515 | REQUIRE(!nodes.at(1).popOutput(Node::Output::LogRecordMessage)); 516 | REQUIRE(!nodes.at(2).popOutput(Node::Output::LogRecordMessage)); 517 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 518 | REQUIRE(!nodes.at(1).popOutput(Node::Output::FileReadRequest)); 519 | received = nodes.at(2).popOutput(Node::Output::FileReadRequest).value(); 520 | reference.transfer_id = 3; // transfer-ID incremented, the rest is the same 521 | REQUIRE(received == reference); 522 | 523 | // READ RESPONSE TO THE THIRD ATTEMPT 524 | // The serialized representation was constructed manually from the binary file 525 | nodes.at(2).pushInput(Node::Input::FileReadResponse, 526 | Transfer(3, 527 | {0, 0, 128, 0, 72, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100, 63, 32, 528 | 32, 32, 32, 199, 196, 192, 111, 20, 21, 68, 94, 65, 80, 68, 101, 115, 99, 529 | 48, 48, 124, 194, 145, 71, 22, 198, 82, 134, 64, 2, 0, 0, 0, 0, 0, 530 | 0, 1, 16, 0, 0, 210, 2, 150, 73, 13, 240, 221, 224, 254, 15, 220, 186, 531 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 532 | 0, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 533 | 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 534 | 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126}, 535 | 2222)); 536 | (void) bl.poll(12'100ms); // Results will appear on the SECOND poll. 537 | REQUIRE(nodes.at(0).popOutput(Node::Output::HeartbeatMessage)); 538 | REQUIRE(nodes.at(1).popOutput(Node::Output::HeartbeatMessage)); 539 | REQUIRE(nodes.at(2).popOutput(Node::Output::HeartbeatMessage)); 540 | REQUIRE(bl.getState() == kocherga::State::BootDelay); 541 | // Completed here. 542 | REQUIRE(kocherga::Final::BootApp == bl.poll(15'000ms).value()); 543 | REQUIRE(kocherga::Final::BootApp == bl.poll(15'100ms).value()); // All subsequent calls yield the same Final. 544 | 545 | // NEW APPLICATION IS NOW AVAILABLE 546 | ai = *bl.getAppInfo(); 547 | REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); 548 | REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); 549 | REQUIRE(5 == ai.version.at(0)); 550 | REQUIRE(6 == ai.version.at(1)); 551 | REQUIRE(!ai.isReleaseBuild()); 552 | REQUIRE(ai.isDirtyBuild()); 553 | 554 | // BAD TRIGGER, GOOD TRIGGER 555 | REQUIRE(bl.trigger(&nodes.at(0), 0, 69, path)); 556 | REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); 557 | const mock::Node stray_node; 558 | REQUIRE(!bl.trigger(&stray_node, 0, 69, path)); 559 | } 560 | -------------------------------------------------------------------------------- /tests/unit/test_main.cpp: -------------------------------------------------------------------------------- 1 | #define CATCH_CONFIG_MAIN 2 | #include "catch.hpp" // NOLINT 3 | #include "util.hpp" 4 | 5 | auto kocherga::getRandomByte() -> std::uint8_t 6 | { 7 | return util::getRandomInteger(); 8 | } 9 | -------------------------------------------------------------------------------- /tests/unit/test_misc.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. 6 | #include "catch.hpp" 7 | #include "util.hpp" 8 | #include 9 | #include 10 | #include 11 | 12 | TEST_CASE("CRC64") 13 | { 14 | kocherga::CRC64 crc; 15 | const char* val = "12345"; 16 | crc.update(reinterpret_cast(val), 5); // NOSONAR NOLINT reinterpret_cast 17 | crc.update(nullptr, 0); 18 | val = "6789"; 19 | crc.update(reinterpret_cast(val), 4); // NOSONAR NOLINT reinterpret_cast 20 | 21 | REQUIRE(0x62EC'59E3'F1A4'F00AULL == crc.get()); 22 | REQUIRE(crc.getBytes().at(0) == 0x62U); 23 | REQUIRE(crc.getBytes().at(1) == 0xECU); 24 | REQUIRE(crc.getBytes().at(2) == 0x59U); 25 | REQUIRE(crc.getBytes().at(3) == 0xE3U); 26 | REQUIRE(crc.getBytes().at(4) == 0xF1U); 27 | REQUIRE(crc.getBytes().at(5) == 0xA4U); 28 | REQUIRE(crc.getBytes().at(6) == 0xF0U); 29 | REQUIRE(crc.getBytes().at(7) == 0x0AU); 30 | 31 | REQUIRE(!crc.isResidueCorrect()); 32 | crc.update(crc.getBytes().data(), 8); 33 | REQUIRE(crc.isResidueCorrect()); 34 | REQUIRE(0xFCAC'BEBD'5931'A992ULL == (~crc.get())); 35 | } 36 | 37 | TEST_CASE("CRC16-CCITT") 38 | { 39 | kocherga::detail::CRC16CCITT crc; 40 | crc.update(static_cast('1')); 41 | crc.update(static_cast('2')); 42 | crc.update(static_cast('3')); 43 | crc.update(static_cast('4')); 44 | crc.update(static_cast('5')); 45 | crc.update(static_cast('6')); 46 | crc.update(static_cast('7')); 47 | crc.update(static_cast('8')); 48 | crc.update(static_cast('9')); 49 | 50 | REQUIRE(0x29B1U == crc.get()); 51 | REQUIRE(crc.getBytes().at(0) == 0x29U); 52 | REQUIRE(crc.getBytes().at(1) == 0xB1U); 53 | 54 | REQUIRE(!crc.isResidueCorrect()); 55 | crc.update(0x29U); 56 | crc.update(0xB1U); 57 | REQUIRE(crc.isResidueCorrect()); 58 | REQUIRE(0x0000U == crc.get()); 59 | } 60 | 61 | TEST_CASE("serial::CRC32C") 62 | { 63 | kocherga::detail::CRC32C crc; 64 | crc.update(static_cast('1')); 65 | crc.update(static_cast('2')); 66 | crc.update(static_cast('3')); 67 | crc.update(static_cast('4')); 68 | crc.update(static_cast('5')); 69 | crc.update(static_cast('6')); 70 | crc.update(static_cast('7')); 71 | crc.update(static_cast('8')); 72 | crc.update(static_cast('9')); 73 | 74 | REQUIRE(0xE306'9283UL == crc.get()); 75 | REQUIRE(crc.getBytes().at(0) == 0x83U); 76 | REQUIRE(crc.getBytes().at(1) == 0x92U); 77 | REQUIRE(crc.getBytes().at(2) == 0x06U); 78 | REQUIRE(crc.getBytes().at(3) == 0xE3U); 79 | 80 | REQUIRE(!crc.isResidueCorrect()); 81 | crc.update(0x83U); 82 | crc.update(0x92U); 83 | crc.update(0x06U); 84 | crc.update(0xE3U); 85 | REQUIRE(crc.isResidueCorrect()); 86 | REQUIRE(0xB798'B438UL == (~crc.get())); 87 | } 88 | 89 | TEST_CASE("VolatileStorage") 90 | { 91 | struct Data 92 | { 93 | std::uint64_t a; 94 | std::uint8_t b; 95 | std::array c; 96 | }; 97 | static_assert(sizeof(Data) <= 16); 98 | static_assert(kocherga::VolatileStorage::StorageSize == (sizeof(Data) + 8U)); 99 | 100 | std::array::StorageSize> arena{}; 101 | 102 | kocherga::VolatileStorage marshaller(arena.data()); 103 | 104 | // The storage is empty, checking 105 | REQUIRE(!marshaller.take()); 106 | 107 | // Writing zeros and checking the representation 108 | marshaller.store(Data()); 109 | std::cout << util::makeHexDump(arena) << std::endl; 110 | REQUIRE(std::all_of(arena.begin(), arena.begin() + 12, [](auto x) { return x == 0; })); 111 | REQUIRE(std::any_of(arena.begin() + sizeof(Data), arena.end(), [](auto x) { return x != 0; })); 112 | 113 | // Reading and making sure it's erased afterwards 114 | auto rd = marshaller.take(); 115 | REQUIRE(rd); 116 | REQUIRE(rd->a == 0); 117 | REQUIRE(rd->b == 0); 118 | REQUIRE(rd->c[0] == 0); 119 | REQUIRE(rd->c[1] == 0); 120 | REQUIRE(rd->c[2] == 0); 121 | 122 | std::cout << util::makeHexDump(arena) << std::endl; 123 | REQUIRE(std::all_of(arena.begin(), arena.end(), [](auto x) { return x == 0xCAU; })); 124 | REQUIRE(!marshaller.take()); 125 | 126 | // Writing non-zeros and checking the representation 127 | marshaller.store({ 128 | 0x11AD'EADB'ADC0'FFEE, 129 | 123, 130 | {{1, 2, 3}}, 131 | }); 132 | std::cout << util::makeHexDump(arena) << std::endl; 133 | 134 | // Reading and making sure it's erased afterwards 135 | rd = marshaller.take(); 136 | REQUIRE(rd); 137 | REQUIRE(rd->a == 0x11AD'EADB'ADC0'FFEE); 138 | REQUIRE(rd->b == 123); 139 | REQUIRE(rd->c[0] == 1); 140 | REQUIRE(rd->c[1] == 2); 141 | REQUIRE(rd->c[2] == 3); 142 | std::cout << util::makeHexDump(arena) << std::endl; 143 | REQUIRE(std::all_of(arena.begin(), arena.end(), [](auto x) { return x == 0xCAU; })); 144 | REQUIRE(!marshaller.take()); 145 | } 146 | -------------------------------------------------------------------------------- /tests/unit/test_presenter.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. 6 | #include "catch.hpp" 7 | #include "mock.hpp" 8 | #include 9 | #include 10 | 11 | /// GENERATION OF REFERENCE SERIALIZED REPRESENTATIONS. 12 | /// Prepare the DSDL-generated packages: 13 | /// $ yakut compile https://github.com/OpenCyphal/public_regulated_data_types/archive/master.zip 14 | /// Run PyCyphal: 15 | /// >>> import pycyphal, uavcan.node, uavcan.diagnostic, uavcan.file 16 | /// >>> list(b''.join(pycyphal.dsdl.serialize(uavcan.node.Heartbeat_1_0( 17 | /// mode=uavcan.node.Mode_1_0(uavcan.node.Mode_1_0.SOFTWARE_UPDATE), 18 | /// health=uavcan.node.Health_1_0(uavcan.node.Health_1_0.ADVISORY), 19 | /// uptime=1)))) 20 | /// [1, 0, 0, 0, 1, 3, 0] 21 | namespace 22 | { 23 | class MockController : public kocherga::detail::IController 24 | { 25 | public: 26 | [[nodiscard]] auto popRebootRequestedFlag() 27 | { 28 | const auto out = reboot_requested_; 29 | reboot_requested_ = false; 30 | return out; 31 | } 32 | 33 | [[nodiscard]] auto popUpdateRequestedFlag() 34 | { 35 | const auto out = update_requested_; 36 | update_requested_ = false; 37 | return out; 38 | } 39 | 40 | [[nodiscard]] auto popFileReadResult() 41 | { 42 | const auto out = file_read_result_; 43 | file_read_result_.reset(); 44 | return out; 45 | } 46 | 47 | void setAppInfo(const std::optional& value) { app_info_ = value; } 48 | 49 | private: 50 | void reboot() override { reboot_requested_ = true; } 51 | 52 | void beginUpdate() override { update_requested_ = true; } 53 | 54 | void handleFileReadResult(const std::optional response) override 55 | { 56 | file_read_result_.emplace(response); 57 | } 58 | 59 | [[nodiscard]] auto getAppInfo() const -> std::optional override { return app_info_; } 60 | 61 | bool reboot_requested_ = false; 62 | bool update_requested_ = false; 63 | 64 | std::optional app_info_; 65 | std::optional> file_read_result_; 66 | }; 67 | 68 | } // namespace 69 | 70 | TEST_CASE("Presenter") // NOLINT NOSONAR complexity threshold 71 | { 72 | using mock::Node; 73 | using mock::Transfer; 74 | 75 | static constexpr auto coa_capacity = 222U; 76 | std::array coa{}; 77 | for (auto i = 0U; i < coa_capacity; i++) 78 | { 79 | coa.at(i) = static_cast(coa.size() - i); 80 | } 81 | 82 | const kocherga::SystemInfo sys_info{ 83 | kocherga::SemanticVersion{33, 11}, 84 | {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}, 85 | "com.zubax.kocherga.test.presenter", 86 | 222, 87 | coa.data(), 88 | }; 89 | 90 | MockController controller; 91 | std::array nodes; 92 | kocherga::detail::Presenter::Params params{}; 93 | params.request_retry_limit = 1; 94 | kocherga::detail::Presenter pres(sys_info, controller, params); 95 | REQUIRE(pres.addNode(&nodes.at(0))); 96 | REQUIRE(pres.addNode(&nodes.at(1))); 97 | REQUIRE(!pres.addNode(&nodes.at(1))); // Double registration has no effect. 98 | 99 | auto ts = std::chrono::microseconds{500'000}; 100 | pres.poll(ts); 101 | for (const auto& n : nodes) 102 | { 103 | REQUIRE(n.getLastPollTime() == ts); 104 | } 105 | 106 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.node.Heartbeat_1_0( 107 | // mode=uavcan.node.Mode_1_0(uavcan.node.Mode_1_0.SOFTWARE_UPDATE), 108 | // health=uavcan.node.Health_1_0(uavcan.node.Health_1_0.ADVISORY), 109 | // uptime=1, vendor_specific_status_code=0x7d)))) 110 | pres.setNodeHealth(kocherga::detail::dsdl::Heartbeat::Health::Advisory); 111 | pres.setNodeVSSC(125); 112 | ts = std::chrono::microseconds{1'500'000}; 113 | pres.poll(ts); 114 | for (auto& n : nodes) 115 | { 116 | REQUIRE(n.getLastPollTime() == ts); 117 | const auto tr = *n.popOutput(Node::Output::HeartbeatMessage); 118 | std::cout << tr.toString() << std::endl; 119 | REQUIRE(tr == Transfer(0, {1, 0, 0, 0, 1, 3, 125})); 120 | } 121 | 122 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.diagnostic.Record_1_1( 123 | // severity=uavcan.diagnostic.Severity_1_0(uavcan.diagnostic.Severity_1_0.NOTICE), 124 | // text='Hello world!')))) 125 | pres.publishLogRecord(kocherga::detail::dsdl::Diagnostic::Severity::Notice, "Hello world!"); 126 | for (auto& n : nodes) 127 | { 128 | const auto tr = *n.popOutput(Node::Output::LogRecordMessage); 129 | std::cout << tr.toString() << std::endl; 130 | REQUIRE(tr == 131 | Transfer(0, {0, 0, 0, 0, 0, 0, 0, 3, 12, 72, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100, 33})); 132 | } 133 | 134 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.diagnostic.Record_1_1( 135 | // severity=uavcan.diagnostic.Severity_1_0(uavcan.diagnostic.Severity_1_0.CRITICAL), 136 | // text='We are going to die :)')))) 137 | pres.publishLogRecord(kocherga::detail::dsdl::Diagnostic::Severity::Critical, "We are going to die :)"); 138 | for (auto& n : nodes) 139 | { 140 | const auto tr = *n.popOutput(Node::Output::LogRecordMessage); 141 | std::cout << tr.toString() << std::endl; 142 | REQUIRE(tr == Transfer(1, {0, 0, 0, 0, 0, 0, 0, 6, 22, 87, 101, 32, 97, 114, 101, 32, 143 | 103, 111, 105, 110, 103, 32, 116, 111, 32, 100, 105, 101, 32, 58, 41})); 144 | } 145 | 146 | // Fails because the remote node-ID is not yet known. 147 | REQUIRE(!pres.requestFileRead(0xB'ADBA'DBADULL)); 148 | 149 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( 150 | // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_BEGIN_SOFTWARE_UPDATE, '/foo/bar/baz.app.bin')))) 151 | nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, 152 | Transfer(123, 153 | {253, 255, 20, 47, 102, 111, 111, 47, 98, 97, 114, 47, 154 | 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, 155 | 3210)); 156 | ts = std::chrono::microseconds{1'600'000}; 157 | pres.poll(ts); 158 | for (const auto& n : nodes) 159 | { 160 | REQUIRE(n.getLastPollTime() == ts); 161 | } 162 | REQUIRE(controller.popUpdateRequestedFlag()); 163 | REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); 164 | REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == 165 | Transfer(123, {0, 0, 0, 0, 0, 0, 0}, 3210)); 166 | 167 | // The file location is known now. 168 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.file.Read_1_1.Request(0xbadbadbad, 169 | // uavcan.file.Path_2_0('/foo/bar/baz.app.bin'))))) 170 | REQUIRE(pres.requestFileRead(0xB'ADBA'DBADULL)); 171 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 172 | REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == 173 | Transfer(1, 174 | {173, 219, 186, 173, 11, 20, 47, 102, 111, 111, 47, 98, 97, 175 | 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, 176 | 3210)); 177 | 178 | // Invalid request serialization. File location specifier not updated. 179 | nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, Transfer(333, std::vector(), 3210)); 180 | ts = std::chrono::microseconds{1'700'000}; 181 | pres.poll(ts); 182 | for (const auto& n : nodes) 183 | { 184 | REQUIRE(n.getLastPollTime() == ts); 185 | } 186 | REQUIRE(!controller.popUpdateRequestedFlag()); 187 | REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == 188 | Transfer(333, {6, 0, 0, 0, 0, 0, 0}, 3210)); // Internal error. 189 | REQUIRE(!nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)); 190 | 191 | // Reboot request. 192 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( 193 | // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_RESTART, '')))) 194 | nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, Transfer(444, {255, 255, 0}, 2222)); 195 | ts = std::chrono::microseconds{1'800'000}; 196 | pres.poll(ts); 197 | for (const auto& n : nodes) 198 | { 199 | REQUIRE(n.getLastPollTime() == ts); 200 | } 201 | REQUIRE(controller.popRebootRequestedFlag()); 202 | REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); 203 | REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == 204 | Transfer(444, {0, 0, 0, 0, 0, 0, 0}, 2222)); 205 | 206 | // Node info request; app info not available. SR generation not shown. 207 | nodes.at(0).pushInput(Node::Input::NodeInfoRequest, Transfer(555, std::vector{}, 3333)); 208 | ts = std::chrono::microseconds{1'900'000}; 209 | pres.poll(ts); 210 | for (const auto& n : nodes) 211 | { 212 | REQUIRE(n.getLastPollTime() == ts); 213 | } 214 | REQUIRE( 215 | (*nodes.at(0).popOutput(Node::Output::NodeInfoResponse)) == 216 | Transfer(555, 217 | {1, 0, 33, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, 218 | 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 33, 99, 111, 109, 46, 122, 117, 98, 97, 120, 219 | 46, 107, 111, 99, 104, 101, 114, 103, 97, 46, 116, 101, 115, 116, 46, 112, 114, 101, 115, 101, 220 | 110, 116, 101, 114, 0, 222, 222, 221, 220, 219, 218, 217, 216, 215, 214, 213, 212, 211, 210, 209, 221 | 208, 207, 206, 205, 204, 203, 202, 201, 200, 199, 198, 197, 196, 195, 194, 193, 192, 191, 190, 189, 222 | 188, 187, 186, 185, 184, 183, 182, 181, 180, 179, 178, 177, 176, 175, 174, 173, 172, 171, 170, 169, 223 | 168, 167, 166, 165, 164, 163, 162, 161, 160, 159, 158, 157, 156, 155, 154, 153, 152, 151, 150, 149, 224 | 148, 147, 146, 145, 144, 143, 142, 141, 140, 139, 138, 137, 136, 135, 134, 133, 132, 131, 130, 129, 225 | 128, 127, 126, 125, 124, 123, 122, 121, 120, 119, 118, 117, 116, 115, 114, 113, 112, 111, 110, 109, 226 | 108, 107, 106, 105, 104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 90, 89, 227 | 88, 87, 86, 85, 84, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, 72, 71, 70, 69, 228 | 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, 229 | 48, 47, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 32, 31, 30, 29, 230 | 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 231 | 8, 7, 6, 5, 4, 3, 2, 1}, 232 | 3333)); 233 | REQUIRE(!nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)); 234 | 235 | // Node info request; app info is available. 236 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.node.GetInfo_1_0.Response( 237 | // protocol_version=uavcan.node.Version_1_0(1, 0), hardware_version=uavcan.node.Version_1_0(33, 11), 238 | // software_version=uavcan.node.Version_1_0(3, 11), software_vcs_revision_id=0xDEADDEADDEADDEAD, 239 | // unique_id=bytes(range(1, 17)), name='com.zubax.kocherga.test.presenter', 240 | // software_image_crc=[0xBADC0FFEE0DDF00D], certificate_of_authenticity=bytes(range(1, 223)[::-1]))))) 241 | controller.setAppInfo(kocherga::AppInfo{ 242 | 0xBADC'0FFE'E0DD'F00DULL, 243 | 0xD'EADBULL, 244 | {}, 245 | {3, 11}, 246 | 2, 247 | {}, 248 | 1234567890, 249 | 0xDEAD'DEAD'DEAD'DEADULL, 250 | {}, 251 | }); 252 | nodes.at(1).pushInput(Node::Input::NodeInfoRequest, Transfer(666, std::vector{}, 1111)); 253 | ts = std::chrono::microseconds{1'910'000}; 254 | pres.poll(ts); 255 | for (const auto& n : nodes) 256 | { 257 | REQUIRE(n.getLastPollTime() == ts); 258 | } 259 | REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); 260 | const Transfer node_info_reference(666, 261 | {1, 0, 33, 11, 3, 11, 173, 222, 173, 222, 173, 222, 173, 222, 1, 2, 262 | 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 33, 99, 263 | 111, 109, 46, 122, 117, 98, 97, 120, 46, 107, 111, 99, 104, 101, 114, 103, 264 | 97, 46, 116, 101, 115, 116, 46, 112, 114, 101, 115, 101, 110, 116, 101, 114, 265 | 1, 13, 240, 221, 224, 254, 15, 220, 186, 222, 222, 221, 220, 219, 218, 217, 266 | 216, 215, 214, 213, 212, 211, 210, 209, 208, 207, 206, 205, 204, 203, 202, 201, 267 | 200, 199, 198, 197, 196, 195, 194, 193, 192, 191, 190, 189, 188, 187, 186, 185, 268 | 184, 183, 182, 181, 180, 179, 178, 177, 176, 175, 174, 173, 172, 171, 170, 169, 269 | 168, 167, 166, 165, 164, 163, 162, 161, 160, 159, 158, 157, 156, 155, 154, 153, 270 | 152, 151, 150, 149, 148, 147, 146, 145, 144, 143, 142, 141, 140, 139, 138, 137, 271 | 136, 135, 134, 133, 132, 131, 130, 129, 128, 127, 126, 125, 124, 123, 122, 121, 272 | 120, 119, 118, 117, 116, 115, 114, 113, 112, 111, 110, 109, 108, 107, 106, 105, 273 | 104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 90, 89, 274 | 88, 87, 86, 85, 84, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, 275 | 72, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 276 | 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 277 | 40, 39, 38, 37, 36, 35, 34, 33, 32, 31, 30, 29, 28, 27, 26, 25, 278 | 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 279 | 8, 7, 6, 5, 4, 3, 2, 1}, 280 | 1111); 281 | const auto node_info_response = *nodes.at(1).popOutput(Node::Output::NodeInfoResponse); 282 | REQUIRE(node_info_reference == node_info_response); 283 | 284 | // It's time for another heartbeat. 285 | pres.setNodeHealth(kocherga::detail::dsdl::Heartbeat::Health::Warning); 286 | pres.setNodeVSSC(0x0000); 287 | ts = std::chrono::microseconds{2'100'000}; 288 | pres.poll(ts); 289 | for (auto& n : nodes) 290 | { 291 | REQUIRE(n.getLastPollTime() == ts); 292 | const auto tr = *n.popOutput(Node::Output::HeartbeatMessage); 293 | std::cout << tr.toString() << std::endl; 294 | REQUIRE(tr == Transfer(1, {2, 0, 0, 0, 3, 3, 0})); 295 | } 296 | 297 | // Deliver the file read response. 298 | // Remember that the second update request was rejected so the actual file location is the old one. 299 | // This one will be rejected because it's the wrong node. 300 | // list(b''.join(pycyphal.dsdl.serialize(uavcan.file.Read_1_1.Response(uavcan.file.Error_1_0(0), b'The data.')))) 301 | nodes.at(0).pushInput(Node::Input::FileReadResponse, 302 | Transfer(0, {0, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); 303 | ts = std::chrono::microseconds{2'200'000}; 304 | pres.poll(ts); 305 | for (const auto& n : nodes) 306 | { 307 | REQUIRE(n.getLastPollTime() == ts); 308 | } 309 | REQUIRE(!controller.popFileReadResult()); 310 | // Correct node -- accepted. 311 | nodes.at(1).pushInput(Node::Input::FileReadResponse, 312 | Transfer(0, {0, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); 313 | ts = std::chrono::microseconds{2'200'000}; 314 | pres.poll(ts); 315 | for (const auto& n : nodes) 316 | { 317 | REQUIRE(n.getLastPollTime() == ts); 318 | } 319 | const kocherga::detail::dsdl::File::ReadResponse read_result = **controller.popFileReadResult(); 320 | REQUIRE(read_result.isSuccessful()); 321 | REQUIRE(9 == read_result.data_length); // The pointer is invalidated here, don't check it. 322 | REQUIRE(nullptr != read_result.data); 323 | // Further responses not accepted because no request is pending. 324 | nodes.at(1).pushInput(Node::Input::FileReadResponse, 325 | Transfer(0, {0, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); 326 | ts = std::chrono::microseconds{2'300'000}; 327 | pres.poll(ts); 328 | for (const auto& n : nodes) 329 | { 330 | REQUIRE(n.getLastPollTime() == ts); 331 | } 332 | REQUIRE(!controller.popFileReadResult()); 333 | 334 | // File read error handling. 335 | nodes.at(1).setFileReadResult(false); 336 | REQUIRE(!pres.requestFileRead(123'456)); 337 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 338 | REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == 339 | Transfer(2, 340 | {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, 341 | 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, 342 | 3210)); 343 | 344 | // Successful request, but the response specifies a non-zero error code. 345 | nodes.at(1).setFileReadResult(true); 346 | REQUIRE(pres.requestFileRead(123'456)); 347 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 348 | REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == 349 | Transfer(3, 350 | {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, 351 | 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, 352 | 3210)); 353 | nodes.at(1).pushInput(Node::Input::FileReadResponse, 354 | Transfer(1, {1, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); 355 | ts = std::chrono::microseconds{2'400'000}; 356 | pres.poll(ts); 357 | for (const auto& n : nodes) 358 | { 359 | REQUIRE(n.getLastPollTime() == ts); 360 | } 361 | REQUIRE(!*controller.popFileReadResult()); // Empty option. 362 | 363 | // Successful request, but the response times out after the configured number of retries. 364 | REQUIRE(1 == params.request_retry_limit); // Ensure the configuration matches the expectations. 365 | nodes.at(1).setFileReadResult(true); 366 | REQUIRE(pres.requestFileRead(123'456)); 367 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 368 | REQUIRE(nodes.at(1).popOutput(Node::Output::FileReadRequest).value() == 369 | Transfer(4, 370 | {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, 371 | 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, 372 | 3210)); 373 | ts = std::chrono::microseconds{2'500'000}; 374 | pres.poll(ts); 375 | for (const auto& n : nodes) 376 | { 377 | REQUIRE(n.getLastPollTime() == ts); 378 | } 379 | REQUIRE(!controller.popFileReadResult()); // Nothing yet. 380 | ts = std::chrono::microseconds{9'000'000}; 381 | pres.poll(ts); 382 | for (auto& n : nodes) 383 | { 384 | REQUIRE(n.popOutput(Node::Output::HeartbeatMessage)); 385 | REQUIRE(n.getLastPollTime() == ts); 386 | } 387 | REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); 388 | REQUIRE(nodes.at(1).popOutput(Node::Output::FileReadRequest).value() == 389 | Transfer(5, // transfer-ID incremented 390 | {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, 391 | 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, 392 | 3210)); 393 | REQUIRE(!controller.popFileReadResult()); 394 | REQUIRE(!nodes.at(0).wasRequestCanceled()); 395 | REQUIRE(!nodes.at(1).wasRequestCanceled()); // Not yet! 396 | ts = std::chrono::microseconds{15'000'000}; 397 | pres.poll(ts); 398 | for (auto& n : nodes) 399 | { 400 | REQUIRE(n.popOutput(Node::Output::HeartbeatMessage)); 401 | REQUIRE(n.getLastPollTime() == ts); 402 | } 403 | REQUIRE(!controller.popFileReadResult().value()); // Empty option. 404 | REQUIRE(!nodes.at(0).wasRequestCanceled()); 405 | REQUIRE(nodes.at(1).wasRequestCanceled()); // Given up. 406 | } 407 | -------------------------------------------------------------------------------- /tests/unit/test_util.cpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #include "util.hpp" 6 | #include "catch.hpp" 7 | 8 | TEST_CASE("util::makeHexDump") 9 | { 10 | REQUIRE("00000000 31 32 33 123 " == 11 | util::makeHexDump(std::string("123"))); 12 | 13 | REQUIRE("00000000 30 31 32 33 34 35 36 37 38 39 61 62 63 64 65 66 0123456789abcdef\n" 14 | "00000010 67 68 69 6a 6b 6c 6d 6e 6f 70 71 72 73 74 75 76 ghijklmnopqrstuv\n" 15 | "00000020 77 78 79 7a 41 42 43 44 45 46 47 48 49 4a 4b 4c wxyzABCDEFGHIJKL\n" 16 | "00000030 4d 4e 4f 50 51 52 53 54 55 56 57 58 59 5a MNOPQRSTUVWXYZ " == 17 | util::makeHexDump(std::string("0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"))); 18 | } 19 | 20 | TEST_CASE("util::FileROMBackend") 21 | { 22 | (void) std::filesystem::remove("test_rom.bin.tmp"); 23 | 24 | util::FileROMBackend back("test_rom.bin.tmp", 1024, 0xFFU); 25 | 26 | const std::vector ref(1024, std::byte{0xFF}); 27 | REQUIRE(back.isSameImage(ref.data(), ref.size())); 28 | 29 | const std::vector ref2(123, std::byte{0xFF}); 30 | REQUIRE(back.isSameImage(ref2.data(), ref2.size())); 31 | 32 | const std::vector ref3(123, std::byte{0xAA}); 33 | REQUIRE(!back.isSameImage(ref3.data(), ref3.size())); 34 | 35 | const util::FileROMBackend existing("test_rom.bin.tmp"); 36 | REQUIRE(existing.getSize() == 1024U); 37 | REQUIRE(existing.isSameImage(ref.data(), ref.size())); 38 | 39 | kocherga::IROMBackend& interface = back; 40 | 41 | std::array buf1{}; 42 | 43 | REQUIRE(0 == back.getReadCount()); 44 | REQUIRE(0 == back.getWriteCount()); 45 | 46 | // Reading 47 | REQUIRE(32 == interface.read(0, buf1.data(), 32)); 48 | REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 32, [](auto x) { return x == std::byte{0xFF}; })); 49 | 50 | REQUIRE(1024 == interface.read(0, buf1.data(), 1024)); 51 | REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 1024, [](auto x) { return x == std::byte{0xFF}; })); 52 | 53 | REQUIRE(512 == interface.read(512, buf1.data(), 1024)); 54 | REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 512, [](auto x) { return x == std::byte{0xFF}; })); 55 | 56 | REQUIRE(128 == interface.read(512, buf1.data(), 128)); 57 | REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 1024, [](auto x) { return x == std::byte{0xFF}; })); 58 | 59 | REQUIRE(0 == interface.read(1024, buf1.data(), 1)); 60 | 61 | REQUIRE(1024 == interface.read(0, buf1.data(), 2000)); 62 | std::all_of(buf1.begin(), buf1.begin() + 1024, [](auto x) { return x == std::byte{0xFF}; }); 63 | REQUIRE(buf1[1024] == std::byte{0}); 64 | REQUIRE(buf1[1025] == std::byte{0}); 65 | REQUIRE(buf1[1026] == std::byte{0}); 66 | 67 | REQUIRE(6 == back.getReadCount()); 68 | REQUIRE(0 == back.getWriteCount()); 69 | 70 | // Writing 71 | REQUIRE_THROWS_AS(interface.write(0, buf1.data(), 123), std::runtime_error); 72 | REQUIRE_THROWS_AS(interface.endWrite(), std::runtime_error); 73 | interface.beginWrite(); 74 | buf1.fill(std::byte{0xAA}); 75 | REQUIRE(128 == *interface.write(0, buf1.data(), 128)); 76 | REQUIRE(128 == *interface.write(512, buf1.data(), 128)); 77 | interface.endWrite(); 78 | 79 | REQUIRE(1024 == interface.read(0, buf1.data(), 2000)); 80 | REQUIRE(std::all_of(buf1.begin() + 0, buf1.begin() + 128, [](auto x) { return x == std::byte{0xAA}; })); 81 | REQUIRE(std::all_of(buf1.begin() + 128, buf1.begin() + 512, [](auto x) { return x == std::byte{0xFF}; })); 82 | REQUIRE(std::all_of(buf1.begin() + 512, buf1.begin() + 640, [](auto x) { return x == std::byte{0xAA}; })); 83 | 84 | REQUIRE(1024 == static_cast(existing).read(0, buf1.data(), 2000)); 85 | REQUIRE(std::all_of(buf1.begin() + 0, buf1.begin() + 128, [](auto x) { return x == std::byte{0xAA}; })); 86 | REQUIRE(std::all_of(buf1.begin() + 128, buf1.begin() + 512, [](auto x) { return x == std::byte{0xFF}; })); 87 | REQUIRE(std::all_of(buf1.begin() + 512, buf1.begin() + 640, [](auto x) { return x == std::byte{0xAA}; })); 88 | 89 | REQUIRE(7 == back.getReadCount()); 90 | REQUIRE(3 == back.getWriteCount()); 91 | 92 | // Failure injection 93 | interface.beginWrite(); 94 | back.enableFailureInjection(true); 95 | REQUIRE(!interface.write(0, buf1.data(), 128)); 96 | back.enableFailureInjection(false); 97 | REQUIRE(128 == *interface.write(0, buf1.data(), 128)); 98 | 99 | REQUIRE(7 == back.getReadCount()); 100 | REQUIRE(5 == back.getWriteCount()); 101 | } 102 | -------------------------------------------------------------------------------- /tests/util/util.hpp: -------------------------------------------------------------------------------- 1 | // This software is distributed under the terms of the MIT License. 2 | // Copyright (c) 2020 Zubax Robotics. 3 | // Author: Pavel Kirienko 4 | 5 | #pragma once 6 | 7 | #include "kocherga.hpp" 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace util 23 | { 24 | static constexpr std::uint8_t BitsPerByte = 8U; 25 | static constexpr std::pair PrintableASCIIRange{32, 126}; 26 | 27 | template 28 | inline auto makeHexDump(InputIterator begin, const InputIterator end) -> std::string 29 | { 30 | static constexpr std::uint8_t BytesPerRow = BitsPerByte * 2; 31 | std::uint32_t offset = 0; 32 | std::ostringstream output; 33 | bool first = true; 34 | output << std::hex << std::setfill('0'); 35 | do 36 | { 37 | if (first) 38 | { 39 | first = false; 40 | } 41 | else 42 | { 43 | output << std::endl; 44 | } 45 | 46 | output << std::setw(BitsPerByte) << offset << " "; 47 | offset += BytesPerRow; 48 | 49 | auto it = begin; 50 | for (std::uint8_t i = 0; i < BytesPerRow; ++i) 51 | { 52 | if (i == BitsPerByte) 53 | { 54 | output << ' '; 55 | } 56 | 57 | if (it != end) 58 | { 59 | output << std::setw(2) << static_cast(*it) << ' '; 60 | ++it; 61 | } 62 | else 63 | { 64 | output << " "; 65 | } 66 | } 67 | 68 | output << " "; 69 | for (std::uint8_t i = 0; i < BytesPerRow; ++i) 70 | { 71 | if (begin != end) 72 | { 73 | output << (((static_cast(*begin) >= PrintableASCIIRange.first) && 74 | (static_cast(*begin) <= PrintableASCIIRange.second)) 75 | ? static_cast(*begin) // NOSONAR intentional conversion to plain char 76 | : '.'); 77 | ++begin; 78 | } 79 | else 80 | { 81 | output << ' '; 82 | } 83 | } 84 | } while (begin != end); 85 | return output.str(); 86 | } 87 | 88 | template 89 | inline auto makeHexDump(const Container& cont) 90 | { 91 | return makeHexDump(std::begin(cont), std::end(cont)); 92 | } 93 | 94 | /// A ROM backend mapped onto a local file. 95 | class FileROMBackend : public kocherga::IROMBackend 96 | { 97 | public: 98 | class Error : public std::runtime_error 99 | { 100 | public: 101 | explicit Error(const std::string& x) : std::runtime_error(x) {} 102 | }; 103 | 104 | /// This overload creates a new file filled as specified. If the file exists, it will be written over. 105 | FileROMBackend(const std::filesystem::path& file_path, 106 | const std::size_t rom_size, 107 | const std::uint8_t memory_fill = static_cast((1U << BitsPerByte) - 1U)) : 108 | path_(file_path), rom_size_(rom_size) 109 | { 110 | std::ofstream f(path_, std::ios::binary | std::ios::out | std::ios::trunc); 111 | if (f) 112 | { 113 | const std::vector empty_image(rom_size, 114 | static_cast( 115 | memory_fill)); // NOSONAR intentional conversion to plain char 116 | f.write(empty_image.data(), static_cast(empty_image.size())); 117 | f.flush(); 118 | } 119 | else 120 | { 121 | throw Error("Could not init the ROM mapping file"); 122 | } 123 | } 124 | 125 | /// This overload opens an existing file. 126 | explicit FileROMBackend(const std::filesystem::path& file_path) : 127 | path_(file_path), rom_size_(static_cast(std::filesystem::file_size(file_path))) 128 | { 129 | checkFileHealth(); 130 | } 131 | 132 | void enableFailureInjection(const bool enabled) { trigger_failure_ = enabled; } 133 | 134 | auto isSameImage(const std::byte* const reference, const std::size_t reference_size) const 135 | { 136 | checkFileHealth(); 137 | if (std::ifstream f(path_, std::ios::binary | std::ios::in); f) 138 | { 139 | f.seekg(0); 140 | std::vector buffer(reference_size, '\0'); 141 | f.read(buffer.data(), static_cast(reference_size)); 142 | return std::memcmp(reference, buffer.data(), reference_size) == 0; 143 | } 144 | throw Error("Could not open the ROM mapping file for verification"); 145 | } 146 | 147 | [[nodiscard]] auto read(const std::size_t offset, std::byte* const out_data, const std::size_t size) const 148 | -> std::size_t override 149 | { 150 | read_count_++; 151 | const std::size_t out = ((offset + size) > rom_size_) ? (rom_size_ - offset) : size; 152 | assert(out <= size); 153 | assert((out + offset) <= rom_size_); 154 | checkFileHealth(); 155 | if (std::ifstream f(path_, std::ios::binary | std::ios::in); f) 156 | { 157 | f.seekg(static_cast(offset)); 158 | f.read(reinterpret_cast(out_data), // NOLINT NOSONAR reinterpret_cast 159 | static_cast(out)); 160 | return out; 161 | } 162 | throw Error("Could not open the ROM emulation file for reading"); 163 | } 164 | 165 | auto getReadCount() const { return read_count_; } 166 | auto getWriteCount() const { return write_count_; } 167 | 168 | auto getSize() const { return rom_size_; } 169 | 170 | private: 171 | void checkFileHealth() const 172 | { 173 | if (std::filesystem::file_size(path_) != rom_size_) 174 | { 175 | throw Error("Invalid size of the ROM mapping file"); 176 | } 177 | } 178 | 179 | void beginWrite() override 180 | { 181 | checkFileHealth(); 182 | if (upgrade_in_progress_) 183 | { 184 | throw Error("Bad sequencing"); 185 | } 186 | upgrade_in_progress_ = true; 187 | } 188 | 189 | void endWrite() override 190 | { 191 | checkFileHealth(); 192 | if (!upgrade_in_progress_) 193 | { 194 | throw Error("Bad sequencing"); 195 | } 196 | upgrade_in_progress_ = false; 197 | } 198 | 199 | [[nodiscard]] auto write(const std::size_t offset, const std::byte* const data, const std::size_t size) 200 | -> std::optional override 201 | { 202 | write_count_++; 203 | if (!upgrade_in_progress_) 204 | { 205 | throw Error("Bad sequencing"); 206 | } 207 | checkFileHealth(); 208 | if (trigger_failure_) 209 | { 210 | // Can't use {} because of a bug in GCC: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=92092 211 | return std::optional(std::nullopt); // NOLINT 212 | } 213 | const std::size_t out = ((offset + size) > rom_size_) ? (rom_size_ - offset) : size; 214 | assert(out <= size); 215 | assert((out + offset) <= rom_size_); 216 | if (std::ofstream f(path_, std::ios::binary | std::ios::out | std::ios::in); f) 217 | { 218 | f.seekp(static_cast(offset)); 219 | f.write(reinterpret_cast(data), // NOLINT NOSONAR reinterpret_cast 220 | static_cast(out)); 221 | f.flush(); 222 | return out; 223 | } 224 | throw Error("Could not open the ROM emulation file for writing"); 225 | } 226 | 227 | const std::filesystem::path path_; 228 | const std::size_t rom_size_; 229 | 230 | mutable std::uint64_t read_count_ = 0; 231 | std::uint64_t write_count_ = 0; 232 | bool upgrade_in_progress_ = false; 233 | bool trigger_failure_ = false; 234 | }; 235 | 236 | class EnvironmentError : public std::runtime_error 237 | { 238 | public: 239 | explicit EnvironmentError(const std::string& x) : std::runtime_error(x) {} 240 | }; 241 | 242 | /// A safe wrapper over the standard getenv() that returns an empty option if the variable is not set. 243 | inline auto getEnvironmentVariableMaybe(const std::string& name) -> std::optional 244 | { 245 | if (auto* const st = std::getenv(name.c_str()); st != nullptr) // NOSONAR getenv is considered unsafe. 246 | { 247 | return std::string(st); 248 | } 249 | return {}; 250 | } 251 | 252 | /// Like above but throws EnvironmentError if the variable is missing. 253 | inline auto getEnvironmentVariable(const std::string& name) -> std::string 254 | { 255 | if (const auto v = getEnvironmentVariableMaybe(name)) 256 | { 257 | return *v; 258 | } 259 | throw EnvironmentError("Environment variable is required but not set: " + name); 260 | } 261 | 262 | inline auto getSourceDir() -> std::filesystem::path 263 | { 264 | return getEnvironmentVariable("SOURCE_DIR"); 265 | } 266 | 267 | inline auto getImagePath(const std::string& name) -> std::filesystem::path 268 | { 269 | return util::getSourceDir() / "images" / name; 270 | } 271 | 272 | template 273 | inline auto getRandomInteger() -> std::enable_if_t, T> 274 | { 275 | return static_cast((static_cast(std::rand()) * std::numeric_limits::max()) / RAND_MAX); 276 | } 277 | 278 | } // namespace util 279 | -------------------------------------------------------------------------------- /tools/pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | line-length = 120 3 | target-version = ['py310'] 4 | include = ''' 5 | (.*\.pyi?$) 6 | ''' 7 | --------------------------------------------------------------------------------