From 667d7ba915e4a4f413b3fc9043e067549c47ba9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 7 Sep 2021 15:31:21 +0300 Subject: [PATCH] Full rewrite, support UAVCAN/CAN (both v1 and v0), UAVCAN/serial (#7) --- .clang-format | 96 + .clang-tidy | 30 + .gitignore | 4 +- .gitmodules | 9 - .idea/dictionaries/pavel.xml | 43 + .travis.yml | 62 - CONTRIBUTING.md | 75 + README.md | 417 +++- docs/state_machine.dia | Bin 0 -> 2328 bytes state_machine.svg => docs/state_machine.svg | 62 +- kocherga.hpp | 902 ------- kocherga/kocherga.hpp | 1267 ++++++++++ kocherga/kocherga_can.hpp | 2201 +++++++++++++++++ kocherga/kocherga_serial.hpp | 686 +++++ kocherga_popcop.hpp | 534 ---- kocherga_uavcan.hpp | 1284 ---------- kocherga_ymodem.hpp | 640 ----- populate_app_descriptor.py | 293 --- state_machine.dia | Bin 2517 -> 0 bytes test/CMakeLists.txt | 79 - test/images.hpp | 854 ------- test/libcanard | 1 - test/mocks.hpp | 292 --- test/piped_process.hpp | 162 -- test/popcop | 1 - test/popcop_tester/main.py | 167 -- .../test.image-1.0.bin | Bin 111 -> 0 bytes test/populate_app_descriptor/test.sh | 9 - test/senoval | 1 - test/test_core.cpp | 425 ---- test/test_main.cpp | 34 - test/test_mocks.cpp | 157 -- test/test_piped_process.cpp | 77 - test/test_popcop.cpp | 674 ----- test/test_uavcan_python.cpp | 320 --- test/test_util.cpp | 40 - test/test_ymodem.cpp | 319 --- .../io.px4.sapog-1-2.2.7719d6e.compound.bin | Bin 210372 -> 0 bytes .../invalid-images/with-invalid-header.bin | Bin 1024 -> 0 bytes .../invalid-images/without-header.bin | Bin 19404 -> 0 bytes test/uavcan_tester/main.py | 105 - test/uavcan_tester/uavcan_tester.py | 395 --- test/uavcan_tester/valid-images/a.bin | Bin 10504 -> 0 bytes ...io.px4.sapog-1-2.2.7719d6e.application.bin | Bin 193992 -> 0 bytes test/util.hpp | 108 - tests/.clang-tidy | 38 + {test => tests/3rd_party}/catch.hpp | 0 tests/CMakeLists.txt | 42 + tests/integration/.gitignore | 4 + tests/integration/CMakeLists.txt | 40 + tests/integration/bootloader/main.cpp | 320 +++ tests/integration/validator/can.orc.yaml | 32 + ...ee0ddf00d.452a4267971a3928.app.release.bin | 1 + .../integration/validator/manual_v0.orc.yaml | 26 + tests/integration/validator/serial.orc.yaml | 39 + tests/integration/validator/validate.sh | 27 + tests/tools/.gitignore | 2 + tests/tools/demo-be.bin | Bin 0 -> 111 bytes tests/tools/demo-le.bin | Bin 0 -> 111 bytes tests/tools/invalid.bin | Bin 0 -> 83 bytes tests/tools/test.sh | 33 + tests/unit/CMakeLists.txt | 47 + tests/unit/can/test_misc.cpp | 759 ++++++ tests/unit/can/test_node.cpp | 1175 +++++++++ tests/unit/images/bad-le-crc-x3.bin | Bin 0 -> 576 bytes tests/unit/images/bad-le-short.bin | Bin 0 -> 200 bytes ...33333333333.8b61938ee5f90b1f.app.dirty.bin | Bin 0 -> 576 bytes ...ee0ddf00d.452a4267971a3928.app.release.bin | Bin 0 -> 144 bytes tests/unit/mock.hpp | 220 ++ tests/unit/serial/test_misc.cpp | 35 + tests/unit/serial/test_node.cpp | 323 +++ tests/unit/serial/test_stream.cpp | 667 +++++ tests/unit/test_app_locator.cpp | 66 + tests/unit/test_core.cpp | 509 ++++ tests/unit/test_main.cpp | 2 + tests/unit/test_misc.cpp | 93 + tests/unit/test_presenter.cpp | 388 +++ tests/unit/test_util.cpp | 101 + tests/util/util.hpp | 281 +++ tools/kocherga_image.py | 775 ++++++ tools/pyproject.toml | 6 + xmodem_ymodem_protocol_reference.txt | 2112 ---------------- 82 files changed, 10785 insertions(+), 10203 deletions(-) create mode 100644 .clang-format create mode 100644 .clang-tidy delete mode 100644 .gitmodules create mode 100644 .idea/dictionaries/pavel.xml delete mode 100644 .travis.yml create mode 100644 CONTRIBUTING.md create mode 100644 docs/state_machine.dia rename state_machine.svg => docs/state_machine.svg (77%) delete mode 100644 kocherga.hpp create mode 100644 kocherga/kocherga.hpp create mode 100644 kocherga/kocherga_can.hpp create mode 100644 kocherga/kocherga_serial.hpp delete mode 100644 kocherga_popcop.hpp delete mode 100644 kocherga_uavcan.hpp delete mode 100644 kocherga_ymodem.hpp delete mode 100755 populate_app_descriptor.py delete mode 100644 state_machine.dia delete mode 100644 test/CMakeLists.txt delete mode 100644 test/images.hpp delete mode 160000 test/libcanard delete mode 100644 test/mocks.hpp delete mode 100644 test/piped_process.hpp delete mode 160000 test/popcop delete mode 100755 test/popcop_tester/main.py delete mode 100644 test/populate_app_descriptor/test.image-1.0.bin delete mode 100755 test/populate_app_descriptor/test.sh delete mode 160000 test/senoval delete mode 100644 test/test_core.cpp delete mode 100644 test/test_main.cpp delete mode 100644 test/test_mocks.cpp delete mode 100644 test/test_piped_process.cpp delete mode 100644 test/test_popcop.cpp delete mode 100644 test/test_uavcan_python.cpp delete mode 100644 test/test_util.cpp delete mode 100644 test/test_ymodem.cpp delete mode 100644 test/uavcan_tester/invalid-images/io.px4.sapog-1-2.2.7719d6e.compound.bin delete mode 100644 test/uavcan_tester/invalid-images/with-invalid-header.bin delete mode 100755 test/uavcan_tester/invalid-images/without-header.bin delete mode 100755 test/uavcan_tester/main.py delete mode 100644 test/uavcan_tester/uavcan_tester.py delete mode 100644 test/uavcan_tester/valid-images/a.bin delete mode 100644 test/uavcan_tester/valid-images/io.px4.sapog-1-2.2.7719d6e.application.bin delete mode 100644 test/util.hpp create mode 100644 tests/.clang-tidy rename {test => tests/3rd_party}/catch.hpp (100%) create mode 100644 tests/CMakeLists.txt create mode 100644 tests/integration/.gitignore create mode 100644 tests/integration/CMakeLists.txt create mode 100644 tests/integration/bootloader/main.cpp create mode 100755 tests/integration/validator/can.orc.yaml create mode 120000 tests/integration/validator/com.zubax.kocherga.test.integration-10-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin create mode 100755 tests/integration/validator/manual_v0.orc.yaml create mode 100755 tests/integration/validator/serial.orc.yaml create mode 100755 tests/integration/validator/validate.sh create mode 100644 tests/tools/.gitignore create mode 100644 tests/tools/demo-be.bin create mode 100644 tests/tools/demo-le.bin create mode 100644 tests/tools/invalid.bin create mode 100755 tests/tools/test.sh create mode 100644 tests/unit/CMakeLists.txt create mode 100644 tests/unit/can/test_misc.cpp create mode 100644 tests/unit/can/test_node.cpp create mode 100644 tests/unit/images/bad-le-crc-x3.bin create mode 100644 tests/unit/images/bad-le-short.bin create mode 100644 tests/unit/images/good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin create mode 100644 tests/unit/images/good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin create mode 100644 tests/unit/mock.hpp create mode 100644 tests/unit/serial/test_misc.cpp create mode 100644 tests/unit/serial/test_node.cpp create mode 100644 tests/unit/serial/test_stream.cpp create mode 100644 tests/unit/test_app_locator.cpp create mode 100644 tests/unit/test_core.cpp create mode 100644 tests/unit/test_main.cpp create mode 100644 tests/unit/test_misc.cpp create mode 100644 tests/unit/test_presenter.cpp create mode 100644 tests/unit/test_util.cpp create mode 100644 tests/util/util.hpp create mode 100755 tools/kocherga_image.py create mode 100644 tools/pyproject.toml delete mode 100644 xmodem_ymodem_protocol_reference.txt diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..d6b6f78 --- /dev/null +++ b/.clang-format @@ -0,0 +1,96 @@ +--- +Language: Cpp +# BasedOnStyle: LLVM +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: true +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false + AfterExternBlock: false # Keeps the contents un-indented. +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Custom +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: AfterColon +# BreakInheritanceList: AfterColon +BreakStringLiterals: true +ColumnLimit: 120 +CommentPragmas: '^ (coverity|pragma:)' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeBlocks: Preserve +IndentCaseLabels: false +IndentPPDirectives: AfterHash +IndentWidth: 4 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 10000 # Raised intentionally; prefer breaking all +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 10000 # Raised intentionally because it hurts readability +PointerAlignment: Left +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: false +SpaceAfterCStyleCast: true +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeCtorInitializerColon: true +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInContainerLiterals: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 8 +UseTab: Never +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..5c77544 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,30 @@ +--- +Checks: >- + boost-*, + bugprone-*, + cert-*, + clang-analyzer-*, + cppcoreguidelines-*, + google-*, + hicpp-*, + llvm-*, + misc-*, + modernize-*, + performance-*, + portability-*, + readability-*, + -google-readability-todo, + -readability-avoid-const-params-in-decls, + -readability-function-cognitive-complexity, + -llvm-header-guard, + -google-runtime-references, + -misc-non-private-member-variables-in-classes, + -cppcoreguidelines-non-private-member-variables-in-classes, + -cert-msc30-c, + -cert-msc50-cpp, + -*-easily-swappable-parameters, +WarningsAsErrors: '*' +HeaderFilterRegex: '.*' +AnalyzeTemporaryDtors: false +FormatStyle: file +... diff --git a/.gitignore b/.gitignore index a09c6ea..210b97a 100644 --- a/.gitignore +++ b/.gitignore @@ -27,10 +27,12 @@ cmake-build-* *.log # IDE -.idea .metadata .settings .project .cproject .pydevproject *.kdev4 +**/.idea/* +!**/.idea/dictionaries +!**/.idea/dictionaries/* diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 8356299..0000000 --- a/.gitmodules +++ /dev/null @@ -1,9 +0,0 @@ -[submodule "test/senoval"] - path = test/senoval - url = https://github.com/Zubax/senoval -[submodule "test/popcop"] - path = test/popcop - url = https://github.com/Zubax/popcop -[submodule "test/libcanard"] - path = test/libcanard - url = https://github.com/UAVCAN/libcanard diff --git a/.idea/dictionaries/pavel.xml b/.idea/dictionaries/pavel.xml new file mode 100644 index 0000000..d71207b --- /dev/null +++ b/.idea/dictionaries/pavel.xml @@ -0,0 +1,43 @@ + + + + aaaaaaaa + abcdef + automati + badc + bootloaders + brickproof + castagnoli + cccccccc + cfamily + deadbeef + deja + devirtualization + dsdl + dsonar + encobs + ffee + ffff + gcov + kirienko + kocherga + kochergá + mmmmmmmm + multiframe + nosonar + patchee + prog + pyuavcan + sidrane + telega + tttt + uavcan + unicast + usec + vcan + vssc + vvvvvvvv + zubax + + + \ No newline at end of file diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 0a034e6..0000000 --- a/.travis.yml +++ /dev/null @@ -1,62 +0,0 @@ -sudo: required -dist: trusty - -before_script: - # Configuring a virtual CAN bus interface for UAVCAN protocol testing - - sudo modprobe can - - sudo modprobe can_raw - - sudo modprobe vcan - - sudo ip link add dev kocherga0 type vcan - - sudo ip link set up kocherga0 - - sudo ifconfig kocherga0 up - # Installing dependencies for the YMODEM protocol test - - sudo apt-get install lrzsz - # Installing Python dependencies - - sudo apt-get install python3-pip - - sudo pip3 install "uavcan>=1.0.0.dev30" - -after_script: - - cd test/make_boot_descriptor/ - - ./test.sh - -matrix: - include: - # - # GCC 7 - # - - language: cpp - addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - # We need i386 packages because we compile libcanard in 32-bit mode, it doesn't support 64-bit platforms. - - g++-7 - - g++-7-multilib - - gcc-7-multilib - - linux-libc-dev:i386 - script: - - CC=gcc-7 && CXX=g++-7 && cd test/ && cmake . && make - - ./kocherga_test --rng-seed time - - # - # Clang 5 - # - - language: cpp - addons: - apt: - sources: - - ubuntu-toolchain-r-test - - llvm-toolchain-trusty-5.0 - packages: - # We need i386 packages because we compile libcanard in 32-bit mode, it doesn't support 64-bit platforms. - - clang-5.0 - - libstdc++-7-dev:i386 # This package contains the C++ standard library used by Clang-5.0 - - linux-libc-dev:i386 - - libc6-dev-i386 - script: - - clang++-5.0 -E -x c++ - -v < /dev/null # Print the Clang configuration for troubleshooting purposes - - cd test/ - - cmake -DCMAKE_C_COMPILER=clang-5.0 -DCMAKE_CXX_COMPILER=clang++-5.0 . - - make - - ./kocherga_test --rng-seed time diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..8abdb53 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,75 @@ +# Kocherga contribution guide + +## Directory layout + +The production sources are located under `/kocherga/`. +Do not put anything else in there. + +The tests are located under `/tests/`. +This directory also contains the top `CMakeLists.txt` needed to build and run the tests on the local machine. + +The high-level documentation is written in the main README, API documentation is given directly in the header files. + +## Standards + +The library shall be implemented in ISO C++17 with partial adherence to MISRA C++. +The MISRA compliance is enforced by Clang-Tidy and SonarQube. +Deviations are documented directly in the source code as follows: + +```c +// Intentional violation of MISRA: +<... deviant construct ...> // NOLINT NOSONAR +``` + +The full list of deviations with the accompanying explanation can be found by grepping the sources. + +Do not suppress compliance warnings using the means provided by static analysis tools because such deviations +are impossible to track at the source code level. +An exception applies for the case of false-positive (invalid) warnings -- those should not be mentioned in the codebase. + +[Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah) shall be followed. +Formatting is enforced by Clang-Format; it is used also to fail the CI/CD build if violations are detected. + +Unfortunately, some rules are hard or impractical to enforce automatically, +so code reviewers shall be aware of MISRA and general high-reliability coding practices +to prevent non-compliant code from being accepted into upstream. + +## Tools + +For the full list of the tools please refer to the CI scripts. + +### Clang-Tidy + +Clang-Tidy is used to enforce compliance with MISRA and Zubax Coding Conventions. + +Clang-Tidy is invoked automatically on each translation unit before it is compiled; +the build will fail if the tool is not available locally. +To disable this behavior, pass `NO_STATIC_ANALYSIS=1` to CMake at the generation time. + +### Clang-Format + +Clang-Format is used to enforce compliance with MISRA and Zubax Coding Conventions. + +To reformat the sources, generate the project and build the target `format`; e.g., for Make: `make format`. + +### SonarQube + +SonarQube is a cloud solution so its use is delegated to the CI/CD pipeline. +If you need access, please get in touch with the maintainers. + +### IDE + +The recommended development environment is JetBrains CLion. The root project file can be found under `tests/`. +The repository contains the spelling dictionaries for CLion located under `.idea/`, be sure to use them. + +## Testing + +Generate the CMake project, build all, and then build the target `test` (e.g., `make test`). + +Some of the tests are intended to be run manually due to lack of adequate automation solutions in the v0 ecosystem. +Please navigate to `/tests/integration/validator/` for details. + +## Releasing + +1. Bump the version numbers (`KOCHERGA_VERSION_MAJOR`, `KOCHERGA_VERSION_MINOR`) in `kocherga.hpp`. Push the change. +2. Create a new release on GitHub: diff --git a/README.md b/README.md index d29b19b..b0f5121 100644 --- a/README.md +++ b/README.md @@ -1,154 +1,359 @@ # Kochergá -[![Forum](https://img.shields.io/discourse/https/forum.zubax.com/users.svg)](https://forum.zubax.com) -[![Travis CI build status](https://travis-ci.org/Zubax/kocherga.svg?branch=master)](https://travis-ci.org/Zubax/kocherga) +[![Forum](https://img.shields.io/discourse/https/forum.zubax.com/users.svg?color=e00000)](https://forum.zubax.com) +[![Forum](https://img.shields.io/discourse/https/forum.uavcan.org/users.svg?color=1700b3)](https://forum.uavcan.org) -**Kochergá is a robust portable multi-protocol bootloader for deeply embedded systems.** +**Kochergá is a robust platform-agnostic [UAVCAN](https://uavcan.org) bootloader for deeply embedded systems.** -## Features - -### Portability - -Kochergá has no external dependencies -(although protocol implementations may require their own dependencies). - -Kocherga is written in standard C++17 and is distributed as a header-only library. - -### Robustness +Technical support is provided on the [UAVCAN Forum](https://forum.uavcan.org/). -Kochergá is brick-proof. +A standard-compliant implementation of the firmware update server is provided in +[Yakut](https://github.com/UAVCAN/yakut#updating-node-software). -The application (i.e., firmware) update process can be interrupted at any point (e.g., by turning off the power supply -or by disconnecting the interface), and it is guaranteed that the device will always end up in -a known valid state. -No matter what happens during the update process, Kochergá won't let the user brick the device. - -Even if a misbehaving application image was uploaded, Kochergá always can take control and let the user replace it. - -### Security +## Features -Kochergá verifies the correctness of the application (i.e., firmware) image with a strong 64-bit hash function -before every boot. +- **Portability** -- Kochergá is written in standard C++17 and is distributed as a header-only library with no external + dependencies. -### Supported protocols +- **Robustness** -- Kochergá is brick-proof. The application (i.e., firmware) update process can be interrupted at any + point (e.g., by turning off the power supply or by disconnecting the interface), and it is guaranteed that the device + will always end up in a known valid state. If a dysfunctional application image is uploaded, Kochergá can regain + control after a watchdog reset. -Kochergá supports several communication interfaces and protocols: +- **Safety** -- Kochergá verifies the correctness of the application image with a 64-bit hash before every boot. + Kochergá's own codebase features extensive test coverage. -Interface | Protocols ---------------------|------------------------------------------------------------------------------ -Serial (USB or UART)| XMODEM, YMODEM, XMODEM-1K, Popcop -CAN bus | UAVCAN +- **Multiple supported transports:** + - **UAVCAN/CAN** -- supports both v1 and the legacy v0, the protocol version is auto-detected at runtime. + - **UAVCAN/serial** + - More may appear in the future -- new transports are easy to add. ## Usage -Just read the code ;) - -The entire library is contained in the header file `kocherga.hpp`; -protocol implementations are provided each in a separate header file named `kocherga_*.hpp`. -For example, the YMODEM-family protocols are implemented in the header file `kocherga_ymodem.hpp`; -the UAVCAN protocol is implemented in the file `kocherga_uavcan.hpp`, etc. -You don't need to use all of them, of course; just take what you need. - -Kocherga does not have any compilation units of its own, so just include the required headers and you're ready to roll. +### Integration -When implementing support for new protocols, please follow the same naming convention: -`kocherga_whatever.hpp` for the header file, where `whatever` is the name of the protocol; -the implementation should be contained in a namespace eponymous with the header file, -e.g., `::kocherga_whatever`. +The entire library is contained in the header file `kocherga.hpp`; protocol implementations are provided each in a +separate header file named `kocherga_*.hpp`. Kochergá does not have any compilation units of its own. -To integrate Kocherga into your application, just include this repository as a git submodule, -or simply copy-paste the required header files into your source tree. +To integrate Kochergá into your application, just include this repository as a git subtree/submodule, or simply +copy-paste the required header files into your source tree. -The core logic is implemented in the class `kocherga::BootloaderController`. -Instantiate this class once in your application and use it to perform application updates as necessary -using one of the provided (or custom!) protocol implementations. +### Application signature -The bootloader will be looking for an instance of the `AppInfo` structure located in the ROM image of the -application. -Only if a valid `AppInfo` structure is found the application will be launched. -It is recommended to allocate the structure closer to the beginning of the image in order to speed up its verification. -The structure contains the following fields: +The bootloader looks for an instance of the `AppInfo` structure located in the ROM image of the application at every +boot. Only if a valid `AppInfo` structure is found the application will be launched. It is recommended to allocate the +structure closer to the beginning of the image in order to speed up its verification. The structure is defined as +follows: Offset | Type | Description -------|----------|----------------------------------------------------------------------------------------------------- -0 |`uint8[8]`| Eight constant ASCII characters: `APDesc00`. -8 |`uint64` | CRC-64-WE of the entire application image when this field itself is set to zero. -16 |`uint32` | Size of the application image, in bytes. Note that the image must be padded to eight bytes. -20 |`uint32` | Version control system (VCS) revision ID. For example, a git hash, or an SVN revision number. -24 |`uint8` | Major semantic version number. -25 |`uint8` | Minor semantic version number. -26 |`uint8` | Flags: 1 - this is a release build; 2 - this is a dirty build (uncommitted changes present). -27 | | Reserved; set to 0xFF. -28 |`uint32` | UNIX build timestamp, UTC; i.e., the number of seconds since 1970-01-01T00:00:00Z. - -When computing the application image CRC, the process will eventually encounter the location where the CRC itself -is stored. In order to avoid recursive dependency, the CRC storage location must be replaced with zero bytes -when computing/verifying the CRC. -The parameters of the CRC-64 algorithm are the following: +-16 |`uint64` | Constant value 0x5E4415146FC0C4C7 used for locating the descriptor and detecting the byte order. +-8 |`uint8[8]`| Set to `APDesc00`; used for compatibility with legacy deployments. +0 |`uint64` | CRC-64-WE of the entire application image when this field itself is set to zero. +8 |`uint32` | Size of the application image, in bytes. Note that the image must be padded to eight bytes. +12 |`void32` | Reserved. Used to contain the 32-bit version control system revision ID; see replacement below. +16 |`uint8[2]`| Major and minor semantic version numbers. +18 |`uint8` | Flags: 1 - this is a release build; 2 - this is a dirty build (uncommitted changes present). +19 |`void8` | Reserved; set to 0. +20 |`uint32` | UNIX UTC build timestamp; i.e., the number of seconds since 1970-01-01T00:00:00Z. +24 |`uint64` | Version control system (VCS) revision ID (e.g., the git commit hash). +32 |`void64` | Reserved. +40 |`void64` | Reserved. + +When computing the application image CRC, the process will eventually encounter the location where the CRC itself is +stored. In order to avoid recursive dependency, the CRC storage location must be replaced with zero bytes when +computing/verifying the CRC. The parameters of the CRC-64 algorithm are the following: + * Initial value: 0xFFFF'FFFF'FFFF'FFFF * Polynomial: 0x42F0'E1EB'A9EA'3693 * Reverse: no * Output xor: 0xFFFF'FFFF'FFFF'FFFF * Check: 0x62EC'59E3'F1A4'F00A -The CRC and size fields cannot be populated until after the application binary is compiled and linked. -A possible way to populate these fields is to initialize them with zeroes in the source code, -and then use the script `populate_app_descriptor.py` after the binary is generated to update the fields -with their actual values. -The script can be invoked from the build system (e.g., from a Makefile rule) trivially as follows: +The CRC and size fields cannot be populated until after the application binary is compiled and linked. One possible way +to populate these fields is to initialize them with zeroes in the source code, and then use the +script `tools/kocherga_image.py` after the binary is generated to update the fields with their actual values. The script +can be invoked from the build system (e.g., from a Makefile rule) trivially as follows: ```sh -populate_app_descriptor.py firmware.bin +kocherga_image.py application-name-goes-here.bin ``` -The following diagram documents the state machine implemented in the `BootloaderController` class: -![Kocherga State Machine Diagram](state_machine.svg "Kocherga State Machine Diagram") +The output will be stored in a file whose name follows the pattern expected by the firmware update server implemented in +the [Yakut CLI tool](https://github.com/UAVCAN/yakut#updating-node-software). -## Protocols +### State machine -As has been stated earlier, Kocherga itself is dependency-free. -However, protocol implementations may require additional dependencies themselves. -They are documented here. +The following diagram documents the state machine of the bootloader: -### XMODEM/YMODEM/XMODEM-1K +![Kochergá State Machine Diagram](docs/state_machine.svg "Kochergá State Machine Diagram") -No additional dependencies are needed. +The bootloader states are mapped onto UAVCAN node states as follows: -### UAVCAN +Bootloader state | Node mode | Node health| Vendor-specific status code +---------------------|-----------------|------------|------------------------------- +NoAppToBoot |`SOFTWARE_UPDATE`| `WARNING` | 0 +BootDelay |`SOFTWARE_UPDATE`| `NOMINAL` | 0 +BootCancelled |`SOFTWARE_UPDATE`| `ADVISORY` | 0 +AppUpdateInProgress |`SOFTWARE_UPDATE`| `NOMINAL` | number of read requests, always >0 + +### API usage + +The following snippet demonstrates how to integrate Kochergá into your bootloader executable. +User-provided functions are shown in `SCREAMING_SNAKE_CASE()`. +This is a stripped-down example; the full API documentation is available in the header files. + +The integration test application available under `/tests/integration/bootloader/` may also be a good reference. + +#### ROM interface + +The ROM backend abstracts the specifics of reading and writing your ROM (usually this is the on-chip flash memory). + +```c++ +class MyROMBackend : public kocherga::IROMBackend +{ + auto write(const std::size_t offset, const std::byte* const data, const std::size_t size) override + -> std::optional + { + if (WRITE_ROM(offset, data, size)) + { + return size; + } + return {}; // Failure case + } + + auto read(const std::size_t offset, std::byte* const out_data, const std::size_t size) const override + -> std::size_t + { + return READ_ROM(offset, out_data, size); // Return the number of bytes read (may be less than size). + } +}; +``` -The UAVCAN protocol support requires the following libraries: +#### Media layer interfaces + +Transport implementations --- UAVCAN/CAN, UAVCAN/serial, etc., depending on which transports you need --- +are interfaced with your hardware as follows. + +```c++ +class MySerialPort : public kocherga::serial::ISerialPort +{ + auto receive() -> std::optional override + { + if (SERIAL_RX_PENDING()) + { + return SERIAL_READ_BYTE(); + } + return {}; + } + + auto send(const std::uint8_t b) -> bool override { return SERIAL_WRITE_BYTE(b); } +}; +``` -* [Libcanard](http://uavcan.org/Implementations/Libcanard) - a lightweight UAVCAN stack implementation in C99. -* [Senoval](https://github.com/Zubax/senoval) - an utility library for deeply embedded systems. +```c++ +class MyCANDriver : public kocherga::can::ICANDriver +{ + auto configure(const Bitrate& bitrate, + const bool silent, + const kocherga::can::CANAcceptanceFilterConfig& filter) -> std::optional override + { + CAN_CONFIGURE(bitrate, silent, filter); + return Mode::FD; // Or Mode::Classic if CAN FD is not supported by the CAN controller. + } + + auto push(const bool force_classic_can, + const std::uint32_t extended_can_id, + const std::uint8_t payload_size, + const void* const payload) -> bool override + { + if (force_classic_can) + { + return CAN_PUSH_CLASSIC(extended_can_id, payload_size, payload); // No DLE, no BRS -- Classic only. + } + else + { + return CAN_PUSH_FD(extended_can_id, payload_size, payload); + } + } + + auto pop(PayloadBuffer& payload_buffer) -> std::optional> override + { + return CAN_POP(payload_buffer.data()); // The return value is optional(can_id, payload_size). + } +}; +``` -The bootloader states are mapped onto UAVCAN node states as follows: +#### Passing arguments from the application + +When the application is commanded to upgrade itself, it needs to store relevant context into a struct, +write this struct into a pre-defined memory location, and then reboot. +The bootloader would check that location to see if there is a valid argument struct in it. +Kochergá provides a convenient class for that --- `kocherga::VolatileStorage<>` --- +which checks the presence and validity of the arguments with a strong 64-bit CRC. + +```c++ +/// The application may pass this structure when rebooting into the bootloader. +/// Feel free to modify the contents to suit your system. +/// It is a good idea to include an explicit version field here for future-proofing. +struct ArgumentsFromApplication +{ + bool linger; ///< Whether to boot immediately or to wait for commands. + + std::uint16_t uavcan_serial_node_id; ///< Invalid if unknown. + + std::pair uavcan_can_bitrate; ///< Zeros if unknown. + std::uint8_t uavcan_can_protocol_version; ///< v0 or v1; 0xFF if unknown. + std::uint8_t uavcan_can_node_id; ///< Invalid if unknown. + + std::uint8_t trigger_node_index; ///< 0 - serial, 1 - CAN, >1 - none. + std::uint16_t file_server_node_id; ///< Invalid if unknown. + std::array remote_file_path; ///< Null-terminated string. +}; +static_assert(std::is_trivial_v); +``` -Bootloader state | Node mode | Node health ----------------------|----------------|---------------- -NoAppToBoot | SoftwareUpdate | Error -BootDelay | Initialization | Ok -BootCancelled | SoftwareUpdate | Warning -AppUpgradeInProgress | SoftwareUpdate | Ok -ReadyToBoot | Initialization | Ok +#### Running the bootloader + +```c++ +#include // Pick the transports you need. +#include // In this example we are using UAVCAN/serial + UAVCAN/CAN. + +/// Maximum possible size of the application image for your platform. +static constexpr std::size_t MaxAppSize = 1024 * 1024; + +int main() +{ + // Check if the application has passed any arguments to the bootloader via shared RAM. + // The address where the arguments are stored obviously has to be shared with the application. + // If the application uses heap, then it might be a good idea to alias this area with the heap. + const std::optional args = + kocherga::VolatileStorage(reinterpret_cast(0x2000'4000U)).take(); + + // Initialize the bootloader core. + MyROMBackend rom_backend; + kocherga::SystemInfo system_info = GET_SYSTEM_INFO(); + kocherga::Bootloader boot(rom_backend, system_info, MaxAppSize, args && args->linger); + + // Add a UAVCAN/serial node to the bootloader instance. + MySerialPort serial_port; + kocherga::serial::SerialNode serial_node(serial_port, system_info.unique_id); + if (args && (args->uavcan_serial_node_id <= kocherga::serial::MaxNodeID)) + { + serial_node.setLocalNodeID(args->uavcan_serial_node_id); + } + boot.addNode(&serial_node); + + // Add a UAVCAN/CAN node to the bootloader instance. + std::optional can_bitrate; + std::optional uavcan_can_version; + std::optional uavcan_can_node_id; + if (args) + { + if (args->uavcan_can_bitrate.first > 0) + { + can_bitrate = ICANDriver::Bitrate{args.uavcan_can_bitrate.first, args.uavcan_can_bitrate.second}; + } + uavcan_can_version = args->uavcan_can_protocol_version; // Will be ignored if invalid. + uavcan_can_node_id = args->uavcan_can_node_id; // Will be ignored if invalid. + } + MyCANDriver can_driver; + kocherga::can::CANNode can_node(can_driver, + system_info.unique_id, + can_bitrate, + uavcan_can_version, + uavcan_can_node_id); + boot.addNode(&can_node); + + // Trigger the update process internally if the required arguments are provided by the application. + if (args && (args->trigger_node_index < 2)) + { + (void) boot.trigger(args->trigger_node_index, // Use serial or CAN? + args->file_server_node_id, // Which node to download the file from? + std::strlen(args->remote_file_path.data()), // Remote file path length. + args->remote_file_path.data()); + } + + while (true) + { + const auto uptime = GET_TIME_SINCE_BOOT(); + if (const auto fin = boot.poll(std::chrono::duration_cast(uptime))) + { + if (*fin == kocherga::Final::BootApp) + { + BOOT_THE_APPLICATION(); + } + if (*fin == kocherga::Final::Restart) + { + RESTART_THE_BOOTLOADER(); + } + assert(false); + } + // Sleep until the next hardware event (like reception of CAN frame or UART byte) but no longer than + // 1 second. A fixed sleep is also acceptable but the resulting polling interval should be adequate + // to avoid data loss (about 100 microseconds is usually ok). + WAIT_FOR_EVENT(); + } +} +``` -### Popcop +#### Building a compliant application image + +Define the following application signature structure somewhere in your application: + +```c++ +struct AppDescriptor +{ + std::uint64_t magic = 0x5E44'1514'6FC0'C4C7ULL; + std::array signature{{'A', 'P', 'D', 'e', 's', 'c', '0', '0'}}; + + std::uint64_t image_crc = 0; // Populated after build + std::uint32_t image_size = 0; // Populated after build + [[maybe_unused]] std::array _reserved_a{}; + std::uint8_t version_major = SOFTWARE_VERSION_MAJOR; + std::uint8_t version_minor = SOFTWARE_VERSION_MINOR; + std::uint8_t flags = +#if RELEASE_BUILD + Flags::ReleaseBuild +#else + 0 +#endif + ; + [[maybe_unused]] std::array _reserved_b{}; + std::uint32_t build_timestamp_utc = TIMESTAMP_UTC; + std::uint64_t vcs_revision_id = GIT_HASH; + [[maybe_unused]] std::array _reserved_c{}; + + struct Flags + { + static constexpr std::uint8_t ReleaseBuild = 1U; + static constexpr std::uint8_t DirtyBuild = 2U; + }; +}; + +static const volatile AppDescriptor g_app_descriptor; +// Optionally, use explicit placement near the beginning of the binary: +// __attribute__((used, section(".app_descriptor"))); +// and define the .app_descriptor section in the linker file. +``` -The Popcop protocol support requires the following libraries: +Then modify your build script to invoke `kocherga_image.py` as explained earlier. -* [Libpopcop](https://github.com/Zubax/popcop) - an implementation of the Popcop protocol in C++. +#### Rebooting into the bootloader from the application -## Development +If the application needs to pass arguments to the bootloader, it can be done with the help of +`kocherga::VolatileStorage<>`. +Ensure that the definition of `ArgumentsFromApplication` used by the application and by the bootloader use +the same binary layout. -You will need a GNU/Linux machine for development. +Note that the application does not need to depend on the Kochergá library. +It is recommended to copy-paste relevant pieces from Kochergá instead; specifically: -The code must follow the [Zubax Coding Conventions](https://kb.zubax.com/x/84Ah). -The code must be compilable using at least GCC and CLang -(assuming relatively modern versions here; see the CI script for details). +- `kocherga::VolatileStorage<>` +- `kocherga::detail::CRC64` -Build and run the tests like this: `cd test/ && cmake . && make && ./kocherga_test`. -Don't forget to configure the environment beforehand; -please refer to the CI build script for details. +## Revisions -## License +### v0.1 -Kochergá is available under the terms of the MIT License. +The first revision to go public. diff --git a/docs/state_machine.dia b/docs/state_machine.dia new file mode 100644 index 0000000000000000000000000000000000000000..b4fb47251f963bfdbe12672d299c94c41cbb7ca5 GIT binary patch literal 2328 zcmV+z3Fr17iwFP!000021MOX1Z{s!=efO^rJTF_ol|@q2N7JO3+1;WA7TpDQx(|IZ zXp6Dc%92;2Kjcr?1H(Kk2ElD~L<3m4uY@&Q1py@#f`Cwq*^DWnQ z!Rwfu?{3_mj;?#HZM?ZguFzW5`(2 zaQ}}a%1-G({`QyEcJ9iZr(tk>;qF<~w6X~UwwOFC8m_$c7e%co{fj%5G z@=JfB(Gz9ijO>Es`FC(InJYgoIpua4k|pbm`Wa$g;%B(M;4g z6A23X8|6Y(^N?yDvO$kLR9;W-mWN&fsZ*MZ zG#9DjqQ8=O#^*qw=}0vlVK9QW>1aCYm5$H_0(8((9-xDG8=ym$`T$+qF5NyAiItqcvY^dh4Q|D3Cn@s86D} z@vY0hIphW;4LDEyBaExIEx3CW&9rRa8v$2wAk7y}9MeG_yu;YE94Ml$$N?=?^elJ3tMXk`RRx{XXv9BswP7(nfD@9Y$)@aJJNY5(*z_c# z@%$L|w&s(qiw1H(*@Ku>eD3l!mK}`v>tBooUzG)_vqYv;jqP9ghwU8JDwSG2bMb!` zoVZinTr7U4^Wq049zjcO5MB5HTrTomrpV_)1f@?0TUQeLG-fjPse_-rIiu3S>yQazlbhDCQ7UE+2Qr@FH2!}e9`5}50e5%7dQaqnO*NJN^C>l2_@m{fJK zI`|@jz81PdVbVTGs)fk~go#xzOzJNN#l=X8s%egWd0n9cW3>fpa;s1js5zjQ0<{iZ zIwkhyyuCz>4t1R=QrUtI$Z8dFWhj=Uma5A0^fI2|>R{L9gI&Lc^WVt4BrRWez5XR~ zBd8Bm31xP~GA-BQQcuLZ+PY2IV$!H5OZ{qLAQWk%f#AK?xh2t&Bn|f`;jsyLY$H{F z-8uI})-2&1!zy^-*eTA-%jGVIezaV^fLyls6|Z}aTDDZj0FPoK=zP8qd1^rsxuy?& zRDhxl@{M*6py)M$Vv~U=4{tQkps14@?*bIPb3pNHB}PE^LvEhPTBto%KTIlRvUKnJP!&tKopsYx-Tnt>-A7& zR@uqwbZ+Z$(7D|AyAOk3H^5|H;;|u$YdMfu{=kt@cPEd!L#xB$KQY4OG?bP$8{lSx za!phfuAFP~7K2ppEV!jX^(|HIdi`Q*=i+^NCRm6LPbIKgvfQO8-G`8G2t|pU*Sh58 zG+3?bTa{I_!EtPb?Q8x|!o&;Up3Y`!Y&##@P?zFA>#G8zbAjq%WT_RKJ}z>ZPW~_k zpc)VNyHsreju|+x+28`HP%SWZ_Edv#4G6;y=~KI}>}lu28wiMp(3g5iZG`8ex{_Ep(Pbc{a5?Q*|!& z8gr?g&y3izKv(HG*t~RBBvCOkEpZB#@2TzUIRzVtr_mbpc@?HK82sf=nkL5dg}?ns z))D(gW0mr=vE@0cvFaMJ%J06p6=$TCo?wt3poxUOLdb-!=WvfExF&s-(*Le$rl0%M z%LC#{Gx8Rm4)^Ym30fD(M~Hc#x*|r zg#nUZZ_wGMF=2}(SmU>yaJ|VwlkN8mxA4#yL - + - - NoAppToBoot + + NoAppToBoot - - BootDelay + + BootDelay - - BootCancelled + + BootCanceled - - AppUpgradeInProgress - - - - - - - ReadyToBoot + + AppUpdateInProgress - - Bootloader started + + Bootloader started @@ -57,24 +50,13 @@ - - - - Starting the - application + + + + Boot the + application - - - - - - - ReadyToBoot is an - auxiliary state, it is - left automatically - as soon as possible - @@ -107,22 +89,22 @@ - + - Upgrade successful, received image is valid + Update successful, received image is valid Boot delay expired (zero by default) - - Upgrade requested + + Update requested - Upgrade failed, + Update failed, no valid image is now available @@ -133,7 +115,7 @@ - Upgrade failed, + Update failed, but the existing valid image was not altered diff --git a/kocherga.hpp b/kocherga.hpp deleted file mode 100644 index f38b38c..0000000 --- a/kocherga.hpp +++ /dev/null @@ -1,902 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2018 Zubax Robotics - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of - * this software and associated documentation files (the "Software"), to deal in - * the Software without restriction, including without limitation the rights to - * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - * the Software, and to permit persons to whom the Software is furnished to do so, - * subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - * Author: Pavel Kirienko - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * This macro can be defined by the application to provide debug output. - * By default, all trace outputs are removed by the preprocessor. - * The expected signature is that of std::printf(). - */ -#ifndef KOCHERGA_TRACE -# define KOCHERGA_TRACE(...) (void)0 -#endif - - -namespace kocherga -{ -/** - * Error codes. - * These are returned from functions in negated form, e.g., -1000 means error code 1000. - */ -static constexpr std::int16_t ErrOK = 0; -static constexpr std::int16_t ErrInvalidState = 1001; -static constexpr std::int16_t ErrAppImageTooLarge = 1002; -static constexpr std::int16_t ErrROMWriteFailure = 1003; -static constexpr std::int16_t ErrInvalidParams = 1004; - -/** - * The library performs operations on data blocks not larger than this. - * It is a hard guarantee that the library will NEVER deliver to the application a larger data block than this. - * If the application attempts to pass a larger block to the library, the library will return an error. - */ -static constexpr std::uint16_t MaxDataBlockSize = 32767; - -/** - * This is used to verify integrity of the application and other data. - * Note that firmware CRC verification is a computationally expensive process that needs to be completed - * in a limited time interval, which should be minimized. Therefore, this class has been carefully manually - * optimized to achieve the optimal balance between speed and ROM footprint. - * - * CRC-64-WE - * Description: http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 - * Initial value: 0xFFFFFFFFFFFFFFFF - * Poly: 0x42F0E1EBA9EA3693 - * Reverse: no - * Output xor: 0xFFFFFFFFFFFFFFFF - * Check: 0x62EC59E3F1A4F00A - */ -class CRC64 -{ - static constexpr std::uint64_t Poly = std::uint64_t(0x42F0E1EBA9EA3693ULL); - static constexpr std::uint64_t Mask = std::uint64_t(1) << 63U; - - std::uint64_t crc_ = std::uint64_t(0xFFFFFFFFFFFFFFFFULL); - -public: - void add(const void* data, std::size_t len) - { - auto bytes = static_cast(data); - assert(bytes != nullptr); - while (len --> 0) - { - crc_ ^= std::uint64_t(*bytes++) << 56U; - // Unrolled for performance reasons. This path directly affects the boot-up time, so it is very - // important to keep it optimized for speed. Rolling this into a loop causes a significant performance - // degradation at least with GCC since the compiler refuses to unroll the loop when size optimization - // is selected (which is normally used for bootloaders). - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - crc_ = (crc_ & Mask) ? (crc_ << 1U) ^ Poly : crc_ << 1U; - } - } - - std::uint64_t get() const { return crc_ ^ 0xFFFFFFFFFFFFFFFFULL; } -}; - -/** - * Bootloader controller states. - * Some of the states are designed as commands to the outer logic, e.g: - * @ref ReadyToBoot means that the application should be started. - * - * The following state transition diagram illustrates the operating principles of the bootloader controller class: - * - * No valid application found ###################### Valid application found - * /----------------# Bootloader started #----------\ /-------------------------------------------\ - * | ###################### | | | - * v v v Boot delay expired | - * +-------------+ +-----------+ (typically zero) +-------------+ | - * /-->| NoAppToBoot | /----------------------| BootDelay |------------------->| ReadyToBoot | | - * | +-------------+ / +-----------+ +-------------+ | - * | | / |Boot cancelled, |ReadyToBoot is | - * |Upgrade |<-----------/ |e.g., received a state transition|an auxiliary / - * |failed, |Upgrade requested, |request to BootCancelled. |state, it is / - * |no valid |e.g., received a state transition v |left automati- / - * |image is |request to AppUpgradeInProgress. +---------------+ |cally ASAP. / - * |now ava- |<--------------------------------| BootCancelled | v / - * |ilable | +---------------+ ############### / - * | v ^ # Booting the # / - * | +----------------------+ Upgrade failed, but the | # application # / - * \-| AppUpgradeInProgress |--------------------------/ ############### / - * +----------------------+ existing valid image was not / - * | altered and remains valid. / - * | / - * | Upgrade successful, received image is valid. / - * \--------------------------------------------------------------------------------/ - */ -enum class State : std::uint8_t -{ - NoAppToBoot, - BootDelay, - BootCancelled, - AppUpgradeInProgress, - ReadyToBoot -}; - -/** - * These fields are defined by the Brickproof Bootloader specification. - * Observe that the fields are ordered from largest to smallest in order to avoid padding. - */ -struct AppInfo -{ - static constexpr std::uint8_t FlagReleaseBuild = 1U; - static constexpr std::uint8_t FlagDirtyBuild = 2U; - - // Offset 0 bytes - std::uint64_t image_crc = 0; ///< CRC-64-WE of the firmware image padded to 8 bytes while this field zero - - // Offset 8 bytes - std::uint32_t image_size = 0; ///< Size of the application image in bytes - std::uint32_t vcs_commit = 0; ///< Version control system revision ID (e.g., git commit hash) - - // Offset 16 bytes - std::uint8_t major_version = 0; ///< Major semantic version number - std::uint8_t minor_version = 0; ///< Minor semantic version number - std::uint8_t flags = 0; ///< Flags: 1 - release build, 2 - dirty build - std::uint8_t _reserved_a_ = 0; ///< Reserved for future use - - // Offset 20 bytes - std::uint32_t build_timestamp_utc = 0; ///< UTC Unix time in seconds when the application was built - - bool isReleaseBuild() const { return (flags & FlagReleaseBuild) != 0; } - bool isDirtyBuild() const { return (flags & FlagDirtyBuild) != 0; } - - /// Returns true if the build timestamp is not zero and not 2**32-1. Used for compatibility with older formats. - bool isBuildTimestampValid() const - { - return (build_timestamp_utc != 0) && (build_timestamp_utc != 0xFF'FF'FF'FFUL); - } -}; - -static_assert(std::is_standard_layout_v, "AppInfo is not standard layout; check your compiler"); - -/** - * This interface abstracts the platform-specific functionality. - * The implementation depends on the hardware and whether there is an operating system. - */ -class IPlatform -{ -public: - virtual ~IPlatform() = default; - - /** - * Mutex lock/unlock functions are only meaningful in multi-threaded environments, - * where the bootloader's instance can be accessed concurrently from multiple threads. - * Single-thread environments and also environments where the bootloader's instance is not - * shared across different threads should not implement these methods. - * Note that the mutex must be recursive. - */ - virtual void lockMutex() { } - virtual void unlockMutex() { } - - /** - * Returns the time since boot as a monotonic (i.e., steady) clock. - * The clock must never overflow. - * This method is invoked only when the mutex is locked. - */ - virtual std::chrono::microseconds getMonotonicUptime() const = 0; -}; - -/** - * This interface abstracts the target-specific ROM routines. - * Upgrade scenario: - * 1. beginUpgrade() - * 2. write() repeated until finished. - * 3. endUpgrade(success or not) - * - * Please note that the performance of the ROM reading routine is critical. - * Slow access may lead to watchdog timeouts (assuming that the watchdog is used), - * disruption of communications, and premature expiration of the boot timeout. - */ -class IROMBackend -{ -public: - virtual ~IROMBackend() = default; - - /** - * @return 0 on success, negative on error - */ - virtual std::int16_t beginUpgrade() = 0; - - /** - * The size cannot exceed 32767 bytes. - * @return number of bytes written; negative on error - */ - virtual std::int16_t write(std::size_t offset, const void* data, std::uint16_t size) = 0; - - /** - * @return 0 on success, negative on error - */ - virtual std::int16_t endUpgrade(bool success) = 0; - - /** - * The size cannot exceed 32767 bytes. - * @return number of bytes read; negative on error - */ - virtual std::int16_t read(std::size_t offset, void* data, std::uint16_t size) const = 0; -}; - -/** - * This interface proxies data received by the protocol into the bootloader. - */ -class IDownloadSink -{ -public: - virtual ~IDownloadSink() = default; - - /** - * The data chunk length cannot exceed 32767 bytes. - * @return Negative on error, non-negative on success. - */ - virtual std::int16_t handleNextDataChunk(const void* data, std::uint16_t size) = 0; -}; - -/** - * Inherit this class to implement firmware loading protocol, from remote to the local storage. - */ -class IProtocol -{ -public: - virtual ~IProtocol() = default; - - /** - * Performs the download operation synchronously. - * Every received data chunk is fed into the sink using the corresponding method (refer to the interface - * definition). If the sink returns error, downloading will be aborted. - * @return Negative on error, 0 on success. - */ - virtual std::int16_t downloadImage(IDownloadSink& sink) = 0; -}; - -/** - * Main bootloader controller. - * Beware that this class has a large buffer field used to cache ROM reads. Do not allocate it on the stack. - * See the @ref State enum definition for state transition logic. - */ -class BootloaderController final -{ - /** - * RAII mutex manager. - */ - class MutexLocker final - { - IPlatform& pl_; - public: - explicit MutexLocker(IPlatform& pl) : pl_(pl) { pl_.lockMutex(); } - ~MutexLocker() { pl_.unlockMutex(); } - }; - - /** - * A proxy that streams the data from the protocol into the application storage. - * Note that every access to the storage backend is protected with the mutex! - */ - class ProxySink : public IDownloadSink - { - IPlatform& platform_; - IROMBackend& backend_; - const std::size_t max_image_size_; - std::size_t offset_ = 0; - - std::int16_t handleNextDataChunk(const void* data, std::uint16_t size) final - { - if (size > MaxDataBlockSize) - { - return -ErrInvalidParams; - } - - MutexLocker mlock(platform_); - - if ((offset_ + size) <= max_image_size_) - { - const auto res = backend_.write(offset_, data, size); - if ((res >= 0) && (res != int(size))) - { - return -ErrROMWriteFailure; - } - - offset_ += size; - return res; - } - else - { - return -ErrAppImageTooLarge; - } - } - - public: - ProxySink(IPlatform& pl, - IROMBackend& back, - std::size_t max_image_size) : - platform_(pl), - backend_(back), - max_image_size_(max_image_size) - { } - }; - - State state_{}; - IPlatform& platform_; - IROMBackend& backend_; - - const std::uint32_t max_application_image_size_; - const std::chrono::microseconds boot_delay_; - std::chrono::microseconds boot_delay_started_at_{}; - - /// Larger buffer enables faster CRC verification, which is important, especially with large firmwares! - std::array rom_buffer_{}; - - /// Caching is needed because app check can sometimes take a very long time (several seconds) - std::optional cached_app_info_; - - /** - * Refer to the Brickproof Bootloader specs. - * Note that the structure must be aligned at 8 bytes boundary, and the image must be padded to 8 bytes! - */ - struct AppDescriptor - { - static constexpr std::size_t ImagePaddingBytes = 8; - - alignas(8) std::array signature{}; - alignas(8) AppInfo app_info; // Being explicit about expected memory layout - - static constexpr std::array getSignatureValue() - { - return {{'A','P','D','e','s','c','0','0'}}; - } - - bool isValid(const std::uint32_t max_application_image_size) const - { - const auto sgn = getSignatureValue(); - return std::equal(std::begin(signature), std::end(signature), std::begin(sgn)) && - (app_info.image_size > 0) && - (app_info.image_size <= max_application_image_size) && - ((app_info.image_size % ImagePaddingBytes) == 0); - } - }; - static_assert(sizeof(AppDescriptor) == 32, "Invalid packing"); - static_assert(std::is_standard_layout_v, "AppInfo is not standard layout; check your compiler"); - static_assert(offsetof(AppDescriptor, app_info) + offsetof(AppInfo, image_crc) == 8); - - std::optional locateAppDescriptor() - { - constexpr auto Step = 8; - - for (std::size_t offset = 0;; offset += Step) - { - // Reading the storage in 8 bytes increments until we've found the signature - { - std::uint8_t signature[Step] = {}; - const auto res = backend_.read(offset, signature, sizeof(signature)); - if (res != std::int16_t(sizeof(signature))) - { - break; - } - const auto reference = AppDescriptor::getSignatureValue(); - if (!std::equal(std::begin(signature), std::end(signature), std::begin(reference))) - { - continue; - } - } - - // Reading the entire descriptor - AppDescriptor desc; - { - const auto res = backend_.read(offset, &desc, sizeof(desc)); - if (res != std::int16_t(sizeof(desc))) - { - break; - } - if (!desc.isValid(max_application_image_size_)) - { - continue; - } - } - - // Checking firmware CRC. - // This block is computationally expensive, so it has been carefully optimized for speed. - { - const auto crc_offset = offset + offsetof(AppDescriptor, app_info) + offsetof(AppInfo, image_crc); - CRC64 crc; - - // Read large chunks until the CRC field is reached (in most cases it will fit in just one chunk) - for (std::size_t i = 0; i < crc_offset;) - { - const auto res = - backend_.read(i, rom_buffer_.data(), - std::uint16_t(std::min(rom_buffer_.size(), crc_offset - i))); - if (res > 0) - { - i += std::size_t(res); - crc.add(rom_buffer_.data(), std::size_t(res)); - } - else - { - break; - } - } - - // Fill CRC with zero - { - static const std::uint8_t dummy[8]{0}; - crc.add(&dummy[0], sizeof(dummy)); - } - - // Read the rest of the image in large chunks - for (std::size_t i = crc_offset + 8; i < desc.app_info.image_size;) - { - const auto res = backend_.read(i, rom_buffer_.data(), - std::uint16_t(std::min(rom_buffer_.size(), - desc.app_info.image_size - i))); - if (res > 0) - { - i += std::size_t(res); - crc.add(rom_buffer_.data(), std::size_t(res)); - } - else - { - break; - } - } - - if (crc.get() != desc.app_info.image_crc) - { - KOCHERGA_TRACE("App descriptor found, but CRC is invalid\n"); - continue; // Look further... - } - } - - // Returning if the descriptor is correct - KOCHERGA_TRACE("App descriptor located at offset %x\n", unsigned(offset)); - return {desc}; - } - - return {}; - } - - void verifyAppAndUpdateState(const State state_on_success) - { - if (const auto appdesc = locateAppDescriptor()) - { - cached_app_info_ = appdesc->app_info; - state_ = state_on_success; - boot_delay_started_at_ = - platform_.getMonotonicUptime(); // This only makes sense if the new state is BootDelay - KOCHERGA_TRACE("App found; version %u.%u.%x, flags %u, built %u, %u bytes\n", - unsigned(appdesc->app_info.major_version), - unsigned(appdesc->app_info.minor_version), - unsigned(appdesc->app_info.vcs_commit), - unsigned(appdesc->app_info.flags), - unsigned(appdesc->app_info.build_timestamp_utc), - unsigned(appdesc->app_info.image_size)); - } - else - { - cached_app_info_.reset(); - state_ = State::NoAppToBoot; - KOCHERGA_TRACE("App not found\n"); - } - } - -public: - /** - * Time since boot will be measured starting from the moment when the object was constructed. - * - * The max application image size parameter is very important for performance reasons; - * without it, the bootloader may encounter an unrelated data structure in the ROM that looks like a - * valid app descriptor (by virtue of having the same signature, which is only 64 bit long), - * and it may spend a considerable amount of time trying to check the CRC that is certainly invalid. - * Having an upper size limit for the application image allows the bootloader to weed out too large - * values early, greatly improving the worst case boot time. - * - * By default, the boot delay is set to zero; i.e., if the application is valid it will be launched immediately. - */ - BootloaderController(IPlatform& platform, - IROMBackend& rom_backend, - std::uint32_t max_application_image_size = 0xFFFFFFFFUL, - std::chrono::microseconds boot_delay = std::chrono::microseconds(0)) : - platform_(platform), - backend_(rom_backend), - max_application_image_size_(max_application_image_size), - boot_delay_(boot_delay) - { - MutexLocker mlock(platform_); - verifyAppAndUpdateState(State::BootDelay); - } - - /** - * @ref State. - */ - State getState() - { - MutexLocker mlock(platform_); - if ((state_ == State::BootDelay) && - ((platform_.getMonotonicUptime() - boot_delay_started_at_) >= boot_delay_)) - { - KOCHERGA_TRACE("Boot delay expired\n"); - state_ = State::ReadyToBoot; - } - - return state_; - } - - /** - * If there is a valid application in the ROM, returns info about it. - * Otherwise returns an empty option. - */ - std::optional getAppInfo() - { - MutexLocker mlock(platform_); - if (cached_app_info_) - { - return *cached_app_info_; - } - else - { - return {}; - } - } - - /** - * Switches the state to @ref BootCancelled, if allowed. - */ - void cancelBoot() - { - MutexLocker mlock(platform_); - switch (state_) - { - case State::BootDelay: - case State::ReadyToBoot: - { - state_ = State::BootCancelled; - KOCHERGA_TRACE("Boot cancelled\n"); - break; - } - case State::NoAppToBoot: - case State::BootCancelled: - case State::AppUpgradeInProgress: - { - break; - } - } - } - - /** - * Switches the state to @ref ReadyToBoot, if allowed. - */ - void requestBoot() - { - MutexLocker mlock(platform_); - - switch (state_) - { - case State::BootDelay: - case State::BootCancelled: - { - state_ = State::ReadyToBoot; - KOCHERGA_TRACE("Boot requested\n"); - break; - } - case State::NoAppToBoot: - case State::AppUpgradeInProgress: - case State::ReadyToBoot: - { - break; - } - } - } - - /** - * Template method that implements all of the high-level steps of the application update procedure. - * Returns zero on success, negative on failure. - */ - std::int16_t upgradeApp(IProtocol& proto) - { - /* - * Preparation stage. - * Note that access to the backend and all members is always protected with the mutex, this is important. - */ - { - MutexLocker mlock(platform_); - - switch (state_) - { - case State::BootDelay: - case State::BootCancelled: - case State::NoAppToBoot: - { - break; // OK, continuing below - } - case State::ReadyToBoot: - case State::AppUpgradeInProgress: - { - return -ErrInvalidState; - } - } - - state_ = State::AppUpgradeInProgress; - cached_app_info_.reset(); // Invalidate now, as we're going to modify the storage - - const auto res = backend_.beginUpgrade(); - if (res < 0) - { - verifyAppAndUpdateState(State::BootCancelled); // The backend could have modified the storage - return res; - } - } - - KOCHERGA_TRACE("Starting app upgrade...\n"); - - /* - * Downloading stage. - * New application is downloaded into the storage backend via the ProxySink proxy class. - * Every write() via the ProxySink is mutex-protected. - */ - ProxySink sink(platform_, backend_, max_application_image_size_); - - auto res = proto.downloadImage(sink); - KOCHERGA_TRACE("App download finished with status %d\n", res); - - /* - * Finalization stage. - * Checking if the protocol has succeeded, checking if the backend is able to finalize successfully. - * Notice the mutex. - */ - MutexLocker mlock(platform_); - - assert(state_ == State::AppUpgradeInProgress); - state_ = State::NoAppToBoot; // Default state until proven otherwise - - if (res < 0) // Download failed - { - (void)backend_.endUpgrade(false); // Making sure the backend is finalized; error is irrelevant - verifyAppAndUpdateState(State::BootCancelled); - return res; - } - - res = backend_.endUpgrade(true); - if (res < 0) // Finalization failed - { - KOCHERGA_TRACE("App storage backend finalization failed (%d)\n", res); - verifyAppAndUpdateState(State::BootCancelled); - return res; - } - - /* - * Everything went well, checking if the application is valid and updating the state accordingly. - * This method will report success even if the application image it just downloaded is not valid, - * since that would be out of the scope of its responsibility. - */ - verifyAppAndUpdateState(State::BootDelay); - - return ErrOK; - } - - /** - * Returns the uptime provided by the platform driver. - * Just like any other public method, it is thread safe. - */ - std::chrono::microseconds getMonotonicUptime() const - { - MutexLocker mlock(platform_); - return platform_.getMonotonicUptime(); - } -}; - -/** - * This class allows the user to exchange arbitrary data between the bootloader and the application. - * The data is CRC-64 protected to ensure its validity. - * Two usage scenarios are supported: - * - * 1. Simple - the data is simply stored at a dedicated location in RAM. Normally this approach is recommended. - * Normally the reserved RAM area would be situated at the very end of the RAM. - * - * 2. Memory-efficient - the data is scattered across a set of special-function registers specified by the user. - * This approach permits the user to avoid reserving any memory regions for data exchange, which is - * useful in RAM-constrained systems. - * - * For more information, refer to the factory function @ref makeAppDataExchangeMarshaller(). - */ -template -class AppDataExchangeMarshaller -{ - static_assert(std::is_standard_layout_v, "Container must be a standard layout type."); - - Pointers pointers_; - - class ContainerWrapper - { - std::uint64_t crc_ = 0; - - public: - Container container; - - ContainerWrapper() : container() { } - - explicit ContainerWrapper(const Container& c) : - container(c) - { - CRC64 crc_computer; - crc_computer.add(&container, sizeof(container)); - crc_ = crc_computer.get(); - } - - bool isValid() const - { - CRC64 crc_computer; - crc_computer.add(&container, sizeof(container)); - return crc_ == crc_computer.get(); - } - }; - - template - struct ValueAsType - { - static constexpr decltype(N) Value = N; - }; - - template // Holy pants why auto doesn't work here - ValueAsType(sizeof(T), MaxSize)> readOne(void* destination, const volatile T* ptr) - { - const T x = *ptr; // Guaranteeing proper pointer access - std::memmove(destination, &x, std::min(sizeof(T), MaxSize)); - return {}; - } - - template - ValueAsType readOne(void* destination, const void* ptr) - { - std::memmove(destination, ptr, MaxSize); // Raw memory access - return {}; - } - - template - ValueAsType(sizeof(T), MaxSize)> writeOne(const void* source, volatile T* ptr) - { - T x = T(); - std::memmove(&x, source, std::min(sizeof(T), MaxSize)); - *ptr = x; // Guaranteeing proper pointer access - return {}; - } - - template - ValueAsType writeOne(const void* source, void* ptr) - { - std::memmove(ptr, source, MaxSize); // Raw memory access - return {}; - } - - template - std::enable_if_t<(RemainingSize > 0)> unwindReadWrite(void* structure) - { - static_assert(PtrIndex < std::tuple_size::value, "Storage is not large enough for the structure"); - const auto ret = WriteNotRead ? - writeOne(structure, std::get(pointers_)) : - readOne(structure, std::get(pointers_)); - - constexpr auto Increment = decltype(ret)::Value; - static_assert(RemainingSize >= Increment, "Rock is dead"); - - structure = static_cast(static_cast(structure) + Increment); - unwindReadWrite(structure); - } - - template - std::enable_if_t<(RemainingSize == 0)> unwindReadWrite(void*) - { - // Here we could implement a check whether all storage has been utilized. - } - -public: - /** - * Do not instantiate this class manually, it's difficult. - * Use the factory method instead, @ref makeAppDataExchangeMarshaller(). - */ - explicit AppDataExchangeMarshaller(const Pointers& ptrs) : pointers_(ptrs) { } - - /** - * Checks if the data is available and reads it; then erases the storage to prevent deja-vu. - * Returns an empty option if no data is available (in that case the storage is not erased). - */ - std::optional readAndErase() - { - ContainerWrapper wrapper; - unwindReadWrite(&wrapper); - if (wrapper.isValid()) - { - ContainerWrapper empty; - std::memset(&empty, 0, sizeof(empty)); - unwindReadWrite(&empty); - return wrapper.container; - } - else - { - return {}; - } - } - - /** - * Writes the data. This function cannot fail. - */ - void write(const Container& cont) - { - ContainerWrapper wrapper(cont); - unwindReadWrite(&wrapper); - } -}; - -/** - * Constructs an object that can be used to store and retrieve data for exchange with the application. - * Usage example: - * - * auto marshaller = makeAppDataExchangeMarshaller(®_A, ®_B, ®_C, ®_D, ®_E, ®_F); - * // Reading data (always destructively): - * if (auto data = marshaller.readAndErase()) - * { - * // Process the data... - * } - * else - * { - * // Data is not available (not stored) - * } - * // Writing data: - * marshaller.write(the_data); - * - * The application should use the following memory layout for writing and reading the shared struct: - * - * Offset Length Purpose - * 0 8 CRC64 of the following payload. Must be 8-byte aligned (or more if required by the platform). - * 8 >0 Payload written by the application or by the bootloader - * - * @tparam Container Payload data type, i.e., a structure that should be stored or read. - * - * @param pointers List of pointers to registers or memory where the structure will be stored or - * retrieved from. Pointer type defines access mode and size, e.g., a uint32 - * pointer will be accessed in 32-bit mode, and its memory block will be used to - * store exactly 4 bytes, etc. Supported pointer sizes are 8, 16, 32, and 64 bit. - * A single void pointer can be passed as well, in that case all of the data - * will be simply stored at that pointer using conventional std::memmove(). - * - * @return An instance of @ref AppDataExchangeMarshaller<>. - */ -template -inline auto makeAppDataExchangeMarshaller(RegisterPointers... pointers) -{ - return AppDataExchangeMarshaller>(std::make_tuple(pointers...)); -} - -} diff --git a/kocherga/kocherga.hpp b/kocherga/kocherga.hpp new file mode 100644 index 0000000..39db4b7 --- /dev/null +++ b/kocherga/kocherga.hpp @@ -0,0 +1,1267 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#define KOCHERGA_VERSION_MAJOR 0 // NOLINT +#define KOCHERGA_VERSION_MINOR 1 // NOLINT + +namespace kocherga +{ +/// Semantic version number pair: major then minor. +using SemanticVersion = std::array; + +using TransferID = std::uint64_t; +using NodeID = std::uint16_t; +using PortID = std::uint16_t; + +/// UAVCAN subjects used by Kocherga. +enum class SubjectID : PortID +{ + NodeHeartbeat = 7509, + PnPNodeIDAllocationData_v2 = 8165, + PnPNodeIDAllocationData_v1 = 8166, + DiagnosticRecord = 8184, +}; + +/// UAVCAN services used by Kocherga. +enum class ServiceID : PortID +{ + FileRead = 408, + NodeGetInfo = 430, + NodeExecuteCommand = 435, +}; + +/// Version of the UAVCAN specification implemented by this library, major and minor. +static constexpr SemanticVersion UAVCANSpecificationVersion{{1, 0}}; + +/// The service response timeout used by the bootloader. +/// This value applies when the bootloader invokes uavcan.file.Read during the update. +static constexpr std::chrono::microseconds ServiceResponseTimeout{5'000'000}; + +/// The largest transfer size used by the bootloader. Use this to size the buffers in transport implementations. +static constexpr std::size_t MaxSerializedRepresentationSize = 600; + +// -------------------------------------------------------------------------------------------------------------------- + +/// The structure is mapped to the ROM. +struct AppInfo +{ + static constexpr std::uint8_t Size = 48; + + template + using Skip = std::array; + + std::uint64_t image_crc; ///< CRC-64-WE padded to 8 bytes computed with this field =0. + std::uint32_t image_size; ///< Size of the application image in bytes. + [[maybe_unused]] Skip<4> _reserved_a; ///< Used to contain 32-bit vcs_revision_id. + SemanticVersion version; ///< Semantic version numbers, major then minor. + std::uint8_t flags; ///< Flags; see the constants. Unused flags shall not be set. + [[maybe_unused]] Skip<1> _reserved_b; ///< Write zero, ignore when reading. + std::uint32_t timestamp_utc; ///< UTC UNIX timestamp when the application was built. + std::uint64_t vcs_revision_id; ///< Version control system revision ID (e.g., git commit hash). + [[maybe_unused]] Skip<16> _reserved_c; ///< Write zero, ignore when reading. + + struct Flags + { + static constexpr std::uint8_t ReleaseBuild = 1U; + static constexpr std::uint8_t DirtyBuild = 2U; + }; + + [[nodiscard]] auto isReleaseBuild() const { return (flags & Flags::ReleaseBuild) != 0; } + [[nodiscard]] auto isDirtyBuild() const { return (flags & Flags::DirtyBuild) != 0; } +}; +static_assert(std::is_trivial_v, "Check your compiler."); +static_assert(AppInfo::Size == sizeof(AppInfo), "Check your compiler."); + +// -------------------------------------------------------------------------------------------------------------------- + +/// This information is provided by the bootloader host system during initialization. It does not change at runtime. +/// For documentation, please refer to uavcan.node.GetInfo.Response. +struct SystemInfo +{ + SemanticVersion hardware_version{}; + + using UniqueID = std::array; + UniqueID unique_id{}; + + const char* node_name = ""; + + /// CoA normally points into a specific region of ROM, but this is not required. Set 0/nullptr if not available. + std::uint8_t certificate_of_authenticity_len = 0; + const std::uint8_t* certificate_of_authenticity = nullptr; +}; + +// -------------------------------------------------------------------------------------------------------------------- + +/// A standard IoC delegate for handling protocol events in the node. +/// The user code will INVOKE this interface, not implement it. +/// A reference to the reactor is supplied to INode implementations to let them delegate transfer processing back +/// to the bootloader core. +/// The methods accept raw serialized representation and return one as well. +/// Serialization/deserialization is done by the bootloader core because DSDL is transport-agnostic. +/// All methods are non-blocking and return immediately. +/// Implementations utilizing just a single transport should not incur any polymorphism-induced overhead because decent +/// C++ compilers are quite good at devirtualization. +class IReactor +{ +public: + /// Returns the size of the response payload. Returns an empty option if no response should be sent. + [[nodiscard]] virtual auto processRequest(const PortID service_id, + const NodeID client_node_id, + const std::size_t request_length, + const std::uint8_t* const request, + std::uint8_t* const out_response) -> std::optional = 0; + + /// The service-ID is not communicated back because there may be at most one request pending at a time. + /// Hence, the bootloader core knows what response it is by checking which request was sent last. + virtual void processResponse(const std::size_t response_length, const std::uint8_t* const response) = 0; + + virtual ~IReactor() = default; + IReactor() = default; + IReactor(const IReactor&) = delete; + IReactor(IReactor&&) = delete; + auto operator=(const IReactor&) -> IReactor& = delete; + auto operator=(IReactor&&) -> IReactor& = delete; +}; + +// -------------------------------------------------------------------------------------------------------------------- + +/// The transport-specific node abstraction interface. Kocherga runs a separate node per transport interface. +/// If redundant transports are desired, they should be implemented in a custom implementation of INode. +/// If the node implementation is unable to perform the requested action (for example, because a node-ID allocation +/// is still in progress), it shall ignore the commanded action or return an error if such possibility is provided. +/// The priority of outgoing transfers should be the lowest, excepting the service responses -- those should use the +/// same priority level as the corresponding request transfer. +class INode +{ +public: + /// The bootloader invokes this method every tick to let the node run background activities such as + /// processing incoming transfers. If a reaction is required (such as responding to a service request), + /// it is delegated to the bootloader core via the IoC IReactor. + virtual void poll(IReactor& reactor, const std::chrono::microseconds uptime) = 0; + + /// Send a request. The response will be delivered later asynchronously via IReactor. + /// There may be AT MOST one pending request at a time. + /// The return value is True on success and False if the request could not be sent (aborts the update process). + [[nodiscard]] virtual auto sendRequest(const ServiceID service_id, + const NodeID server_node_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool = 0; + + /// Cancel the request that was previously set pending by sendRequest(); no longer expect the response. + /// This method is invoked when the request has timed out. + virtual void cancelRequest() = 0; + + /// Publish a message. + /// The return value is True on success and False if the message could not be sent (does not abort the update). + [[nodiscard]] virtual auto publishMessage(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool = 0; + + virtual ~INode() = default; + INode() = default; + INode(const INode&) = delete; + INode(INode&&) = delete; + auto operator=(const INode&) -> INode& = delete; + auto operator=(INode&&) -> INode& = delete; +}; + +// -------------------------------------------------------------------------------------------------------------------- + +/// This interface abstracts the target-specific ROM routines. +/// App update scenario: +/// 1. beginWrite() +/// 2. write() repeated until finished. +/// 3. endWrite() (the number of preceding writes may be zero) +/// +/// The read() method may be invoked at any time. Its performance is critical. +/// Slow access may lead to watchdog timeouts (assuming that the watchdog is used) and/or disruption of communications. +/// To avoid issues, ensure that the entirety of the image can be read x10 in less than the watchdog timeout interval. +/// +/// The zero offset shall point to the beginning of the ROM segment dedicated to the application. +class IROMBackend +{ +public: + /// This hook allows the ROM driver to enable write operations, erase ROM, etc, depending on the hardware. + /// This operation cannot fail. + virtual void beginWrite() + { + // No effect by default. + } + + /// This hook allows the ROM driver to disable write operations or to perform other hardware-specific steps. + /// This operation cannot fail. + virtual void endWrite() + { + // No effect by default. + } + + /// @return Number of bytes written; a value less than size indicates an overflow; empty option indicates failure. + [[nodiscard]] virtual auto write(const std::size_t offset, const std::byte* const data, const std::size_t size) + -> std::optional = 0; + + /// @return Number of bytes read; a value less than size indicates an overrun. This operation cannot fail. + [[nodiscard]] virtual auto read(const std::size_t offset, std::byte* const out_data, const std::size_t size) const + -> std::size_t = 0; + + virtual ~IROMBackend() = default; + IROMBackend() = default; + IROMBackend(const IROMBackend&) = delete; + IROMBackend(IROMBackend&&) = delete; + auto operator=(const IROMBackend&) -> IROMBackend& = delete; + auto operator=(IROMBackend&&) -> IROMBackend& = delete; +}; + +// -------------------------------------------------------------------------------------------------------------------- + +/// Internal use only. +namespace detail +{ +static constexpr auto BitsPerByte = 8U; + +static constexpr std::chrono::microseconds DefaultTransferIDTimeout{2'000'000}; ///< Default taken from Specification. + +/// This is used to verify integrity of the application and other data. +/// Note that the firmware CRC verification is a computationally expensive process that needs to be completed +/// in a limited time interval, which should be minimized. This class has been carefully manually optimized to +/// achieve the optimal balance between speed and ROM footprint. +/// The function is CRC-64/WE, see http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64. +class CRC64 +{ +public: + static constexpr std::size_t Size = 8U; + + void update(const std::uint8_t* const data, const std::size_t len) + { + const auto* bytes = data; + for (auto remaining = len; remaining > 0; remaining--) + { + crc_ ^= static_cast(*bytes) << InputShift; + ++bytes; + // Unrolled for performance reasons. This path directly affects the boot-up time, so it is very + // important to keep it optimized for speed. Rolling this into a loop causes a significant performance + // degradation at least with GCC since the compiler refuses to unroll the loop when size optimization + // is selected (which is normally used for bootloaders). + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + crc_ = ((crc_ & Mask) != 0) ? ((crc_ << 1U) ^ Poly) : (crc_ << 1U); + } + } + + /// The current CRC value. + [[nodiscard]] auto get() const { return crc_ ^ Xor; } + + /// The current CRC value represented as a big-endian sequence of bytes. + /// This method is designed for inserting the computed CRC value after the data. + [[nodiscard]] auto getBytes() const -> std::array + { + auto x = get(); + std::array out{}; + const auto rend = std::rend(out); + for (auto it = std::rbegin(out); it != rend; ++it) + { + *it = static_cast(x); + x >>= BitsPerByte; + } + return out; + } + + /// True if the current CRC value is a correct residue (i.e., CRC verification successful). + [[nodiscard]] auto isResidueCorrect() const { return crc_ == Residue; } + +private: + static constexpr auto Poly = static_cast(0x42F0'E1EB'A9EA'3693ULL); + static constexpr auto Mask = static_cast(1) << 63U; + static constexpr auto Xor = static_cast(0xFFFF'FFFF'FFFF'FFFFULL); + static constexpr auto Residue = static_cast(0xFCAC'BEBD'5931'A992ULL); + + static constexpr auto InputShift = 56U; + + std::uint64_t crc_ = Xor; +}; + +/// Detects the application in the ROM, verifies its integrity, and retrieves the information about it. +class AppLocator final +{ +public: + AppLocator(const IROMBackend& backend, const std::size_t max_app_size) : + max_app_size_(max_app_size), backend_(backend) + {} + + /// Returns the AppInfo if the app is found and its integrity is intact. Otherwise, returns an empty option. + [[nodiscard]] auto identifyApplication() const -> std::optional + { + for (std::size_t offset = 0; offset < max_app_size_; offset += AppDescriptor::MagicSize) + { + AppDescriptor desc{}; + if (sizeof(desc) == backend_.read(offset, reinterpret_cast(&desc), sizeof(desc))) + { + if (desc.isValid(max_app_size_) && + validateImageCRC(offset + AppDescriptor::CRCOffset, + static_cast(desc.getAppInfo().image_size), + desc.getAppInfo().image_crc)) + { + return desc.getAppInfo(); + } + } + else + { + break; + } + } + return {}; + } + +private: + class AppDescriptor final + { + public: + static constexpr std::size_t MagicSize = 8U; + static constexpr std::size_t SignatureSize = 8U; + static constexpr std::size_t CRCOffset = MagicSize + SignatureSize; + + [[nodiscard]] auto isValid(const std::size_t max_app_size) const -> bool + { + return (magic == ReferenceMagic) && + std::equal(signature.begin(), signature.end(), ReferenceSignature.begin()) && + (app_info.image_size > 0) && (app_info.image_size <= max_app_size) && + ((app_info.image_size % MagicSize) == 0); + } + + [[nodiscard]] auto getAppInfo() const -> const AppInfo& { return app_info; } + + private: + /// The magic is also used for byte order detection. + /// The value of the magic was obtained from a random number generator, it does not mean anything. + static constexpr std::uint64_t ReferenceMagic = 0x5E44'1514'6FC0'C4C7ULL; + static constexpr std::array ReferenceSignature{ + {'A', 'P', 'D', 'e', 's', 'c', '0', '0'}}; + + std::uint64_t magic; + std::array signature; + AppInfo app_info; + }; + static_assert(std::is_trivial_v, "Check your compiler"); + static_assert((AppInfo::Size + AppDescriptor::MagicSize + AppDescriptor::SignatureSize) == sizeof(AppDescriptor), + "Check your compiler"); + static_assert(64 == sizeof(AppDescriptor), "Check your compiler"); + + [[nodiscard]] auto validateImageCRC(const std::size_t crc_storage_offset, + const std::size_t image_size, + const std::uint64_t image_crc) const -> bool + { + std::array buffer{}; + CRC64 crc; + std::size_t offset = 0U; + // Read large chunks until the CRC field is reached (in most cases it will fit in just one chunk). + while (offset < crc_storage_offset) + { + const auto res = backend_.read(offset, + reinterpret_cast(buffer.data()), + std::min(std::size(buffer), crc_storage_offset - offset)); + if (res > 0) + { + offset += res; + crc.update(buffer.data(), res); + } + else + { + return false; + } + } + // Fill CRC with zero. + static const std::array dummy{}; + offset += CRC64::Size; + crc.update(dummy.data(), CRC64::Size); + // Read the rest of the image in large chunks. + while (offset < image_size) + { + const auto res = backend_.read(offset, + reinterpret_cast(buffer.data()), + std::min(std::size(buffer), image_size - offset)); + if (res > 0) + { + offset += res; + crc.update(buffer.data(), res); + } + else + { + return false; + } + } + return crc.get() == image_crc; + } + + static constexpr std::size_t ROMBufferSize = 256; + + const std::size_t max_app_size_; + const IROMBackend& backend_; +}; + +/// These DSDL-derived definitions substitute for the lack of code generation. +namespace dsdl +{ +static constexpr std::size_t NameCapacity = 255; + +struct Heartbeat +{ + static constexpr std::size_t Size = 7; + static constexpr std::chrono::microseconds Period{1'000'000}; + + static constexpr std::uint8_t ModeSoftwareUpdate = 3; + + enum class Health : std::uint8_t + { + Nominal = 0, + Advisory = 1, + Warning = 3, + }; +}; + +struct ExecuteCommand +{ + static constexpr std::size_t ResponseSize = 7; + static constexpr std::size_t RequestSizeMin = 3; + + enum class Command : std::uint16_t + { + Restart = 65'535, + BeginSoftwareUpdate = 65'533, + EmergencyStop = 65'531, + }; + + enum class Status : std::uint8_t + { + Success = 0, + BadCommand = 3, + InternalError = 6, + }; +}; + +struct Diagnostic +{ + enum class Severity : std::uint8_t + { + Notice = 3, + Warning = 4, + Critical = 6, + }; + + static constexpr std::size_t RecordSize = 257; +}; + +struct File +{ + static constexpr std::size_t PathCapacity = 255; + static constexpr std::size_t ReadRequestCapacity = PathCapacity + 6; + static constexpr std::size_t ReadResponseSizeMin = 4; + static constexpr std::size_t ReadResponseDataCapacity = 256; + + /// If the server returned an error, data will be nullptr and the data length will be zero. + struct ReadResponse + { + std::uint16_t data_length = 0; + const std::byte* data = nullptr; + + [[nodiscard]] auto isSuccessful() const { return data != nullptr; } + }; +}; + +struct PnPNodeIDAllocation +{ + static constexpr std::chrono::microseconds MaxRequestInterval{2'000'000}; + + static constexpr std::size_t MessageSize_v2 = 18; +}; + +} // namespace dsdl + +/// A higher-level variant of IReactor that operates on application-level representations instead of raw transfers. +class IController +{ +public: + /// Remote node has commanded us to reboot (back into the bootloader). The command shall always be accepted. + virtual void reboot() = 0; + + /// Begin software update. + /// The remote node and path are stored in the presenter so that the controller does not need to manage that. + virtual void beginUpdate() = 0; + + /// Response from the file server received or timed out. In case of timeout, the argument is an empty option. + virtual void handleFileReadResult(const std::optional response) = 0; + + [[nodiscard]] virtual auto getAppInfo() const -> std::optional = 0; + + virtual ~IController() = default; + IController() = default; + IController(const IController&) = delete; + IController(const IController&&) = delete; + auto operator=(const IController&) -> IController& = delete; + auto operator=(const IController&&) -> IController& = delete; +}; + +/// Unifies multiple INode and performs DSDL serialization. Manages the network at the presentation layer. +class Presenter final : public IReactor +{ +public: + Presenter(const SystemInfo& system_info, IController& controller) : + system_info_(system_info), controller_(controller) + {} + + [[nodiscard]] auto addNode(INode* const node) -> bool + { + for (const auto* n : nodes_) + { + if (n == node) + { + return false; // This node is already registered. + } + } + if (num_nodes_ < nodes_.size()) + { + nodes_.at(num_nodes_++) = node; + return true; + } + return false; + } + + [[nodiscard]] auto trigger(const INode* const node, + const NodeID file_server_node_id, + const std::size_t app_image_file_path_length, + const std::uint8_t* const app_image_file_path) -> bool + { + for (std::uint8_t i = 0U; i < num_nodes_; i++) + { + if (nodes_.at(i) == node) + { + beginUpdate(i, file_server_node_id, app_image_file_path_length, app_image_file_path); + return true; + } + } + return false; + } + + [[nodiscard]] auto trigger(const std::uint8_t node_index, + const NodeID file_server_node_id, + const std::size_t app_image_file_path_length, + const std::uint8_t* const app_image_file_path) -> bool + { + if (node_index < num_nodes_) + { + beginUpdate(node_index, file_server_node_id, app_image_file_path_length, app_image_file_path); + return true; + } + return false; + } + + void poll(const std::chrono::microseconds uptime) + { + last_poll_at_ = uptime; + + current_node_index_ = 0; + for (INode* const node : nodes_) + { + if (node != nullptr) + { + node->poll(*this, uptime); + } + ++current_node_index_; + } + + if (file_loc_spec_) + { + FileLocationSpecifier& fls = *file_loc_spec_; + if (fls.response_deadline && (uptime > *fls.response_deadline)) + { + INode* const nd = nodes_.at(fls.local_node_index); + nd->cancelRequest(); + fls.response_deadline.reset(); + controller_.handleFileReadResult({}); + } + } + + if (uptime >= next_heartbeat_deadline_) // Postpone heartbeat to reflect the latest state changes. + { + next_heartbeat_deadline_ += dsdl::Heartbeat::Period; + publishHeartbeat(uptime); + } + } + + void setNodeHealth(const dsdl::Heartbeat::Health value) { node_health_ = value; } + void setNodeVSSC(const std::uint8_t value) { node_vssc_ = value; } + + /// The timeout will be managed by the presenter automatically. + [[nodiscard]] auto requestFileRead(const std::uint64_t offset) -> bool + { + if (file_loc_spec_) + { + std::array buf{}; + + auto of = offset; + buf[0] = static_cast(of); + of >>= BitsPerByte; + buf[1] = static_cast(of); + of >>= BitsPerByte; + buf[2] = static_cast(of); + of >>= BitsPerByte; + buf[3] = static_cast(of); + of >>= BitsPerByte; + buf[4] = static_cast(of); + + static constexpr auto length_minus_path = 6U; + FileLocationSpecifier& fls = *file_loc_spec_; + buf.at(length_minus_path - 1U) = static_cast(fls.path_length); + (void) std::memmove(&buf.at(length_minus_path), fls.path.data(), fls.path_length); + INode* const node = nodes_.at(fls.local_node_index); + + read_transfer_id_++; + const bool out = node->sendRequest(ServiceID::FileRead, + fls.server_node_id, + read_transfer_id_, + fls.path_length + length_minus_path, + buf.data()); + if (out) + { + fls.response_deadline = last_poll_at_ + ServiceResponseTimeout; + } + return out; + } + return false; + } + + void publishLogRecord(const dsdl::Diagnostic::Severity severity, const char* const text) + { + std::array buf{}; + buf[7] = static_cast(severity); + std::uint8_t text_length = 0; + const char* ch = text; + auto* const buf_end = std::end(buf); + for (auto it = std::begin(buf) + 9; it != buf_end; ++it) // NOLINT + { + if ('\0' == *ch) + { + break; + } + *it = static_cast(*ch++); + ++text_length; + } + buf[8] = text_length; + for (INode* const node : nodes_) + { + if (node != nullptr) + { + // Ignore transient errors. + (void) node->publishMessage(SubjectID::DiagnosticRecord, tid_log_record_, text_length + 9U, buf.data()); + } + } + ++tid_log_record_; + } + +private: + [[nodiscard]] auto processRequest(const PortID service_id, + const NodeID client_node_id, + const std::size_t request_length, + const std::uint8_t* const request, + std::uint8_t* const out_response) -> std::optional override + { + std::optional out; + switch (service_id) + { + case static_cast(ServiceID::NodeExecuteCommand): + { + out = processExecuteCommandRequest(client_node_id, request_length, request, out_response); + break; + } + case static_cast(ServiceID::NodeGetInfo): + { + out = processNodeInfoRequest(out_response); + break; + } + case static_cast(ServiceID::FileRead): + default: + { + break; // Unknown services ignored. + } + } + return out; + } + + auto processExecuteCommandRequest(const NodeID client_node_id, + const std::size_t request_length, + const std::uint8_t* const request, + std::uint8_t* const out_response) -> std::size_t + { + auto result = dsdl::ExecuteCommand::Status::InternalError; + if (request_length >= dsdl::ExecuteCommand::RequestSizeMin) + { + const auto* ptr = request; + auto command = static_cast(*ptr); + ++ptr; + command = static_cast( + command | static_cast(static_cast(*ptr) << BitsPerByte)); + ++ptr; + if ((command == static_cast(dsdl::ExecuteCommand::Command::EmergencyStop)) || + (command == static_cast(dsdl::ExecuteCommand::Command::Restart))) + { + controller_.reboot(); + result = dsdl::ExecuteCommand::Status::Success; + } + else if (command == static_cast(dsdl::ExecuteCommand::Command::BeginSoftwareUpdate)) + { + const std::size_t path_length = *ptr++; + beginUpdate(current_node_index_, client_node_id, path_length, ptr); + result = dsdl::ExecuteCommand::Status::Success; + } + else + { + result = dsdl::ExecuteCommand::Status::BadCommand; + } + } + (void) std::memset(out_response, 0, dsdl::ExecuteCommand::ResponseSize); + *out_response = static_cast(result); + return dsdl::ExecuteCommand::ResponseSize; + } + + [[nodiscard]] auto processNodeInfoRequest(std::uint8_t* const out_response) const -> std::size_t + { + const auto app_info = controller_.getAppInfo(); + auto* const base_ptr = out_response; + auto* ptr = base_ptr; + *ptr++ = UAVCANSpecificationVersion.at(0); + *ptr++ = UAVCANSpecificationVersion.at(1); + *ptr++ = system_info_.hardware_version.at(0); + *ptr++ = system_info_.hardware_version.at(1); + if (app_info) + { + *ptr++ = app_info->version.at(0); + *ptr++ = app_info->version.at(1); + std::uint64_t vcs_revision_id = app_info->vcs_revision_id; + for (auto i = 0U; i < sizeof(std::uint64_t); i++) + { + *ptr++ = static_cast(vcs_revision_id); + vcs_revision_id >>= BitsPerByte; + } + } + else + { + *ptr++ = 0; + *ptr++ = 0; + for (auto i = 0U; i < sizeof(std::uint64_t); i++) + { + *ptr++ = 0; + } + } + for (auto uid : system_info_.unique_id) + { + *ptr++ = uid; + } + auto& name_length = *ptr++; + const char* ch = system_info_.node_name; + for (auto i = 0U; i < dsdl::NameCapacity; i++) + { + if ('\0' == *ch) + { + break; + } + name_length++; + *ptr++ = static_cast(*ch++); + } + if (app_info) + { + *ptr++ = 1; + std::uint64_t crc = app_info->image_crc; + for (auto i = 0U; i < sizeof(std::uint64_t); i++) + { + *ptr++ = static_cast(crc); + crc >>= BitsPerByte; + } + } + else + { + *ptr++ = 0; + } + *ptr++ = system_info_.certificate_of_authenticity_len; + for (auto i = 0U; i < system_info_.certificate_of_authenticity_len; i++) + { + *ptr++ = static_cast(system_info_.certificate_of_authenticity[i]); + } + return static_cast(ptr - base_ptr); + } + + void processResponse(const std::size_t response_length, const std::uint8_t* const response) override + { + if (file_loc_spec_ && (response_length >= dsdl::File::ReadResponseSizeMin)) + { + FileLocationSpecifier& fls = *file_loc_spec_; + if (fls.response_deadline && (fls.local_node_index == current_node_index_)) + { + fls.response_deadline.reset(); + static const std::array zero_error{}; + std::optional argument; + if (std::equal(std::begin(zero_error), std::end(zero_error), response)) // Error = OK + { + argument = dsdl::File::ReadResponse{ + static_cast( + static_cast(static_cast(response[2])) | + static_cast(static_cast(response[3]) << BitsPerByte)), + reinterpret_cast(&response[4]), + }; + } + if (argument && (argument->data_length > dsdl::File::ReadResponseDataCapacity)) + { + argument.reset(); + } + controller_.handleFileReadResult(argument); + } + } + } + + void publishHeartbeat(const std::chrono::microseconds uptime) + { + const auto ut = static_cast(std::chrono::duration_cast(uptime).count()); + std::array buf{{ + static_cast(ut >> (BitsPerByte * 0U)), + static_cast(ut >> (BitsPerByte * 1U)), + static_cast(ut >> (BitsPerByte * 2U)), + static_cast(ut >> (BitsPerByte * 3U)), + static_cast(node_health_), + static_cast(dsdl::Heartbeat::ModeSoftwareUpdate), + static_cast(node_vssc_), + }}; + for (INode* const node : nodes_) + { + if (node != nullptr) + { + // Ignore transient errors. + (void) node->publishMessage(SubjectID::NodeHeartbeat, tid_heartbeat_, buf.size(), buf.data()); + } + } + ++tid_heartbeat_; + } + + struct FileLocationSpecifier + { + std::uint8_t local_node_index{}; + NodeID server_node_id{}; + std::size_t path_length{}; + std::array path{}; + std::optional response_deadline{}; + }; + + void beginUpdate(const std::uint8_t local_node_index, + const NodeID file_server_node_id, + const std::size_t app_image_file_path_length, + const std::uint8_t* const app_image_file_path) + { + FileLocationSpecifier fls{}; + fls.local_node_index = local_node_index; + fls.server_node_id = file_server_node_id; + fls.path_length = std::min(app_image_file_path_length, std::size(fls.path)); + (void) std::memmove(fls.path.data(), app_image_file_path, fls.path_length); + + if (file_loc_spec_ && file_loc_spec_->response_deadline) + { + nodes_.at(file_loc_spec_->local_node_index)->cancelRequest(); + } + file_loc_spec_ = fls; + current_node_index_ = fls.local_node_index; + controller_.beginUpdate(); + } + + static constexpr std::uint8_t MaxNodes = 8; + + const SystemInfo system_info_; + std::array nodes_{}; + std::uint8_t num_nodes_ = 0; + IController& controller_; + + std::chrono::microseconds last_poll_at_{}; + + std::uint8_t current_node_index_ = 0; + + std::optional file_loc_spec_; + TransferID read_transfer_id_ = 0; + + TransferID tid_heartbeat_ = 0; + TransferID tid_log_record_ = 0; + + std::chrono::microseconds next_heartbeat_deadline_{dsdl::Heartbeat::Period}; + dsdl::Heartbeat::Health node_health_ = dsdl::Heartbeat::Health::Nominal; + std::uint8_t node_vssc_ = 0; +}; + +} // namespace detail + +// -------------------------------------------------------------------------------------------------------------------- + +/// The following state transition diagram illustrates the operating principles of the bootloader. +/// +/// ###################### +/// .----------------# Bootloader started #-------. Valid +/// | No valid ###################### | application found. +/// v application found. v +/// +-------------+ +-----------+ Boot delay expired. ################ +/// .-->| NoAppToBoot | .------------------| BootDelay |--------------------># Boot the app # +/// | +-------------+ / +-----------+ (default delay 0) ################ +/// | | / Boot | ^ +/// |Update |<-----------' canceled.| | +/// |failed, |Update requested. | | +/// |no valid | v | +/// |image is now | +--------------+ | +/// |available. |<-----------------------| BootCanceled | | +/// | | +--------------+ | +/// | v ^ | +/// | +----------------------+ Update failed, but the | |Update successful, +/// '-| AppUpdateInProgress |-------------------------/ |the received image +/// +----------------------+ existing valid image was not |is valid. +/// | altered and remains valid. | +/// | | +/// '------------------------------------------------' +/// +/// The current state is communicated to the outside world via uavcan.node.Heartbeat. The mapping is as follows: +/// +/// State Node mode Node health Vendor-specific status code +/// ----------------------------------------------------------------------------------------------- +/// NoAppToBoot SOFTWARE_UPDATE WARNING =0 +/// BootDelay SOFTWARE_UPDATE NOMINAL =0 +/// BootCanceled SOFTWARE_UPDATE ADVISORY =0 +/// AppUpdateInProgress SOFTWARE_UPDATE NOMINAL >0 (see below) +/// +/// In the mode AppUpdateInProgress, the vendor-specific status code equals the number of uavcan.file.Read requests +/// sent since the update process was initiated; it is always greater than zero. This is used for progress reporting. +enum class State +{ + NoAppToBoot, + BootDelay, + BootCanceled, + AppUpdateInProgress, +}; + +/// The bootloader instructs the outer logic what action needs to be taken when its execution has completed. +/// Once the final action is returned, the bootloader has terminated itself and need not be run anymore. +enum class Final +{ + BootApp, ///< Jump to the application image. The bootloader has verified its correctness. + Restart, ///< Restart the bootloader itself. +}; + +/// The bootloader core. +/// +/// The bootloader may run multiple nodes on different transports concurrently to support multi-transport functionality. +/// For example, a device may provide the firmware update capability via CAN and a serial port. +/// The nodes are registered using addNode() after the instance is constructed. +/// +/// If the boot delay is zero and the valid application is found, the bootloader proceeds to start it immediately. +class Bootloader : public detail::IController +{ +public: + /// The max application image size parameter is very important for performance reasons. + /// Without it, the bootloader may encounter an unrelated data structure in the ROM that looks like a + /// valid app descriptor (by virtue of having the same magic, which is only 64 bit long), + /// and it may spend a considerable amount of time trying to check the CRC that is certainly invalid. + /// Having an upper size limit for the application image allows the bootloader to weed out too large + /// values early, greatly improving the worst case boot time. + /// + /// SystemInfo is used for responding to uavcan.node.GetInfo requests. + /// + /// If the linger flag is set, the bootloader will not boot the application after the initial verification. + /// If the application is valid, then the initial state will be BootCanceled instead of BootDelay. + /// If the application is invalid, the flag will have no effect. + /// It is designed to support the common use case where the application commands the bootloader to start and + /// sit idle until instructed otherwise, or if the application itself commands the bootloader to begin the update. + /// The flag affects only the initial verification and has no effect on all subsequent checks; for example, + /// after the application is updated and validated, it will be booted after BootDelay regardless of this flag. + Bootloader(IROMBackend& rom_backend, + const SystemInfo& system_info, + const std::size_t max_app_size, + const bool linger, + const std::chrono::seconds boot_delay = std::chrono::seconds(0)) : + max_app_size_(max_app_size), + boot_delay_(boot_delay), + backend_(rom_backend), + presentation_(system_info, *this), + linger_(linger) + {} + + /// Nodes shall be registered using this method after the instance is constructed. + /// The return value is true on success, false if there are too many nodes already or this node is already + /// registered (no effect in this case). + [[nodiscard]] auto addNode(INode* const node) -> bool { return presentation_.addNode(node); } + + /// Non-blocking periodic state update. + /// The outer logic should invoke this method after any hardware event (for example, if WFE/WFI is used on an + /// ARM platform), and periodically at least once per second. Typically, it would be invoked from the main loop. + /// The watchdog, if used, should be reset before or after each invocation. + [[nodiscard]] auto poll(const std::chrono::microseconds time_since_boot) -> std::optional + { + uptime_ = time_since_boot; + if (!inited_) + { + inited_ = true; + reset(!linger_); + } + + if ((State::AppUpdateInProgress == state_) && request_read_) + { + request_read_ = false; + ++read_request_count_; + presentation_.setNodeVSSC(static_cast(read_request_count_)); // Indicate progress. + if (!presentation_.requestFileRead(rom_offset_)) + { + presentation_.publishLogRecord(detail::dsdl::Diagnostic::Severity::Critical, + "Could not send request uavcan.file.Read, abort"); + reset(false); + } + } + + if ((State::BootDelay == state_) && (uptime_ >= boot_deadline_)) // Fast boot path. + { + final_ = Final::BootApp; + } + + if (!final_) + { + // If the application is valid and the boot delay is zero, the nodes will never be polled by design. + // This avoids unnecessary delays at startup. In many embedded systems fast boot-up is critical. + presentation_.poll(uptime_); + } + + if (pending_log_) + { + // Send logs as late as possible to take into account the new entries from this poll(). + presentation_.publishLogRecord(pending_log_->first, pending_log_->second); + pending_log_.reset(); + } + + return final_; + } + + /// Manual trigger: commence the application update process without waiting for an external node to trigger it. + /// This is normally used when the application commands the bootloader to begin the update directly. + /// Returns false if the node reference is invalid; otherwise true. + /// The path is truncated if too long. + auto trigger(const INode* const node, + const NodeID file_server_node_id, + const std::size_t app_image_file_path_length, + const std::uint8_t* const app_image_file_path) -> bool + { + return presentation_.trigger(node, file_server_node_id, app_image_file_path_length, app_image_file_path); + } + auto trigger(const std::uint8_t node_index, + const NodeID file_server_node_id, + const std::size_t app_image_file_path_length, + const std::uint8_t* const app_image_file_path) -> bool + { + return presentation_.trigger(node_index, file_server_node_id, app_image_file_path_length, app_image_file_path); + } + + /// Returns the information about the installed application, if there is one. Otherwise, returns an empty option. + /// The return value may not match the actual state of the application before the first poll() is executed. + [[nodiscard]] auto getAppInfo() const -> std::optional override { return app_info_; } + + /// This method is mostly intended for state indication purposes, like driving the status LEDs. + /// The return value may not match the actual state of the application before the first poll() is executed. + [[nodiscard]] auto getState() const { return state_; } + +private: + /// Execution may take several seconds due to the ROM scanning and CRC verification. + /// The argument is true if the application should be automatically launched if present. + void reset(const bool auto_boot) + { + if (State::AppUpdateInProgress == state_) + { + backend_.endWrite(); + } + app_info_ = detail::AppLocator(backend_, max_app_size_).identifyApplication(); + final_.reset(); + if (app_info_) + { + if (auto_boot) + { + state_ = State::BootDelay; + boot_deadline_ = uptime_ + boot_delay_; + presentation_.setNodeHealth(detail::dsdl::Heartbeat::Health::Nominal); + } + else + { + state_ = State::BootCanceled; + presentation_.setNodeHealth(detail::dsdl::Heartbeat::Health::Advisory); + } + } + else + { + state_ = State::NoAppToBoot; + presentation_.setNodeHealth(detail::dsdl::Heartbeat::Health::Warning); + } + presentation_.setNodeVSSC(0); + } + + void reboot() override { final_ = Final::Restart; } + + void beginUpdate() override + { + // If an update was already in progress, it is simply interrupted and we begin from scratch. + if (State::AppUpdateInProgress == state_) + { + backend_.endWrite(); // Cycle the state to re-init ROM if needed. + pending_log_ = {detail::dsdl::Diagnostic::Severity::Warning, "Ongoing update process restarted"}; + } + else + { + pending_log_ = {detail::dsdl::Diagnostic::Severity::Notice, "Update started"}; + } + state_ = State::AppUpdateInProgress; + rom_offset_ = 0; + read_request_count_ = 0; + request_read_ = true; + backend_.beginWrite(); + presentation_.setNodeHealth(detail::dsdl::Heartbeat::Health::Nominal); + } + + void handleFileReadResult(const std::optional response) override + { + if (!response) + { + pending_log_ = {detail::dsdl::Diagnostic::Severity::Critical, "File request timeout or remote error"}; + reset(false); + } + else + { + const auto result = backend_.write(rom_offset_, response->data, response->data_length); + const bool ok = result && (result == response->data_length); + if (ok && (response->data_length >= detail::dsdl::File::ReadResponseDataCapacity)) + { + rom_offset_ += response->data_length; + request_read_ = true; + } + else + { + if (ok) + { + pending_log_ = {detail::dsdl::Diagnostic::Severity::Notice, "File transfer completed"}; + } + else + { + pending_log_ = {detail::dsdl::Diagnostic::Severity::Critical, "ROM write failure"}; + } + reset(true); + } + } + } + + const std::size_t max_app_size_; + const std::chrono::seconds boot_delay_; + + IROMBackend& backend_; + detail::Presenter presentation_; + const bool linger_; + + std::chrono::microseconds uptime_{}; + bool inited_ = false; + State state_ = State::NoAppToBoot; + std::optional final_; + std::chrono::microseconds boot_deadline_{}; + std::size_t rom_offset_ = 0; + std::uint32_t read_request_count_ = 0; + bool request_read_ = false; + std::optional app_info_; + + std::optional> pending_log_; +}; + +// -------------------------------------------------------------------------------------------------------------------- + +/// This helper class allows the bootloader and the application to exchange arbitrary data in a robust way. +/// The data is stored in the specified memory location (usually it is a small dedicated area a few hundred bytes +/// large at the very end of the slowest RAM segment) together with a strong CRC64 hash to ensure its validity. +/// When one component (either the bootloader or the application) needs to pass data to another (e.g., when commencing +/// the update process, the application needs to reboot into the bootloader and pass specific parameters to it), +/// the data is prepared in a particular application-specific data structure which is then passed into this class. +/// The class writes the data structure into the provided memory region and appends the CRC64 hash immediately +/// afterwards (no padding inserted). The other component then checks the memory region where the data is expected to +/// be found and validates its CRC; if the CRC matches, the data is reported to be found, otherwise it is reported +/// that there is no data to read (the latter occurs when the bootloader is started after power-on reset, +/// a power loss, or a hard reset). +/// +/// The stored data type shall be a trivial type (see https://en.cppreference.com/w/cpp/named_req/TrivialType). +/// The storage space shall be large enough to accommodate an instance of the stored data type plus eight bytes +/// for the CRC (no padding inserted). +/// +/// Here is a usage example. Initialization: +/// +/// struct MyData; +/// VolatileStorage storage(my_memory_location); +/// +/// Reading the data from the storage (the storage is always erased when reading to prevent deja-vu after restart): +/// +/// if (auto data = storage.take()) +/// { +/// // Process the data... +/// } +/// else +/// { +/// // Data is not available (not stored) +/// } +/// +/// Writing the data into the storage: storage.store(data). +template +class VolatileStorage +{ +public: + /// The amount of memory required to store the data. This is the size of the container plus 8 bytes for the CRC. + static constexpr auto StorageSize = sizeof(Container) + detail::CRC64::Size; + + explicit VolatileStorage(std::uint8_t* const location) : ptr_(location) {} + + /// Checks if the data is available and reads it, then erases the storage to prevent deja-vu. + /// Returns an empty option if no data is available (in that case the storage is not erased). + [[nodiscard]] auto take() -> std::optional + { + detail::CRC64 crc; + crc.update(ptr_, StorageSize); + if (crc.isResidueCorrect()) + { + Container out{}; + (void) std::memmove(&out, ptr_, sizeof(Container)); + (void) std::memset(ptr_, EraseFillValue, StorageSize); + return out; + } + return {}; + } + + /// Writes the data into the storage with CRC64 protection. + void store(const Container& data) + { + (void) std::memmove(ptr_, &data, sizeof(Container)); + detail::CRC64 crc; + crc.update(ptr_, sizeof(Container)); + const auto crc_ptr = ptr_ + sizeof(Container); // NOLINT NOSONAR pointer arithmetic + (void) std::memmove(crc_ptr, crc.getBytes().data(), detail::CRC64::Size); + } + +protected: + static_assert(std::is_trivial_v, "Container shall be a trivial type."); + + static constexpr std::uint8_t EraseFillValue = 0xCA; + + std::uint8_t* const ptr_; +}; + +} // namespace kocherga diff --git a/kocherga/kocherga_can.hpp b/kocherga/kocherga_can.hpp new file mode 100644 index 0000000..a9e3682 --- /dev/null +++ b/kocherga/kocherga_can.hpp @@ -0,0 +1,2201 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2021 Zubax Robotics. +// Author: Pavel Kirienko + +#pragma once + +#include "kocherga.hpp" +#include +#include +#include +#include + +namespace kocherga::can +{ +/// Frames that are not extended data frames shall always be rejected. +struct CANAcceptanceFilterConfig +{ + std::uint32_t extended_can_id = AllSet; + std::uint32_t mask = AllSet; + + static constexpr std::uint32_t AllSet = (1ULL << 29U) - 1U; + + /// Constructs a filter that accepts all extended data frames. + [[nodiscard]] static auto makePromiscuous() -> CANAcceptanceFilterConfig { return {0, 0}; } + + [[nodoscard]] auto operator==(const CANAcceptanceFilterConfig& cfg) const -> bool + { + return (extended_can_id == cfg.extended_can_id) && (mask == cfg.mask); + } +}; + +/// Bridges Kocherga/CAN with the platform-specific CAN driver implementation. +/// Implement this and pass a reference to CANNode. +class ICANDriver +{ +public: + using PayloadBuffer = std::array; + + struct Bitrate + { + std::uint32_t arbitration{}; + std::uint32_t data{}; ///< This is ignored if only Classic CAN is supported. + + [[nodoscard]] auto operator==(const Bitrate& rhs) const -> bool + { + return (data == rhs.data) && (arbitration == rhs.arbitration); + } + }; + + /// Bitrates predefined by the UCANPHY specification. + /// Only these values are passed to the configure() method if bitrate auto-detection is used. + static constexpr std::array StandardBitrates{{ + {1'000'000, 4'000'000}, + {500'000, 2'000'000}, + {250'000, 1'000'000}, + {125'000, 500'000}, + }}; + + /// Version of the CAN standard supported by the underlying hardware. + enum class Mode : std::uint8_t + { + Classic, + FD + }; + + /// Set up the specified configuration of the CAN hardware. + /// If the hardware does not support CAN FD, then the data bitrate setting should be ignored. + /// The "silent" flag selects the silent mode, also known as listen-only mode. + /// Bitrate autodetection is performed by the bootloader by trying different configurations sequentially. + /// The return value indicates whether the controller supports CAN FD or only Classic CAN. + /// The bootloader is always able to accept CAN FD frames regardless of the returned value. + /// The return value is an empty option if the controller is unable to accept the specified configuration. + [[nodiscard]] virtual auto configure(const Bitrate& bitrate, + const bool silent, + const CANAcceptanceFilterConfig& filter) -> std::optional = 0; + + /// Non-blocking addition to the transmission queue of a single CAN frame. + /// The transmission queue shall be at least 100 Classic CAN frames deep, or at least 10 CAN FD frames deep. + /// Returns true on success, false if: 1. no space available; 2. a transient error occurred; 3. payload_size > MTU. + /// The payload pointer is invalidated immediately upon return. + /// If a frame could not be delivered to the bus in about 1 second since being enqueued, it shall be aborted. + [[nodiscard]] virtual auto push(const bool force_classic_can, + const std::uint32_t extended_can_id, + const std::uint8_t payload_size, + const void* const payload) -> bool = 0; + + /// Non-blocking read of a pending CAN frame from the RX queue. + /// The frame can be either classic or FD, the bootloader doesn't care. + /// Returns (CAN ID, payload size) if a frame has been read successfully, empty option otherwise. + [[nodiscard]] virtual auto pop(PayloadBuffer& payload_buffer) + -> std::optional> = 0; + + virtual ~ICANDriver() = default; + ICANDriver() = default; + ICANDriver(const ICANDriver&) = delete; + ICANDriver(ICANDriver&&) = delete; + auto operator=(const ICANDriver&) -> ICANDriver& = delete; + auto operator=(ICANDriver&&) -> ICANDriver& = delete; + + /// The length is rounded UP to the next valid DLC. + constexpr static std::array LengthToDLC{{ + 0, 1, 2, 3, 4, 5, 6, 7, 8, // 0-8 + 9, 9, 9, 9, // 9-12 + 10, 10, 10, 10, // 13-16 + 11, 11, 11, 11, // 17-20 + 12, 12, 12, 12, // 21-24 + 13, 13, 13, 13, 13, 13, 13, 13, // 25-32 + 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, // 33-48 + 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, // 49-64 + }}; + constexpr static std::array DLCToLength{{0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}}; +}; + +static constexpr std::uint8_t MaxNodeID = 127U; + +namespace detail +{ +using kocherga::detail::BitsPerByte; + +static constexpr std::uint8_t TailByteStartOfTransfer = 0b1000'0000; +static constexpr std::uint8_t TailByteEndOfTransfer = 0b0100'0000; +static constexpr std::uint8_t TailByteToggleBit = 0b0010'0000; + +static constexpr std::uint8_t MaxTransferID = 31; + +class CRC16CCITT +{ +public: + static constexpr std::size_t Size = 2; + + void update(const std::uint8_t b) noexcept + { + value_ ^= static_cast(static_cast(b) << 8U); + // Manually unrolled because the difference in performance is drastic. Can't use table because size limitations. + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + value_ = static_cast(static_cast(value_ << 1U) ^ (((value_ & Top) != 0U) ? Poly : 0U)); + } + + void update(const std::size_t size, const std::uint8_t* const ptr) noexcept + { + const auto* p = ptr; + for (std::size_t s = 0; s < size; s++) + { + update(*p++); + } + } + + [[nodiscard]] auto get() const noexcept { return value_; } + + [[nodiscard]] auto getBytes() const noexcept -> std::array + { + const auto x = get(); + return {static_cast(x >> BitsPerByte), static_cast(x)}; + } + + [[nodiscard]] auto isResidueCorrect() const noexcept { return value_ == 0; } + +private: + static constexpr std::uint16_t Top = 0x8000U; + static constexpr std::uint16_t Poly = 0x1021U; + + std::uint16_t value_ = std::numeric_limits::max(); +}; + +inline auto makePseudoUniqueID(const SystemInfo::UniqueID& uid) -> std::uint64_t +{ + ::kocherga::detail::CRC64 crc; + crc.update(uid.data(), uid.size()); + return crc.get(); +} + +/// If the local-ID is provided, the filter will match only on service transfers addressed to the local node. +/// If the local-ID is not provided, the filter will match on PnP allocation response messages only. +template +[[nodiscard]] auto makeAcceptanceFilter(const std::optional local_node_id) -> CANAcceptanceFilterConfig; +template <> +[[nodiscard]] inline auto makeAcceptanceFilter<0>(const std::optional local_node_id) + -> CANAcceptanceFilterConfig +{ + if (local_node_id) // Match on v0 service transfers addressed to local_node_id. + { + return { + 0b00000'00000000'0'0000000'1'0000000U | (static_cast(*local_node_id) << 8U), + 0b00000'00000000'0'1111111'1'0000000U, + }; + } + return { + 0b00000'0000000000000001'0'0000000U, // Match on PnP node-ID allocation messages (both request/response). + 0b00000'0000000000000011'1'0000000U, // Ignore the discriminator. + }; +} +template <> +[[nodiscard]] inline auto makeAcceptanceFilter<1>(const std::optional local_node_id) + -> CANAcceptanceFilterConfig +{ + if (local_node_id) // Match on v1 service transfers addressed to local_node_id. + { + return { + 0b000'10'0000000000'0000000'0000000U | (static_cast(*local_node_id) << 7U), + 0b000'10'1000000000'1111111'0000000U, + }; + } + return { + 0b000'00'0001111111100101'00000000U, // Match on PnP node-ID allocation response messages of both versions. + 0b000'11'1001111111111100'10000000U, + }; +} + +struct FrameModel +{ + std::uint8_t priority = std::numeric_limits::max(); + std::uint8_t transfer_id = std::numeric_limits::max(); + + bool start_of_transfer = false; + bool end_of_transfer = false; + bool toggle = false; + + std::size_t payload_size = 0; + const std::uint8_t* payload = nullptr; +}; + +struct MessageFrameModel : public FrameModel +{ + PortID subject_id = std::numeric_limits::max(); + std::optional source_node_id; +}; + +struct ServiceFrameModel : public FrameModel +{ + PortID service_id = std::numeric_limits::max(); + std::uint8_t source_node_id = std::numeric_limits::max(); + std::uint8_t destination_node_id = std::numeric_limits::max(); + bool request_not_response = false; +}; + +/// The payload of the returned frame will be pointing into the supplied payload buffer, so their lifetimes are linked. +/// The CAN ID shall be less than 2**29. +[[nodiscard]] inline auto parseFrame(const std::uint32_t extended_can_id, + const std::size_t payload_size, + const void* const payload) + -> std::optional> +{ + assert(payload != nullptr); + if (0 != (extended_can_id & (1ULL << 23U))) // Reserved bit. + { + return {}; + } + if (payload_size < 1) + { + return {}; + } + // Parse the tail byte. + const auto out_payload_size = static_cast(payload_size - 1U); + const std::uint8_t tail = *((static_cast(payload)) + out_payload_size); // NOSONAR + const bool start_of_transfer = (tail & TailByteStartOfTransfer) != 0; + const bool end_of_transfer = (tail & TailByteEndOfTransfer) != 0; + const bool toggle = (tail & TailByteToggleBit) != 0; + const std::uint8_t transfer_id = (tail & MaxTransferID); + if (start_of_transfer && (!toggle)) + { + return {}; // UAVCAN v0 + } + if ((!start_of_transfer || !end_of_transfer) && (out_payload_size == 0)) + { + return {}; // Multi-frame transfer frames require payload. + } + if (!end_of_transfer && (out_payload_size < 7)) + { + return {}; // Non-last frame of a multi-frame transfer cannot have less than 7 bytes of payload. + } + // Parse the CAN ID. + const auto priority = static_cast((extended_can_id >> 26U) & 7U); + const auto source_node_id_or_discriminator = static_cast(extended_can_id & 0x7FU); + if (const auto is_message = (0 == (extended_can_id & (1ULL << 25U))); is_message) + { + if (0 != (extended_can_id & (1ULL << 7U))) + { + return {}; + } + MessageFrameModel out{}; + out.priority = priority; + out.transfer_id = transfer_id; + out.start_of_transfer = start_of_transfer; + out.end_of_transfer = end_of_transfer; + out.toggle = toggle; + out.payload_size = out_payload_size; + out.payload = static_cast(payload); + out.subject_id = static_cast((extended_can_id >> 8U) & 0x1FFFU); + if (const bool is_anonymous = (0 != (extended_can_id & (1ULL << 24U))); !is_anonymous) + { + out.source_node_id = source_node_id_or_discriminator; + } + else + { + if (!(start_of_transfer && end_of_transfer)) + { + return {}; // Anonymous transfers can be only single-frame transfers. + } + } + return out; + } + ServiceFrameModel out{}; + out.priority = priority; + out.transfer_id = transfer_id; + out.start_of_transfer = start_of_transfer; + out.end_of_transfer = end_of_transfer; + out.toggle = toggle; + out.payload_size = out_payload_size; + out.payload = static_cast(payload); + out.service_id = static_cast((extended_can_id >> 14U) & 0x1FFU); + out.source_node_id = source_node_id_or_discriminator; + out.destination_node_id = static_cast((extended_can_id >> 7U) & 0x7FU); + out.request_not_response = 0 != (extended_can_id & (1ULL << 24U)); + if (out.source_node_id == out.destination_node_id) + { + return {}; + } + return out; +} + +/// This function refers to the "data type ID" as "subject/service-ID" for reasons of unification with v1. +[[nodiscard]] inline auto parseFrameV0(const std::uint32_t extended_can_id, + const std::size_t payload_size, + const void* const payload) + -> std::optional> +{ + assert(payload != nullptr); + if ((payload_size < 1) || (payload_size > 8)) // Legacy UAVCAN v0 is compatible only with Classic CAN. + { // This is because the low granularity of DLC in CAN FD breaks TAO. + return {}; + } + const auto out_payload_size = static_cast(payload_size - 1U); + const std::uint8_t tail = *((static_cast(payload)) + out_payload_size); // NOSONAR + const bool start_of_transfer = (tail & TailByteStartOfTransfer) != 0; + const bool end_of_transfer = (tail & TailByteEndOfTransfer) != 0; + const bool toggle = (tail & TailByteToggleBit) != 0; + const std::uint8_t transfer_id = (tail & MaxTransferID); + if (start_of_transfer && toggle) + { + return {}; // UAVCAN v1 + } + const auto priority = static_cast((extended_can_id >> 24U) & 31U); + const auto source_node_id = static_cast(extended_can_id & 0x7FU); + if (const auto is_message = (0 == (extended_can_id & (1ULL << 7U))); is_message) + { + MessageFrameModel out{}; + out.priority = priority; + out.transfer_id = transfer_id; + out.start_of_transfer = start_of_transfer; + out.end_of_transfer = end_of_transfer; + out.toggle = toggle; + out.payload_size = out_payload_size; + out.payload = static_cast(payload); + if (source_node_id != 0) + { + out.source_node_id = source_node_id; + out.subject_id = static_cast((extended_can_id >> 8U) & 0xFFFFU); + } + else + { + if (!(start_of_transfer && end_of_transfer)) + { + return {}; // Anonymous transfers can be only single-frame transfers. + } + out.subject_id = static_cast((extended_can_id >> 8U) & 0b11U); // Ignore the discriminator. + } + return out; + } + ServiceFrameModel out{}; + out.priority = priority; + out.transfer_id = transfer_id; + out.start_of_transfer = start_of_transfer; + out.end_of_transfer = end_of_transfer; + out.toggle = toggle; + out.payload_size = out_payload_size; + out.payload = static_cast(payload); + out.service_id = static_cast((extended_can_id >> 16U) & 0xFFU); + out.source_node_id = source_node_id; + out.destination_node_id = static_cast((extended_can_id >> 8U) & 0x7FU); + out.request_not_response = 0 != (extended_can_id & (1ULL << 15U)); + if ((out.source_node_id == out.destination_node_id) || (out.source_node_id == 0) || (out.destination_node_id == 0)) + { + return {}; + } + return out; +} + +/// This transfer reassembler is only suitable for very basic applications such as this bootloader, where the edge +/// cases that are not correctly handled by this implementation can be tolerated. Other applications should not rely +/// on it; instead, a proper implementation such as that provided in libcanard should be used. +/// The main limitation is that this reassembler does not support interleaved transfers, which is OK for the bootloader. +/// The user of this class is responsible for checking the port-ID. +template +class SimplifiedTransferReassembler +{ +public: + using Result = std::pair; + +protected: + SimplifiedTransferReassembler() = default; + + /// The payload pointer in the result remains valid until the next update. + [[nodiscard]] auto updateImpl(const FrameModel& frame, const std::uint8_t source) -> std::optional + { + if (frame.start_of_transfer) + { + if ((source == source_node_id_) && (frame.transfer_id == transfer_id_)) + { + return {}; // Drop the duplicate. + } + start(source, frame.transfer_id); + } + else + { + const bool match = (state_) && // + (source == source_node_id_) && // + (frame.transfer_id == transfer_id_) && // + (frame.toggle == !state_->toggle); + if (!match) + { + return {}; + } + state_->toggle = !state_->toggle; + } + state_->crc.update(frame.payload_size, frame.payload); + const auto sz = std::min(frame.payload_size, payload_.size() - state_->stored_payload_size); + std::copy_n(frame.payload, sz, payload_.begin() + state_->stored_payload_size); + state_->stored_payload_size += sz; + state_->received_payload_size += frame.payload_size; + if (frame.end_of_transfer) + { + const TransferState final = *state_; + state_.reset(); + if (frame.start_of_transfer) // This is a single-frame transfer. + { + return Result{final.stored_payload_size, payload_.data()}; + } + if ((final.received_payload_size >= CRC16CCITT::Size) && final.crc.isResidueCorrect()) + { + return Result{std::min(final.stored_payload_size, final.received_payload_size - CRC16CCITT::Size), + payload_.data()}; + } + } + return {}; + } + + std::array payload_{}; + +private: + void start(const std::uint8_t source_node_id, const std::uint8_t transfer_id) + { + state_.emplace(); + source_node_id_ = source_node_id; + transfer_id_ = transfer_id; + } + + std::uint8_t source_node_id_ = std::numeric_limits::max(); + std::uint8_t transfer_id_ = std::numeric_limits::max(); + + struct TransferState + { + std::size_t stored_payload_size = 0; + std::size_t received_payload_size = 0; + CRC16CCITT crc; + bool toggle = true; + }; + std::optional state_; +}; + +/// The user of this class is responsible for checking the subject-ID on the received frames. +template +class SimplifiedMessageTransferReassembler : public SimplifiedTransferReassembler +{ +public: + using typename SimplifiedTransferReassembler::Result; + + /// The payload pointer in the result remains valid until the next update. + [[nodiscard]] auto update(const MessageFrameModel& frame) -> std::optional + { + if (!frame.source_node_id) // Anonymous frames accepted unconditionally. + { + const auto sz = std::min(frame.payload_size, payload_.size()); + std::copy_n(frame.payload, sz, payload_.data()); + return Result{sz, payload_.data()}; + } + return SimplifiedTransferReassembler::updateImpl(frame, *frame.source_node_id); + } + +private: + using SimplifiedTransferReassembler::payload_; +}; + +/// The user of this class is responsible for checking the service-ID and request/response flag on the received frames. +template +class SimplifiedServiceTransferReassembler : public SimplifiedTransferReassembler +{ +public: + using typename SimplifiedTransferReassembler::Result; + + explicit SimplifiedServiceTransferReassembler(const std::uint8_t local_node_id) : local_node_id_(local_node_id) {} + + /// The payload pointer in the result remains valid until the next update. + [[nodiscard]] auto update(const ServiceFrameModel& frame) -> std::optional + { + if (local_node_id_ == frame.destination_node_id) + { + return SimplifiedTransferReassembler::updateImpl(frame, frame.source_node_id); + } + return {}; + } + +private: + const std::uint8_t local_node_id_; +}; + +/// This is like the above but for the legacy v0 protocol. +/// Unlike the v1 implementation, this one does not implement implicit payload truncation as it is not defined for v0. +template +class SimplifiedTransferReassemblerV0 +{ +public: + using Result = std::pair; + +protected: + explicit SimplifiedTransferReassemblerV0(const std::uint64_t signature) : signature_(signature) {} + + /// The payload pointer in the result remains valid until the next update. + [[nodiscard]] auto updateImpl(const FrameModel& frame, const std::uint8_t source) -> std::optional + { + if (frame.start_of_transfer) + { + if ((source == source_node_id_) && (frame.transfer_id == transfer_id_)) + { + return {}; // Drop the duplicate. + } + start(source, frame.transfer_id); + } + else + { + if (!((state_) && (source == source_node_id_) && (frame.transfer_id == transfer_id_) && + (frame.toggle == !state_->toggle))) + { + return {}; + } + state_->toggle = !state_->toggle; + } + if (frame.payload_size > (buffer_.size() - state_->payload_size)) + { + state_.reset(); // Too much payload -- UAVCAN v0 does not define payload truncation. + return {}; + } + std::copy_n(frame.payload, frame.payload_size, buffer_.begin() + state_->payload_size); + state_->payload_size += frame.payload_size; + if (frame.end_of_transfer) + { + const TransferState final = *state_; + state_.reset(); + if (frame.start_of_transfer) // This is a single-frame transfer. + { + return Result{final.payload_size, buffer_.data()}; + } + if (final.payload_size >= CRC16CCITT::Size) + { + CRC16CCITT crc; + for (auto i = 0U; i < 8U; i++) + { + crc.update(static_cast((signature_ >> (i * 8U)) & 0xFFU)); + } + crc.update(final.payload_size - CRC16CCITT::Size, buffer_.begin() + CRC16CCITT::Size); + if ((buffer_.at(0) == (crc.get() & 0xFFU)) && (buffer_.at(1) == (crc.get() >> 8U))) + { + return Result{final.payload_size - CRC16CCITT::Size, buffer_.begin() + CRC16CCITT::Size}; + } + } + } + return {}; + } + + std::array buffer_{}; + +private: + void start(const std::uint8_t source_node_id, const std::uint8_t transfer_id) + { + state_.emplace(); + source_node_id_ = source_node_id; + transfer_id_ = transfer_id; + } + + std::uint8_t source_node_id_ = std::numeric_limits::max(); + std::uint8_t transfer_id_ = std::numeric_limits::max(); + + struct TransferState + { + std::size_t payload_size = 0; + bool toggle = false; + }; + std::optional state_; + + const std::uint64_t signature_; +}; + +/// The user of this class is responsible for checking the subject-ID on the received frames. +template +class SimplifiedMessageTransferReassemblerV0 : public SimplifiedTransferReassemblerV0 +{ + using Base = SimplifiedTransferReassemblerV0; + +public: + using typename Base::Result; + + explicit SimplifiedMessageTransferReassemblerV0(const std::uint64_t signature) : Base(signature) {} + + /// The payload pointer in the result remains valid until the next update. + [[nodiscard]] auto update(const MessageFrameModel& frame) -> std::optional + { + if (!frame.source_node_id) // Anonymous frames accepted unconditionally. + { + const auto sz = std::min(frame.payload_size, buffer_.size()); + std::copy_n(frame.payload, sz, buffer_.data()); + return Result{sz, buffer_.data()}; + } + return Base::updateImpl(frame, *frame.source_node_id); + } + +private: + using Base::buffer_; +}; + +/// The user of this class is responsible for checking the service-ID and request/response flag on the received frames. +template +class SimplifiedServiceTransferReassemblerV0 : public SimplifiedTransferReassemblerV0 +{ + using Base = SimplifiedTransferReassemblerV0; + +public: + using typename Base::Result; + + SimplifiedServiceTransferReassemblerV0(const std::uint64_t signature, const std::uint8_t local_node_id) : + Base(signature), local_node_id_(local_node_id) + {} + + /// The payload pointer in the result remains valid until the next update. + [[nodiscard]] auto update(const ServiceFrameModel& frame) -> std::optional + { + if (local_node_id_ == frame.destination_node_id) + { + return Base::updateImpl(frame, frame.source_node_id); + } + return {}; + } + +private: + const std::uint8_t local_node_id_; +}; + +/// Send one UAVCAN/CAN v1 transfer. The push_frame callback is invoked per each transmitted frame; its type should be: +/// (std::size_t, const std::uint8_t*) -> bool +/// The return value is true on success, false otherwise. +/// The callback shall not be an std::function<> or std::bind<> to avoid heap allocation. +/// The MTU shall be representable as a valid DLC and be no less than 8 bytes. +/// The caller is responsible for computing the CAN ID (which shall be the same for all frames of this transfer). +template +[[nodiscard]] static inline auto transmit(const Callback& push_frame, + const std::size_t transport_layer_mtu, + const std::uint8_t transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool +{ + ICANDriver::PayloadBuffer buf{}; + if ((transport_layer_mtu < 8U) || (transport_layer_mtu > buf.size()) || (transfer_id > MaxTransferID) || + (ICANDriver::DLCToLength.at(ICANDriver::LengthToDLC.at(transport_layer_mtu)) != transport_layer_mtu) || + ((payload == nullptr) && (payload_length > 0))) + { + return false; + } + const std::size_t mtu = transport_layer_mtu - 1U; + assert((mtu >= 7U) && (mtu <= 63U)); + + // SINGLE-FRAME TRANSFER + if (payload_length <= mtu) + { + std::copy_n(payload, payload_length, buf.begin()); + const auto dlc = ICANDriver::LengthToDLC.at(payload_length + 1); + const auto len = ICANDriver::DLCToLength.at(dlc); + assert(len > 0); + buf.at(len - 1U) = static_cast(static_cast(TailByteStartOfTransfer) | + TailByteEndOfTransfer | TailByteToggleBit | transfer_id); + return push_frame(len, buf.data()); + } + + // MULTI-FRAME TRANSFER + CRC16CCITT crc; + crc.update(payload_length, payload); + std::size_t remaining = payload_length; + const std::uint8_t* ptr = payload; + bool toggle = true; + while (remaining > mtu) + { + std::copy_n(ptr, mtu, buf.begin()); + const auto tail_byte = + static_cast(transfer_id | // + (toggle ? TailByteToggleBit : 0U) | // + ((remaining == payload_length) ? TailByteStartOfTransfer : 0U)); + toggle = !toggle; + buf.at(mtu) = tail_byte; + if (!push_frame(mtu + 1U, buf.data())) + { + return false; + } + ptr += mtu; + remaining -= mtu; + } + // Last frame requires special treatment: the CRC may go into the same frame or be split off, this is convoluted. + std::copy_n(ptr, remaining, buf.begin()); + if ((remaining + CRC16CCITT::Size) <= mtu) // Remaining bytes plus CRC fit into one frame; padding may be needed. + { + auto* dst = buf.begin() + remaining; + const auto dlc = ICANDriver::LengthToDLC.at(remaining + CRC16CCITT::Size + 1U); + const auto len = ICANDriver::DLCToLength.at(dlc); + assert(len >= (remaining + CRC16CCITT::Size + 1U)); + const auto padding = len - remaining - CRC16CCITT::Size - 1U; + assert(padding < 16); + for (auto i = 0U; i < padding; i++) + { + crc.update(0); + *dst++ = 0; + } + for (auto x : crc.getBytes()) + { + *dst++ = x; + } + *dst = static_cast(transfer_id | (toggle ? TailByteToggleBit : 0U) | TailByteEndOfTransfer); + return push_frame(len, buf.data()); + } + // Padding is not needed but one extra frame is required to contain (part of) the CRC. + const auto crc_bytes = crc.getBytes(); + const auto* crc_bytes_it = crc_bytes.begin(); + assert(mtu >= remaining); + for (auto i = remaining; i < mtu; i++) + { + buf.at(i) = *crc_bytes_it++; + } + buf.at(mtu) = static_cast(transfer_id | (toggle ? TailByteToggleBit : 0U)); + toggle = !toggle; + if (!push_frame(mtu + 1U, buf.data())) + { + return false; + } + auto* buf_it = std::copy(crc_bytes_it, crc_bytes.end(), buf.begin()); + *buf_it++ = static_cast(transfer_id | (toggle ? TailByteToggleBit : 0U) | TailByteEndOfTransfer); + const auto size = buf_it - buf.begin(); + assert((size >= 2) && (size <= static_cast(CRC16CCITT::Size + 1U))); + return push_frame(static_cast(size), buf.data()); +} + +/// This is like transmit() but for the legacy v0 protocol. +/// It is substantially simpler because v0 does not need padding and the CRC is located in the first frame. +template +[[nodiscard]] static inline auto transmitV0(const Callback& push_frame, + const std::uint64_t signature, + const std::uint8_t transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool +{ + static constexpr auto MTU = 7U; + std::array buf{}; + if ((transfer_id > MaxTransferID) || ((payload == nullptr) && (payload_length > 0))) + { + return false; + } + // SINGLE-FRAME TRANSFER + if (payload_length <= MTU) + { + std::copy_n(payload, payload_length, buf.begin()); + buf.at(payload_length) = static_cast(static_cast(TailByteStartOfTransfer) | + TailByteEndOfTransfer | transfer_id); + return push_frame(payload_length + 1, buf.data()); + } + // MULTI-FRAME TRANSFER + CRC16CCITT crc; + for (auto i = 0U; i < 8U; i++) + { + crc.update(static_cast((signature >> (i * 8U)) & 0xFFU)); + } + crc.update(payload_length, payload); + std::size_t remaining = payload_length; + const std::uint8_t* ptr = payload; + // First frame + buf.at(0) = static_cast(crc.get() >> 0U); + buf.at(1) = static_cast(crc.get() >> 8U); + std::copy_n(ptr, MTU - CRC16CCITT::Size, buf.begin() + CRC16CCITT::Size); + buf.at(MTU) = static_cast(transfer_id | TailByteStartOfTransfer); + ptr += MTU - CRC16CCITT::Size; + remaining -= MTU - CRC16CCITT::Size; + if (!push_frame(MTU + 1U, buf.data())) + { + return false; + } + // Middle frames + bool toggle = true; + while (remaining > MTU) + { + std::copy_n(ptr, MTU, buf.begin()); + buf.at(MTU) = static_cast(transfer_id | (toggle ? TailByteToggleBit : 0U)); + if (!push_frame(MTU + 1U, buf.data())) + { + return false; + } + toggle = !toggle; + ptr += MTU; + remaining -= MTU; + } + // Last frame + assert(remaining <= MTU); + std::copy_n(ptr, remaining, buf.begin()); + buf.at(remaining) = + static_cast(transfer_id | (toggle ? TailByteToggleBit : 0U) | TailByteEndOfTransfer); + return push_frame(static_cast(remaining + 1U), buf.data()); +} + +class IAllocator +{ +public: + [[nodiscard]] virtual auto allocate(const std::size_t size) -> void* = 0; + virtual void deallocate(const void* const ptr) = 0; + + template + [[nodoscard]] auto construct(Args&&... ag) -> T* + { + if (void* const p = allocate(sizeof(T))) + { + return new (p) T(std::forward(ag)...); // NOLINT + } + return nullptr; + } + + template + void destroy(T* const obj) + { + if (obj != nullptr) + { + obj->~T(); + deallocate(obj); + } + } + + virtual ~IAllocator() = default; + IAllocator() = default; + IAllocator(const IAllocator&) = delete; + IAllocator(IAllocator&&) = delete; + auto operator=(const IAllocator&) -> IAllocator& = delete; + auto operator=(IAllocator&&) -> IAllocator& = delete; +}; + +template +class BlockAllocator : public IAllocator +{ +public: + [[nodiscard]] auto allocate(const std::size_t size) -> void* override + { + if (size <= BlockSize) + { + for (auto& blk : pool_) + { + auto& [used, mem] = blk; + if (!used) + { + used = true; + return &mem; + } + } + } + return nullptr; + } + + void deallocate(const void* const ptr) override + { + for (auto& blk : pool_) + { + auto& [used, mem] = blk; + if (used && (&mem == ptr)) + { + used = false; + return; + } + } + assert(false); + } + +private: + using Block = std::pair>; + std::array pool_{}; + + static_assert(sizeof(pool_) > (BlockSize * BlockCount)); +}; + +/// Use the standard State behavioral pattern to segregate business logic by activity. +class IActivity +{ +public: + /// If this method returns a non-null, the new activity shall replace the current one. + /// It is not allowed to call this method again after it returned a non-zero pointer. + [[nodiscard]] virtual auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* = 0; + + // See kocherga::INode + [[nodiscard]] virtual auto sendRequest(const ServiceID service_id, + const NodeID server_node_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + (void) service_id; + (void) server_node_id; + (void) transfer_id; + (void) payload_length; + (void) payload; + return false; + } + + // See kocherga::INode + virtual void cancelRequest() {} + + // See kocherga::INode + [[nodiscard]] virtual auto publishMessage(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + (void) subject_id; + (void) transfer_id; + (void) payload_length; + (void) payload; + return false; + } + + virtual ~IActivity() = default; + IActivity() = default; + IActivity(const IActivity&) = delete; + IActivity(IActivity&&) = delete; + auto operator=(const IActivity&) -> IActivity& = delete; + auto operator=(IActivity&&) -> IActivity& = delete; +}; + +/// The v0 main activity is a simplified translation layer between UAVCAN/CAN v1 and UAVCAN v0. +/// The following ports are supported: +/// +/// Service RX TX +/// ----------------------------------------------------------------------------- +/// FileRead MFT response MFT request +/// GetInfo SFT request MFT response +/// ExecuteCommand MFT request SFT response +/// Heartbeat SFT +class V0MainActivity : public IActivity +{ +public: + V0MainActivity(ICANDriver& driver, const std::uint8_t local_node_id) : + driver_(driver), local_node_id_(local_node_id) + { + assert((local_node_id_ > 0) && (local_node_id_ <= MaxNodeID)); + } + + auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* override + { + (void) uptime; + ICANDriver::PayloadBuffer buf{}; + while (const auto transport_frame = driver_.pop(buf)) + { + const auto [can_id, payload_size] = *transport_frame; + if (const auto frame = detail::parseFrameV0(can_id, payload_size, buf.data())) + { + if (const auto* const s = std::get_if(&*frame)) + { + processReceivedServiceFrame(reactor, *s); + } + // This implementation is not interested in accepting any message transfers. + } + } + return nullptr; + } + + [[nodiscard]] auto getLocalNodeID() const -> std::uint8_t { return local_node_id_; } + +private: + [[nodiscard]] auto sendRequest(const ServiceID service_id, + const NodeID server_node_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + if ((server_node_id > 0) && (server_node_id <= MaxNodeID)) + { + if (service_id == ServiceID::FileRead) + { + return sendFileReadRequest(server_node_id, + static_cast(transfer_id & MaxTransferID), + payload_length, + payload); + } + } + return false; // Don't know how to translate this request v1 --> v0. + } + + [[nodiscard]] auto sendResponse(const std::uint64_t signature, + const std::uint8_t priority, + const std::uint16_t service_id, + const std::uint8_t client_node_id, + const std::uint8_t transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + static constexpr std::uint32_t CANIDMask = 0b00000'00000000'0'0000000'1'0000000UL; + // + const auto can_id = CANIDMask | // + (static_cast(priority) << 24U) | // + (static_cast(service_id) << 16U) | // + (static_cast(client_node_id) << 8U) | // + local_node_id_; + return send(signature, can_id, transfer_id, payload_length, payload); + } + + void cancelRequest() override { pending_request_meta_.reset(); } + + [[nodiscard]] auto publishMessage(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + if (subject_id == SubjectID::NodeHeartbeat) + { + return publishHeartbeat(transfer_id, payload_length, payload); + } + if (subject_id == SubjectID::DiagnosticRecord) + { + return publishDiagnosticRecord(transfer_id, payload_length, payload); + } + return false; // Don't know how to translate this message v1 --> v0. + } + + void processReceivedServiceFrame(IReactor& reactor, const ServiceFrameModel& frame) + { + if (frame.request_not_response) + { + if (frame.service_id == static_cast(ServiceTypeID::GetNodeInfo)) + { + if (const auto req = rx_req_get_node_info_.update(frame)) + { + processGetNodeInfoRequest(reactor, frame, req->first, req->second); + } + } + if (frame.service_id == static_cast(ServiceTypeID::BeginFirmwareUpdate)) + { + if (const auto req = rx_req_begin_fw_upd_.update(frame)) + { + processBeginFirmwareUpdateRequest(reactor, frame, req->first, req->second); + } + } + } + else + { + if (pending_request_meta_ && // + (pending_request_meta_->server_node_id == frame.source_node_id) && // + (pending_request_meta_->service_id == frame.service_id) && // + (pending_request_meta_->transfer_id == frame.transfer_id)) + { + if (frame.service_id == static_cast(ServiceTypeID::FileRead)) + { + if (const auto res = rx_res_file_read_.update(frame)) + { + pending_request_meta_.reset(); + processFileReadResponse(reactor, res->first, res->second); + } + } + else + { + assert(false); // This means that we've sent a request for which there is no response listener. + } + } + } + } + + void processGetNodeInfoRequest(IReactor& reactor, + const ServiceFrameModel& frame, + const std::size_t request_size, + const std::uint8_t* const request_data) + { + assert(frame.destination_node_id == local_node_id_); + assert(frame.request_not_response); + (void) request_size; // No data is needed. + std::array response_data{}; + if (const auto response_size = reactor.processRequest(static_cast(ServiceID::NodeGetInfo), + frame.source_node_id, + 0, + request_data, + response_data.data()); + response_size >= 31) + { + // Translate the response v1 -> v0. + std::array buf{}; + buf.back() = 0xAAU; // Canary. + std::uint8_t* ptr = buf.begin(); + (void) std::memcpy(ptr, last_node_status_.data(), last_node_status_.size()); + ptr += last_node_status_.size(); + *ptr++ = response_data.at(4); // Software version + *ptr++ = response_data.at(5); + ptr += 13U; // Optional fields not available: no VCS hash, no CRC + *ptr++ = response_data.at(2); // Hardware version. + *ptr++ = response_data.at(3); + (void) std::memcpy(ptr, response_data.begin() + 14, 16); // Unique-ID + ptr += 17U; // Skip UID and CoA + const auto name_len = std::min(response_data.at(30), 80); + (void) std::memcpy(ptr, response_data.begin() + 31, name_len); + assert(buf.back() == 0xAAU); // Check the canary. + (void) sendResponse(GetNodeInfoSignature, + frame.priority, + frame.service_id, + frame.source_node_id, + frame.transfer_id, + name_len + 41U, + buf.data()); + } + } + + void processBeginFirmwareUpdateRequest(IReactor& reactor, + const ServiceFrameModel& frame, + const std::size_t request_size, + const std::uint8_t* const request_data) + { + assert(frame.destination_node_id == local_node_id_); + assert(frame.request_not_response); + std::array scratchpad{}; + scratchpad.back() = 0xAA; // Canary + // Translate the request v0 --> v1, store into the scratchpad. + scratchpad.at(0) = 0xFD; + scratchpad.at(1) = 0xFF; + if ((request_size >= 2) && (request_size <= 202)) + { + const auto path_len = static_cast(request_size - 1U); + scratchpad.at(2) = path_len; + (void) std::memcpy(scratchpad.begin() + 3U, request_data + 1U, path_len); + assert(scratchpad.back() == 0xAA); + std::array response_data{}; + if (const auto response_size = reactor.processRequest(static_cast(ServiceID::NodeExecuteCommand), + frame.source_node_id, + path_len + 3U, + scratchpad.data(), + response_data.data()); + response_size >= 1U) + { + // Translate the response v1 --> v0 re-using the same scratchpad. + scratchpad.at(0) = (response_data.at(0) == 0) ? 0 : 255; // Either ERROR_OK or ERROR_UNKNOWN. + (void) sendResponse(BeginFirmwareUpdateSignature, + frame.priority, + frame.service_id, + frame.source_node_id, + frame.transfer_id, + 1U, + scratchpad.data()); + } + } + } + + static void processFileReadResponse(IReactor& reactor, + const std::size_t response_size, + const std::uint8_t* const response_data) + { + if (response_size >= 4) + { + std::array buf{}; + buf.back() = 0xAA; + buf.at(0) = response_data[0]; + buf.at(1) = response_data[1]; + const auto len = response_size - 2U; + if (len <= 256U) + { + buf.at(2) = static_cast(len >> 0U); + buf.at(3) = static_cast(len >> 8U); + (void) std::memcpy(buf.begin() + 4, response_data + 2U, len); + assert(buf.back() == 0xAA); + reactor.processResponse(len + 4U, buf.data()); + } + } + } + + [[nodiscard]] auto sendFileReadRequest(const NodeID server_node_id, + const std::uint8_t transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + if (payload_length >= 6U) + { + // Convert v1 --> v0: apply TAO by removing the length field. + std::array buf{}; + buf.back() = 0xAA; + (void) std::memcpy(buf.data(), payload, 5U); + const auto path_len = payload[5]; + (void) std::memcpy(buf.data() + 5U, payload + 6U, path_len); + assert(buf.back() == 0xAA); + static constexpr std::uint32_t CANIDMask = 0b11110'00000000'1'0000000'1'0000000ULL; + const std::uint32_t extended_can_id = CANIDMask | + (static_cast(ServiceTypeID::FileRead) << 16U) | + (static_cast(server_node_id) << 8U) | local_node_id_; + if (send(FileReadSignature, extended_can_id, transfer_id, path_len + 5U, buf.data())) + { + pending_request_meta_ = PendingRequestMetadata{static_cast(server_node_id), + static_cast(ServiceTypeID::FileRead), + transfer_id}; + return true; + } + } + return false; + } + + [[nodiscard]] auto publishHeartbeat(const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + if (payload_length < 7U) + { + return false; + } + // Translate the message from v1's Heartbeat into v0's NodeStatus. + std::memcpy(last_node_status_.begin(), payload, 4); + last_node_status_.at(4) = static_cast(payload[4] << 6U) | // Health + static_cast(payload[5] << 3U); // Mode + last_node_status_.at(5) = payload[6]; // Vendor-specific status code. + // Publish the v0 NodeStatus + static constexpr std::uint32_t CANIDMask = 0b10000'0000000101010101'0'0000000ULL; + return send(NodeStatusSignature, + CANIDMask | local_node_id_, + transfer_id, + last_node_status_.size(), + last_node_status_.data()); + } + + [[nodiscard]] auto publishDiagnosticRecord(const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + if (payload_length < 9U) + { + return false; + } + std::array buf{}; + buf.at(0) = 0b010'00100U; + buf.at(1) = 'B'; + buf.at(2) = 'o'; + buf.at(3) = 'o'; + buf.at(4) = 't'; + const auto text_len = std::min(payload[8], 90); + std::copy_n(&payload[9], text_len, buf.begin() + 5U); + static constexpr std::uint32_t CANIDMask = 0b11111'0011111111111111'0'0000000ULL; + return send(LogMessageSignature, CANIDMask | local_node_id_, transfer_id, text_len + 5U, buf.data()); + } + + [[nodiscard]] auto send(const std::uint64_t signature, + const std::uint32_t extended_can_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + // This lambda is allocated on the stack, so that the closures do not require heap allocation. + return detail::transmitV0( + [this, extended_can_id](const std::size_t frame_payload_size, + const std::uint8_t* const frame_payload) -> bool { + assert(frame_payload_size <= std::numeric_limits::max()); + return driver_.push(true, // Force Classic CAN mode. + extended_can_id, + static_cast(frame_payload_size), + frame_payload); + }, + signature, + static_cast(transfer_id & detail::MaxTransferID), + payload_length, + payload); + } + + enum class ServiceTypeID : std::uint8_t + { + GetNodeInfo = 1, + BeginFirmwareUpdate = 40, + FileRead = 48, + }; + + struct PendingRequestMetadata + { + std::uint8_t server_node_id{}; + PortID service_id{}; + std::uint8_t transfer_id{}; + }; + + static constexpr std::uint64_t NodeStatusSignature = 0x0F0868D0C1A7C6F1ULL; + static constexpr std::uint64_t LogMessageSignature = 0xD654A48E0C049D75ULL; + static constexpr std::uint64_t GetNodeInfoSignature = 0xEE468A8121C46A9EULL; + static constexpr std::uint64_t BeginFirmwareUpdateSignature = 0xB7D725DF72724126ULL; + static constexpr std::uint64_t FileReadSignature = 0x8DCDCA939F33F678ULL; + + ICANDriver& driver_; + const std::uint8_t local_node_id_; + + std::optional pending_request_meta_; + + std::array last_node_status_{}; + + SimplifiedServiceTransferReassemblerV0<0> rx_req_get_node_info_{GetNodeInfoSignature, local_node_id_}; + SimplifiedServiceTransferReassemblerV0<200> rx_req_begin_fw_upd_{BeginFirmwareUpdateSignature, local_node_id_}; + SimplifiedServiceTransferReassemblerV0<300> rx_res_file_read_{FileReadSignature, local_node_id_}; +}; + +/// The following example shows the CAN exchange dump collected from a real network using the old UAVCAN v0 GUI Tool. +/// +/// Unique-ID of the allocatee: 35 FF D5 05 50 59 31 34 61 41 23 43 00 00 00 00 +/// Preferred node-ID: 0 (any, no preference) +/// Allocated node-ID: 125 (125 << 1 == 0xFA) +/// +/// Dir. Time CAN ID CAN Payload Source +/// ------------------------------------------------------------------------------------------------------- +/// -> 0.525710 1E285100 01 35 FF D5 05 50 59 D8 Anon 1st stage request, bytes [0, 6) +/// <- 0.527186 1400017F 00 35 FF D5 05 50 59 C0 127 1st stage response, 6 bytes of UID +/// -> 0.923752 1E3C1D00 00 31 34 61 41 23 43 D9 Anon 2nd stage request, bytes [6, 12) +/// <- 0.927630 1400017F 9E 3D 00 35 FF D5 05 81 127 2nd stage response, 12 bytes of UID +/// <- 0.927676 1400017F 50 59 31 34 61 41 23 21 127 ... +/// <- 0.927710 1400017F 43 41 127 ... +/// -> 1.083750 1E909D00 00 00 00 00 00 DA Anon 3rd stage request, bytes [12, 16) +/// <- 1.088248 1400017F 8C 7A FA 35 FF D5 05 82 127 3rd stage response, 16 bytes of UID +/// <- 1.088300 1400017F 50 59 31 34 61 41 23 22 127 ... +/// <- 1.088752 1400017F 43 00 00 00 00 42 127 ... +class V0NodeIDAllocationActivity : public IActivity +{ +public: + V0NodeIDAllocationActivity(IAllocator& allocator, + ICANDriver& driver, + const SystemInfo::UniqueID& local_uid, + const ICANDriver::Bitrate& bitrate) : + allocator_(allocator), driver_(driver), local_uid_(local_uid), bitrate_(bitrate) + {} + + auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* override + { + (void) reactor; + { + ICANDriver::PayloadBuffer buf{}; + while (const auto frame = driver_.pop(buf)) + { + const auto [can_id, payload_size] = *frame; + if (IActivity* const out = processReceivedFrame(uptime, can_id, payload_size, buf.data())) + { + return out; + } + } + } + if (deadline_) + { + if (uptime >= *deadline_) + { + handleDeadline(uptime); + } + } + else // First call -- initialize the scheduler + { + reset(uptime); + assert(deadline_); + } + return nullptr; + } + + [[nodiscard]] auto getDeadline() const -> std::chrono::microseconds + { + return deadline_ ? *deadline_ : std::chrono::microseconds{}; + } + + [[nodiscard]] auto getStage() const { return stage_; } + +private: + using DelayRange = std::pair; + + [[nodiscard]] auto processReceivedFrame(const std::chrono::microseconds now, + const std::uint32_t can_id, + const std::size_t payload_size, + const std::uint8_t* const payload) -> IActivity* + { + if (const auto f = detail::parseFrameV0(can_id, payload_size, payload)) + { + if (const auto* const mf = std::get_if(&*f)) + { + if (mf->subject_id == DataTypeID) + { + if (const auto res = rx_.update(*mf)) + { + return acceptAllocationResponseMessage(now, res->first, res->second); + } + } + } + } + return nullptr; + } + + [[nodiscard]] auto acceptAllocationResponseMessage(const std::chrono::microseconds now, + const std::size_t message_data_size, + const std::uint8_t* const message_data) -> IActivity* + { + if ((message_data_size < 2) || (message_data_size > 17)) + { + return nullptr; + } + const auto received_uid_size = message_data_size - 1; + if (0 != std::memcmp(local_uid_.data(), message_data + 1, received_uid_size)) + { + reset(now); // This response is meant for somebody else, set up new schedule to avoid conflicts. + return nullptr; + } + if (received_uid_size < local_uid_.size()) + { + stage_ = (received_uid_size >= 12) ? 2 : 1; + schedule(now, DelayRangeFollowup); + return nullptr; + } + const auto node_id = static_cast(message_data[0] >> 1U); + if (node_id < 1) // Bad allocation + { + return nullptr; + } + assert(node_id <= MaxNodeID); + // Allocation done, full match. + if (const auto bus_mode = driver_.configure(bitrate_, false, makeAcceptanceFilter<0>(node_id))) + { + (void) bus_mode; + return allocator_.construct(driver_, node_id); + } + return nullptr; + } + + void handleDeadline(const std::chrono::microseconds now) + { + if (0 == stage_) + { + const std::array buf{{ + 1, // first_part_of_unique_id=true, any node-ID (no preference) + local_uid_.at(0), + local_uid_.at(1), + local_uid_.at(2), + local_uid_.at(3), + local_uid_.at(4), + local_uid_.at(5), + static_cast(0b1100'0000U | tx_transfer_id_), + }}; + (void) send(buf); + } + else if (1 == stage_) + { + const std::array buf{{ + 0, // first_part_of_unique_id=false, any node-ID (no preference) + local_uid_.at(6), + local_uid_.at(7), + local_uid_.at(8), + local_uid_.at(9), + local_uid_.at(10), + local_uid_.at(11), + static_cast(0b1100'0000U | tx_transfer_id_), + }}; + (void) send(buf); + } + else + { + const std::array buf{{ + 0, // first_part_of_unique_id=false, any node-ID (no preference) + local_uid_.at(12), + local_uid_.at(13), + local_uid_.at(14), + local_uid_.at(15), + static_cast(0b1100'0000U | tx_transfer_id_), + }}; + (void) send(buf); + } + tx_transfer_id_++; + reset(now); + } + + void reset(const std::chrono::microseconds now) + { + stage_ = 0; + schedule(now, DelayRangeRequest); + } + + void schedule(const std::chrono::microseconds now, const DelayRange range) + { + const auto delta = std::abs(range.first.count() - range.second.count()); + const auto randomized = (std::rand() * delta) / RAND_MAX; // NOSONAR rand() ok + assert((0 <= randomized) && (randomized <= delta)); + const auto delay = std::max(std::chrono::microseconds(1), range.first) + std::chrono::microseconds(randomized); + assert(range.first <= delay); + assert(range.second >= delay); + deadline_ = now + delay; + } + + template + [[nodoscard]] auto send(const std::array& payload) -> bool + { + static_assert(PayloadSize <= 8); + CRC16CCITT discriminator_crc; + discriminator_crc.update(PayloadSize, payload.data()); + const std::uint32_t discriminator = discriminator_crc.get() & ((1UL << 14U) - 1U); + const std::uint32_t can_id = CANIDMaskWithoutDiscriminator | (discriminator << 10U); + return driver_.push(true, can_id, PayloadSize, payload.data()); + } + + static constexpr DelayRange DelayRangeRequest{600'000, 1'000'000}; + static constexpr DelayRange DelayRangeFollowup{0, 400'000}; + static constexpr std::uint32_t CANIDMaskWithoutDiscriminator = 0x1E'0001'00UL; + + static constexpr std::uint16_t DataTypeID = 1; // uavcan.protocol.dynamic_node_id.Allocation + + IAllocator& allocator_; + ICANDriver& driver_; + const SystemInfo::UniqueID local_uid_; + const ICANDriver::Bitrate bitrate_; + + std::optional deadline_; + std::uint8_t stage_ = 0; + + std::uint8_t tx_transfer_id_ = 0; + + SimplifiedMessageTransferReassemblerV0<19> rx_{0x0B2A812620A11D40ULL}; +}; + +class V1MainActivity : public IActivity +{ +public: + V1MainActivity(ICANDriver& driver, const ICANDriver::Mode bus_mode, const std::uint8_t local_node_id) : + driver_(driver), + bus_mode_(bus_mode), + local_node_id_(local_node_id), + rx_file_read_response_(local_node_id), + rx_get_info_request_(local_node_id), + rx_execute_command_request_(local_node_id) + {} + + auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* override + { + (void) uptime; + ICANDriver::PayloadBuffer buf{}; + while (const auto transport_frame = driver_.pop(buf)) + { + const auto [can_id, payload_size] = *transport_frame; + if (const auto frame = detail::parseFrame(can_id, payload_size, buf.data())) + { + if (const auto* const s = std::get_if(&*frame)) + { + processReceivedServiceFrame(reactor, *s); + } + // This implementation is not interested in accepting any message transfers. + } + } + return nullptr; + } + + [[nodiscard]] auto getLocalNodeID() const -> std::uint8_t { return local_node_id_; } + +private: + [[nodiscard]] auto sendRequest(const ServiceID service_id, + const NodeID server_node_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + static constexpr std::uint32_t CANIDMask = 0b110'11'0000000000'0000000'0000000UL; + if (server_node_id <= MaxNodeID) + { + const auto can_id = CANIDMask | // + (static_cast(service_id) << 14U) | // + (static_cast(server_node_id) << 7U) | // + local_node_id_; + if (send(can_id, transfer_id, payload_length, payload)) + { + pending_request_meta_ = PendingRequestMetadata{static_cast(server_node_id), + static_cast(service_id), + static_cast(transfer_id & MaxTransferID)}; + return true; + } + } + return false; + } + + [[nodiscard]] auto sendResponse(const std::uint8_t priority, + const std::uint16_t service_id, + const std::uint8_t client_node_id, + const std::uint8_t transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + static constexpr std::uint32_t CANIDMask = 0b000'10'0000000000'0000000'0000000UL; + // + const auto can_id = CANIDMask | // + (static_cast(priority) << 26U) | // + (static_cast(service_id) << 14U) | // + (static_cast(client_node_id) << 7U) | // + local_node_id_; + return send(can_id, transfer_id, payload_length, payload); + } + + void cancelRequest() override { pending_request_meta_.reset(); } + + [[nodiscard]] auto publishMessage(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + static constexpr std::uint32_t CANIDMask = 0b100'00'0110000000000000'0'0000000UL; + const auto can_id = CANIDMask | (static_cast(subject_id) << 8U) | local_node_id_; + return send(can_id, transfer_id, payload_length, payload); + } + + void processReceivedServiceFrame(IReactor& reactor, const ServiceFrameModel& frame) + { + if (frame.request_not_response) + { + if (frame.service_id == static_cast(ServiceID::NodeGetInfo)) + { + if (const auto req = rx_get_info_request_.update(frame)) + { + processServiceRequest(reactor, frame, req->first, req->second); + } + } + if (frame.service_id == static_cast(ServiceID::NodeExecuteCommand)) + { + if (const auto req = rx_execute_command_request_.update(frame)) + { + processServiceRequest(reactor, frame, req->first, req->second); + } + } + } + else + { + if (pending_request_meta_ && // + (pending_request_meta_->server_node_id == frame.source_node_id) && // + (pending_request_meta_->service_id == frame.service_id) && // + (pending_request_meta_->transfer_id == frame.transfer_id)) + { + if (frame.service_id == static_cast(ServiceID::FileRead)) + { + if (const auto res = rx_file_read_response_.update(frame)) + { + processServiceResponse(reactor, res->first, res->second); + } + } + else + { + assert(false); // This means that we've sent a request for which there is no response listener. + } + } + } + } + + void processServiceRequest(IReactor& reactor, + const ServiceFrameModel& frame, + const std::size_t request_size, + const std::uint8_t* const request_data) + { + assert(frame.destination_node_id == local_node_id_); + assert(frame.request_not_response); + std::array response_data{}; + if (const auto response_size = reactor.processRequest(frame.service_id, + frame.source_node_id, + request_size, + request_data, + response_data.data())) + { + (void) sendResponse(frame.priority, + frame.service_id, + frame.source_node_id, + frame.transfer_id, + *response_size, + response_data.data()); + } + } + + void processServiceResponse(IReactor& reactor, + const std::size_t response_size, + const std::uint8_t* const response_data) + { + pending_request_meta_.reset(); + reactor.processResponse(response_size, response_data); + } + + [[nodiscard]] auto send(const std::uint32_t extended_can_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + // This lambda is allocated on the stack, so that the closures do not require heap allocation. + return detail::transmit( + [this, extended_can_id](const std::size_t frame_payload_size, + const std::uint8_t* const frame_payload) -> bool { + assert(frame_payload_size <= std::numeric_limits::max()); + return driver_.push(false, + extended_can_id, + static_cast(frame_payload_size), + frame_payload); + }, + (bus_mode_ == ICANDriver::Mode::Classic) ? 8U : 64U, + static_cast(transfer_id & detail::MaxTransferID), + payload_length, + payload); + } + + struct PendingRequestMetadata + { + std::uint8_t server_node_id{}; + PortID service_id{}; + std::uint8_t transfer_id{}; + }; + + ICANDriver& driver_; + const ICANDriver::Mode bus_mode_; + const std::uint8_t local_node_id_; + + SimplifiedServiceTransferReassembler<300> rx_file_read_response_; + SimplifiedServiceTransferReassembler<0> rx_get_info_request_; + SimplifiedServiceTransferReassembler<300> rx_execute_command_request_; + + std::optional pending_request_meta_; +}; + +class V1NodeIDAllocationActivity : public IActivity +{ + static constexpr std::uint64_t PseudoUIDMask = (1ULL << 48U) - 1U; + +public: + V1NodeIDAllocationActivity(IAllocator& allocator, + ICANDriver& driver, + const SystemInfo::UniqueID& local_uid, + const ICANDriver::Bitrate& bitrate, + const ICANDriver::Mode bus_mode) : + allocator_(allocator), + driver_(driver), + local_uid_(local_uid), + pseudo_uid_(makePseudoUniqueID(local_uid) & PseudoUIDMask), + bitrate_(bitrate), + bus_mode_(bus_mode) + {} + + auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* override + { + (void) reactor; + { + ICANDriver::PayloadBuffer buf{}; + while (const auto frame = driver_.pop(buf)) + { + const auto [can_id, payload_size] = *frame; + if (IActivity* const out = processReceivedFrame(can_id, payload_size, buf.data())) + { + return out; + } + } + } + if (uptime >= deadline_) + { + handleDeadline(); + deadline_ = uptime + getNextPeriod(); + } + return nullptr; + } + +private: + // The upper bound of the randomization interval cannot be less than 1 second, the maximum is not limited by Spec. + static constexpr std::chrono::microseconds MaxPeriod{3'000'000}; + + [[nodiscard]] auto processReceivedFrame(const std::uint32_t can_id, + const std::uint8_t payload_size, + const std::uint8_t* const payload) -> IActivity* + { + if (const auto frame = parseFrame(can_id, payload_size, payload)) + { + if (const auto* const m = std::get_if(&*frame); (m != nullptr) && m->source_node_id) + { + switch (m->subject_id) + { + case static_cast(SubjectID::PnPNodeIDAllocationData_v1): + { + if (const auto res = rx_v1_.update(*m)) + { + return acceptResponseV1(res->first, res->second); + } + break; + } + case static_cast(SubjectID::PnPNodeIDAllocationData_v2): + { + if (const auto res = rx_v2_.update(*m)) + { + return acceptResponseV2(res->first, res->second); + } + break; + } + default: + { + break; + } + } + } + } + return nullptr; + } + + [[nodiscard]] auto acceptResponseV1(const std::size_t body_size, const std::uint8_t* const body) -> IActivity* + { + if (body_size < 9) + { + return nullptr; + } + std::uint64_t received_pseudo_uid = 0; + for (auto i = 0U; i < 6; i++) + { + received_pseudo_uid |= std::uint64_t(body[i]) << (8U * i); + } + if (pseudo_uid_ != received_pseudo_uid) + { + return nullptr; // UID mismatch + } + if (const auto array_length = body[6]; array_length != 1) + { + return nullptr; // Unexpected array length + } + const auto allocated_node_id = static_cast(static_cast(body[8] << 8U) | body[7]); + if (allocated_node_id > MaxNodeID) + { + return nullptr; // Invalid for CAN + } + return constructSuccessor(static_cast(allocated_node_id)); + } + + [[nodiscard]] auto acceptResponseV2(const std::size_t body_size, const std::uint8_t* const body) -> IActivity* + { + if (body_size < 18) + { + return nullptr; + } + const auto allocated_node_id = static_cast(static_cast(body[1] << 8U) | body[0]); + if ((allocated_node_id <= MaxNodeID) && (0 == std::memcmp(&body[2], local_uid_.data(), local_uid_.size()))) + { + return constructSuccessor(static_cast(allocated_node_id)); + } + return nullptr; + } + + void handleDeadline() + { + if (bus_mode_ != ICANDriver::Mode::Classic) + { + if (version_toggle_) + { + publishRequestV1(); + tx_transfer_id_++; + } + else + { + publishRequestV2(); + } + version_toggle_ = !version_toggle_; + } + else + { + publishRequestV1(); + tx_transfer_id_++; + } + } + + void publishRequestV1() + { + const std::array buf{{ + static_cast(pseudo_uid_ >> 0U), + static_cast(pseudo_uid_ >> 8U), + static_cast(pseudo_uid_ >> 16U), + static_cast(pseudo_uid_ >> 24U), + static_cast(pseudo_uid_ >> 32U), + static_cast(pseudo_uid_ >> 40U), + 0, + static_cast(0b1110'0000U | tx_transfer_id_), + }}; + (void) send(true, SubjectID::PnPNodeIDAllocationData_v1, buf); + } + + void publishRequestV2() + { + assert(bus_mode_ != ICANDriver::Mode::Classic); + std::array buf{}; + buf.at(0) = std::numeric_limits::max(); + buf.at(1) = std::numeric_limits::max(); + std::copy(local_uid_.begin(), local_uid_.end(), &buf.at(2)); + buf.back() = static_cast(0b1110'0000U | tx_transfer_id_); + (void) send(false, SubjectID::PnPNodeIDAllocationData_v2, buf); + } + + template + [[nodoscard]] auto send(const bool force_classic_can, + const SubjectID sid, + const std::array& payload) -> bool + { + static constexpr std::uint32_t CANIDMask = 0b110'01'0110000000000000'0'0000000UL; + static_assert(PayloadSize <= 64); + CRC16CCITT discriminator_crc; + discriminator_crc.update(PayloadSize, payload.data()); + const std::uint32_t can_id = + CANIDMask | (static_cast(sid) << 8U) | (discriminator_crc.get() & 0x7FU); + return driver_.push(force_classic_can, can_id, PayloadSize, payload.data()); + } + + [[nodiscard]] auto constructSuccessor(const std::uint8_t allocated_node_id) -> IActivity* + { + if (const auto bus_mode = driver_.configure(bitrate_, false, makeAcceptanceFilter<1>(allocated_node_id))) + { + return allocator_.construct(driver_, *bus_mode, allocated_node_id); + } + return nullptr; + } + + static auto getNextPeriod() -> std::chrono::microseconds + { + const auto randomized = (std::rand() * MaxPeriod.count()) / RAND_MAX; // NOSONAR rand() ok + assert((0 <= randomized) && (randomized <= MaxPeriod.count())); + return std::max(std::chrono::microseconds(1), std::chrono::microseconds(randomized)); + } + + IAllocator& allocator_; + ICANDriver& driver_; + const SystemInfo::UniqueID local_uid_; + const std::uint64_t pseudo_uid_; + const ICANDriver::Bitrate bitrate_; + const ICANDriver::Mode bus_mode_; + + SimplifiedMessageTransferReassembler<9> rx_v1_; + SimplifiedMessageTransferReassembler<18> rx_v2_; + + std::chrono::microseconds deadline_{getNextPeriod()}; + std::uint8_t tx_transfer_id_ = 0; + bool version_toggle_ = false; +}; + +class ProtocolVersionDetectionActivity : public IActivity +{ +public: + ProtocolVersionDetectionActivity(IAllocator& allocator, + ICANDriver& driver, + const SystemInfo::UniqueID& local_uid, + const ICANDriver::Bitrate& bitrate) : + allocator_(allocator), driver_(driver), local_uid_(local_uid), bitrate_(bitrate) + {} + + auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* override + { + (void) reactor; + if (highest_version_seen_ && (uptime > deadline_)) + { + return constructSuccessor(*highest_version_seen_); + } + ICANDriver::PayloadBuffer buf{}; + while (const auto frame = driver_.pop(buf)) + { + const auto [can_id, payload_size] = *frame; + if (payload_size > 0) // UAVCAN frames are guaranteed to contain the tail byte always. + { + if (const auto uavcan_version = tryDetectProtocolVersionFromFrame(can_id, buf.at(payload_size - 1U))) + { + if (!highest_version_seen_) + { + deadline_ = uptime + ListeningPeriod; + } + if (!highest_version_seen_ || (*highest_version_seen_ < *uavcan_version)) + { + highest_version_seen_ = uavcan_version; + assert(highest_version_seen_); + } + } + } + } + return nullptr; + } + +private: + [[nodiscard]] static auto tryDetectProtocolVersionFromFrame(const std::uint32_t can_id, + const std::uint8_t tail_byte) + -> std::optional + { + // CAN ID is not validated at the moment. This may be improved in the future to avoid misdetection if there + // are other protocols besides UAVCAN on the same network. + (void) can_id; + if ((tail_byte & TailByteStartOfTransfer) != 0) + { + if ((tail_byte & TailByteToggleBit) != 0) + { + return 1; + } + return 0; + } + return {}; + } + + [[nodiscard]] auto constructSuccessor(const std::uint8_t detected_protocol_version) -> IActivity* + { + if (0 == detected_protocol_version) + { + if (driver_.configure(bitrate_, false, makeAcceptanceFilter<0>({}))) + { + return allocator_.construct(allocator_, driver_, local_uid_, bitrate_); + } + } + if (1 == detected_protocol_version) + { + if (const auto bus_mode = driver_.configure(bitrate_, false, makeAcceptanceFilter<1>({}))) + { + return allocator_.construct(allocator_, + driver_, + local_uid_, + bitrate_, + *bus_mode); + } + } + return nullptr; + } + + /// Heartbeats are exchanged at 1 Hz, so about one second should be enough to determine which versions are used. + static constexpr std::chrono::microseconds ListeningPeriod{1'100'000}; + + IAllocator& allocator_; + ICANDriver& driver_; + const SystemInfo::UniqueID local_uid_; + const ICANDriver::Bitrate bitrate_; + std::optional highest_version_seen_; + std::chrono::microseconds deadline_{}; +}; + +class BitrateDetectionActivity : public IActivity +{ +public: + BitrateDetectionActivity(IAllocator& allocator, ICANDriver& driver, const SystemInfo::UniqueID& local_uid) : + allocator_(allocator), driver_(driver), local_uid_(local_uid) + {} + +private: + auto poll(IReactor& reactor, const std::chrono::microseconds uptime) -> IActivity* override + { + (void) reactor; + ICANDriver::PayloadBuffer buf{}; + if (bus_mode_ && driver_.pop(buf)) + { + const auto bitrate = ICANDriver::StandardBitrates.at(setting_index_ % ICANDriver::StandardBitrates.size()); + return allocator_.construct(allocator_, driver_, local_uid_, bitrate); + } + if (!bus_mode_ || (uptime > next_try_at_)) + { + const auto br = ICANDriver::StandardBitrates.at((++setting_index_) % ICANDriver::StandardBitrates.size()); + bus_mode_ = driver_.configure(br, true, CANAcceptanceFilterConfig::makePromiscuous()); + next_try_at_ = uptime + ListeningPeriod; + } + return nullptr; + } + + /// Heartbeats are exchanged at 1 Hz, so about one second should be enough to determine if the bit rate is + /// correct. + static constexpr std::chrono::microseconds ListeningPeriod{1'100'000}; + + IAllocator& allocator_; + ICANDriver& driver_; + const SystemInfo::UniqueID local_uid_; + + std::optional bus_mode_; + std::size_t setting_index_ = ICANDriver::StandardBitrates.size() - 1U; + std::chrono::microseconds next_try_at_{}; +}; + +} // namespace detail + +/// Kocherga node implementing the UAVCAN/CAN v1 transport along with UAVCAN v0 with automatic version detection. +class CANNode : public kocherga::INode +{ +public: + /// The local UID shall be the same that is passed to the bootloader. It is used for PnP node-ID allocation. + /// By default, this implementation will auto-detect the parameters of the network and do a PnP node-ID allocation. + /// The application can opt-out of autoconfiguration by providing the required data to the constructor. + /// Unknown parameters shall be set to empty options. + CANNode(ICANDriver& driver, + const SystemInfo::UniqueID& local_unique_id, + const std::optional& can_bitrate = {}, + const std::optional uavcan_version = {}, + const std::optional local_node_id = {}) + { + if ((activity_ == nullptr) && can_bitrate && // + uavcan_version && (*uavcan_version == 0) && // + local_node_id && (*local_node_id > 0) && (*local_node_id <= MaxNodeID)) + { + if (const auto bus_mode = + driver.configure(*can_bitrate, + false, + detail::makeAcceptanceFilter<0>(static_cast(*local_node_id)))) + { + (void) bus_mode; // v0 doesn't care about mode because it only supports Classic CAN. + activity_ = + activity_allocator_.construct(driver, + static_cast(*local_node_id)); + } + } + if ((activity_ == nullptr) && can_bitrate && // + uavcan_version && (*uavcan_version == 1) && // + local_node_id && (*local_node_id <= MaxNodeID)) + { + if (const auto bus_mode = + driver.configure(*can_bitrate, + false, + detail::makeAcceptanceFilter<1>(static_cast(*local_node_id)))) + { + activity_ = + activity_allocator_.construct(driver, + *bus_mode, + static_cast(*local_node_id)); + } + } + if ((activity_ == nullptr) && can_bitrate && uavcan_version && (*uavcan_version == 0)) + { + if (const auto bus_mode = driver.configure(*can_bitrate, false, detail::makeAcceptanceFilter<0>({}))) + { + (void) bus_mode; // v0 doesn't care about mode because it only supports Classic CAN. + activity_ = activity_allocator_.construct(activity_allocator_, + driver, + local_unique_id, + *can_bitrate); + } + } + if ((activity_ == nullptr) && can_bitrate && uavcan_version && (*uavcan_version == 1)) + { + if (const auto bus_mode = driver.configure(*can_bitrate, false, detail::makeAcceptanceFilter<1>({}))) + { + activity_ = activity_allocator_.construct(activity_allocator_, + driver, + local_unique_id, + *can_bitrate, + *bus_mode); + } + } + if ((activity_ == nullptr) && can_bitrate) + { + if (const auto bus_mode = + driver.configure(*can_bitrate, true, CANAcceptanceFilterConfig::makePromiscuous())) + { + (void) bus_mode; // The protocol version detection task doesn't care about the bus mode. + activity_ = activity_allocator_.construct(activity_allocator_, + driver, + local_unique_id, + *can_bitrate); + } + } + if (activity_ == nullptr) + { + activity_ = activity_allocator_.construct(activity_allocator_, + driver, + local_unique_id); + } + assert(activity_ != nullptr); + } + +private: + void poll(IReactor& reactor, const std::chrono::microseconds uptime) override + { + assert(activity_ != nullptr); + assert(uptime.count() >= 0); + if (detail::IActivity* const new_activity = activity_->poll(reactor, uptime)) + { + activity_allocator_.destroy(activity_); + activity_ = new_activity; + } + } + + [[nodiscard]] auto sendRequest(const ServiceID service_id, + const NodeID server_node_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + assert(activity_ != nullptr); + return activity_->sendRequest(service_id, server_node_id, transfer_id, payload_length, payload); + } + + void cancelRequest() override { activity_->cancelRequest(); } + + [[nodiscard]] auto publishMessage(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + assert(activity_ != nullptr); + return activity_->publishMessage(subject_id, transfer_id, payload_length, payload); + } + + detail::BlockAllocator<1024, 2> activity_allocator_; + detail::IActivity* activity_ = nullptr; +}; + +} // namespace kocherga::can diff --git a/kocherga/kocherga_serial.hpp b/kocherga/kocherga_serial.hpp new file mode 100644 index 0000000..e6bad1c --- /dev/null +++ b/kocherga/kocherga_serial.hpp @@ -0,0 +1,686 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#pragma once + +#include "kocherga.hpp" +#include +#include +#include + +namespace kocherga::serial +{ +namespace detail +{ +using kocherga::detail::BitsPerByte; + +constexpr std::uint8_t FrameDelimiter = 0x00; ///< Zeros cannot occur inside frames thanks to COBS encoding. + +/// Reference values to check the header against. +static constexpr std::uint8_t FrameFormatVersion = 0; +static constexpr std::array FrameIndexEOTReference{0, 0, 0, 0x80}; + +/// Size-optimized implementation of CRC32-C (Castagnoli). +class CRC32C +{ +public: + static constexpr std::size_t Size = 4; + + void update(const std::uint8_t b) noexcept + { + value_ ^= static_cast(b); + for (auto i = 0U; i < BitsPerByte; i++) + { + value_ = ((value_ & 1U) != 0) ? ((value_ >> 1U) ^ ReflectedPoly) : (value_ >> 1U); // NOLINT + } + } + + [[nodiscard]] auto get() const noexcept { return value_ ^ Xor; } + + [[nodiscard]] auto getBytes() const noexcept -> std::array + { + const auto x = get(); + return { + static_cast(x >> (BitsPerByte * 0U)), + static_cast(x >> (BitsPerByte * 1U)), + static_cast(x >> (BitsPerByte * 2U)), + static_cast(x >> (BitsPerByte * 3U)), + }; + } + + [[nodiscard]] auto isResidueCorrect() const noexcept { return value_ == Residue; } + +private: + static constexpr std::uint32_t Xor = 0xFFFF'FFFFUL; + static constexpr std::uint32_t ReflectedPoly = 0x82F6'3B78UL; + static constexpr std::uint32_t Residue = 0xB798'B438UL; + + std::uint32_t value_ = Xor; +}; + +/// New instance shall be created per encoded frame. +/// ByteWriter is of type (std::uint8_t) -> bool, returns true on success. +/// This is an original implementation of the algorithm. +template +class COBSEncoder +{ +public: + explicit COBSEncoder(ByteWriter byte_writer) : byte_writer_(byte_writer) {} + + /// Invoke this function once per byte to transmit COBS-encoded bytestream. + /// The leading frame delimiter will be added automatically. + /// The instance shall be discarded immediately if this method returns false. + [[nodiscard]] auto push(const std::uint8_t b) -> bool + { + if (byte_count_ == 0) + { + if (!output(FrameDelimiter)) + { + return false; + } + } + byte_count_++; + if (b != FrameDelimiter) + { + lookahead_.push(b); + } + if ((b == FrameDelimiter) || (lookahead_.size() >= (std::numeric_limits::max() - 1))) + { + return flush(); + } + return true; + } + + /// This function shall be invoked at the end once. + /// The trailing frame delimiter will be added automatically. + /// The instance shall be discarded immediately if this method returns false. + [[nodiscard]] auto end() -> bool { return flush() && output(FrameDelimiter); } + +private: + [[nodiscard]] auto output(const std::uint8_t b) const -> bool { return byte_writer_(b); } + + [[nodiscard]] auto flush() -> bool + { + const auto sz = lookahead_.size(); + assert(sz < std::numeric_limits::max()); + if (!output(static_cast(sz + 1U))) + { + return false; + } + while (auto b = lookahead_.pop()) + { + if (!output(*b)) + { + return false; + } + } + assert(lookahead_.size() == 0); + return true; + } + + class LookaheadFIFO final // NOLINT + { + public: + /// Check size() before calling, otherwise the write pointer will wrap around to the beginning of the buffer. + [[nodiscard]] auto push(const std::uint8_t val) { buf_.at(in_++) = val; } + + /// Returns empty if empty, meaning that the current state is PUSHING. + [[nodiscard]] auto pop() -> std::optional + { + if (out_ < in_) + { + return buf_.at(out_++); + } + out_ = 0; + in_ = 0; + return {}; + } + + /// The number of bytes that currently reside in the queue. + [[nodiscard]] auto size() const noexcept { return in_; } + + private: + std::array buf_; + std::uint8_t in_ = 0; + std::uint8_t out_ = 0; + }; + + LookaheadFIFO lookahead_; + std::size_t byte_count_ = 0; + ByteWriter byte_writer_; +}; + +/// Constant-complexity COBS stream decoder extracts useful payload from a COBS-encoded stream of bytes in real time. +/// It does not perform any error checking; the outer logic is responsible for that (e.g., using CRC). +/// The implementation is derived from "Consistent Overhead Byte Stuffing" by Stuart Cheshire and Mary Baker, 1999. +class COBSDecoder +{ +public: + struct Delimiter ///< Indicates that the previous packet is finished and the next one is started. + {}; + struct Nothing ///< Indicates that there is no output byte to provide in this update cycle. + {}; + using Result = std::variant; + + [[nodiscard]] auto feed(const std::uint8_t bt) -> Result + { + if (bt == 0) + { + code_ = Top; + copy_ = 0; + return Delimiter{}; + } + if (copy_-- != 0) + { + return bt; + } + const auto old_code = code_; + assert(bt >= 1); + copy_ = bt - 1; + code_ = bt; + if (old_code != Top) + { + return std::uint8_t{0}; + } + return Nothing{}; + } + + /// Alias for feed(0). + void reset() { (void) feed(0); } + +private: + static constexpr std::uint8_t Top = std::numeric_limits::max(); + std::uint8_t code_ = Top; + std::uint8_t copy_ = 0; +}; + +struct Transfer +{ + struct Metadata + { + static constexpr std::uint8_t DefaultPriority = 6U; // Second to lowest. + static constexpr NodeID AnonymousNodeID = 0xFFFFU; + static constexpr PortID DataSpecServiceFlag = 0x8000U; + static constexpr PortID DataSpecResponseFlag = 0x4000U; + static constexpr auto DataSpecServiceIDMask = + static_cast(~static_cast(DataSpecServiceFlag | DataSpecResponseFlag)); + + std::uint8_t priority = DefaultPriority; + NodeID source = AnonymousNodeID; + NodeID destination = AnonymousNodeID; + std::uint16_t data_spec = std::numeric_limits::max(); + TransferID transfer_id = std::numeric_limits::max(); + + [[nodiscard]] auto isRequest() const noexcept -> std::optional + { + if (((data_spec & DataSpecServiceFlag) != 0) && ((data_spec & DataSpecResponseFlag) == 0)) + { + return data_spec & DataSpecServiceIDMask; + } + return {}; + } + + [[nodiscard]] auto isResponse() const noexcept -> std::optional + { + if (((data_spec & DataSpecServiceFlag) != 0) && ((data_spec & DataSpecResponseFlag) != 0)) + { + return data_spec & DataSpecServiceIDMask; + } + return {}; + } + }; + Metadata meta{}; + std::size_t payload_len = 0; + const std::uint8_t* payload = nullptr; +}; + +/// UAVCAN/serial stream parser. Extracts UAVCAN/serial frames from raw stream of bytes in constant time. +/// Frames that contain more than MaxPayloadSize bytes of payload are rejected as invalid. +template +class StreamParser +{ +public: + /// If the byte completed a transfer, it will be returned. + /// The returned object contains a pointer to the payload buffer memory allocated inside this instance. + /// The buffer is invalidated on the 32-nd call to update() after reception. + [[nodiscard]] auto update(const std::uint8_t stream_byte) -> std::optional + { + std::optional out; + const auto dec = decoder_.feed(stream_byte); + if (std::holds_alternative(dec)) + { + if (inside_ && (offset_ >= TotalOverheadSize) && crc_.isResidueCorrect() && isMetaValid()) + { + out = Transfer{ + meta_, + offset_ - TotalOverheadSize, + buf_.data(), + }; + } + reset(); + inside_ = true; + } + else if (const std::uint8_t* const decoded_byte = std::get_if(&dec)) + { + crc_.update(*decoded_byte); + if (offset_ < HeaderSize) + { + acceptHeader(*decoded_byte); + } + else + { + const auto buf_offset = offset_ - HeaderSize; + if (buf_offset < buf_.size()) + { + buf_.at(buf_offset) = *decoded_byte; + } + else + { + reset(); + } + } + ++offset_; + } + else + { + assert(std::holds_alternative(dec)); // Stuff byte, skip silently. + } + return out; + } + + /// Reset the decoder state machine, drop the current incomplete frame if any. + void reset() noexcept + { + decoder_.reset(); + offset_ = 0; + inside_ = false; + crc_ = {}; + meta_ = {}; + } + +private: + void acceptHeader(const std::uint8_t bt) + { + if ((OffsetVersion == offset_) && (bt != FrameFormatVersion)) + { + reset(); + } + if (OffsetPriority == offset_) + { + meta_.priority = bt; + } + acceptHeaderField(OffsetSource, meta_.source, bt); + acceptHeaderField(OffsetDestination, meta_.destination, bt); + acceptHeaderField(OffsetDataSpec, meta_.data_spec, bt); + acceptHeaderField(OffsetTransferID, meta_.transfer_id, bt); + if ((OffsetFrameIndexEOT.first <= offset_) && (offset_ <= OffsetFrameIndexEOT.second) && + (FrameIndexEOTReference.at(offset_ - OffsetFrameIndexEOT.first) != bt)) + { + reset(); + } + if (offset_ == (HeaderSize - 1U)) + { + if (!crc_.isResidueCorrect()) + { + reset(); // Header CRC error. + } + // At this point the header has been received and proven to be correct. Here, a generic implementation + // would normally query the subscription list to see if the frame is interesting or it should be dropped; + // also, the amount of dynamic memory that needs to be allocated for the payload would also be determined + // at this moment. The main purpose of the header CRC is to permit such early-stage frame processing. + // This specialized implementation requires none of that. + crc_ = {}; + } + } + + template + void acceptHeaderField(const std::pair range, Field& fld, const std::uint8_t bt) const + { + if ((range.first <= offset_) && (offset_ <= range.second)) + { + if (const auto byte_index = offset_ - range.first; byte_index > 0) + { + fld |= static_cast(static_cast(bt) << (BitsPerByte * byte_index)); + } + else + { + fld = static_cast(bt); + } + } + } + + [[nodiscard]] auto isMetaValid() const -> bool + { + if (meta_.isResponse() || meta_.isRequest()) + { + return (meta_.source != Transfer::Metadata::AnonymousNodeID) && + (meta_.destination != Transfer::Metadata::AnonymousNodeID); + } + return meta_.destination == Transfer::Metadata::AnonymousNodeID; + } + + static constexpr std::size_t HeaderSize = 32; + static constexpr std::size_t TotalOverheadSize = HeaderSize + CRC32C::Size; + // Header field offsets. + static constexpr std::size_t OffsetVersion = 0; + static constexpr std::size_t OffsetPriority = 1; + static constexpr std::pair OffsetSource{2, 3}; + static constexpr std::pair OffsetDestination{4, 5}; + static constexpr std::pair OffsetDataSpec{6, 7}; + static constexpr std::pair OffsetTransferID{16, 23}; + static constexpr std::pair OffsetFrameIndexEOT{24, 27}; + + COBSDecoder decoder_; + std::size_t offset_ = 0; + bool inside_ = false; + CRC32C crc_; + Transfer::Metadata meta_; + std::array buf_{}; +}; + +/// Sends a transfer with minimal buffering (some buffering is required by COBS) to save memory and reduce latency. +/// Callback is of type (std::uint8_t) -> bool whose semantics reflects ISerialPort::send(). +/// Callback shall not be an std::function<> to avoid heap allocation. +template +[[nodiscard]] inline auto transmit(const Callback& send_byte, const Transfer& tr) -> bool +{ + COBSEncoder encoder(send_byte); + CRC32C crc; + const auto out = [&crc, &encoder](const std::uint8_t b) -> bool { + crc.update(b); + return encoder.push(b); + }; + const auto out2 = [&out](const std::uint16_t bb) -> bool { + return out(static_cast(bb)) && out(static_cast(bb >> BitsPerByte)); + }; + bool ok = out(FrameFormatVersion) && out(tr.meta.priority) && // + out2(tr.meta.source) && out2(tr.meta.destination) && out2(tr.meta.data_spec); + for (auto i = 0U; i < sizeof(std::uint64_t); i++) + { + ok = ok && out(0); + } + auto tmp_transfer_id = tr.meta.transfer_id; + for (auto i = 0U; i < sizeof(std::uint64_t); i++) + { + ok = ok && out(static_cast(tmp_transfer_id)); + tmp_transfer_id >>= BitsPerByte; + } + for (const auto x : FrameIndexEOTReference) + { + ok = ok && out(x); + } + for (const auto x : crc.getBytes()) + { + ok = ok && out(x); + } + crc = {}; // Now it's the payload CRC. + { + const auto* ptr = tr.payload; + for (std::size_t i = 0U; i < tr.payload_len; i++) + { + ok = ok && out(*ptr); + ++ptr; + if (!ok) + { + break; + } + } + } + for (const auto x : crc.getBytes()) + { + ok = ok && out(x); + } + return ok && encoder.end(); +} + +} // namespace detail + +static constexpr NodeID MaxNodeID = 0xFFFEU; + +/// Bridges Kocherga/serial with the platform-specific serial port implementation. +/// Implement this and pass a reference to SerialNode. +class ISerialPort +{ +public: + /// Receive a single byte from the RX queue without blocking, if available. Otherwise, return an empty option. + [[nodiscard]] virtual auto receive() -> std::optional = 0; + + /// Send a single byte into the TX queue without blocking if there is free space available. + /// The queue shall be at least 1 KiB deep. + /// Return true if enqueued or sent successfully; return false if no space available. + [[nodiscard]] virtual auto send(const std::uint8_t b) -> bool = 0; + + virtual ~ISerialPort() = default; + ISerialPort() = default; + ISerialPort(const ISerialPort&) = delete; + ISerialPort(ISerialPort&&) = delete; + auto operator=(const ISerialPort&) -> ISerialPort& = delete; + auto operator=(ISerialPort&&) -> ISerialPort& = delete; +}; + +/// Kocherga node implementing the UAVCAN/serial transport. +class SerialNode : public kocherga::INode +{ +public: + /// The local UID shall be the same that is passed to the bootloader. It is used for PnP node-ID allocation. + SerialNode(ISerialPort& port, const SystemInfo::UniqueID& local_unique_id) : + unique_id_(local_unique_id), port_(port) + {} + + /// Set up the local node-ID manually instead of running PnP allocation. + /// If a manual update is triggered, this shall be done beforehand. + /// Do not assign the local node-ID more than once. Invalid values will be ignored. + void setLocalNodeID(const NodeID node_id) noexcept + { + if (node_id <= MaxNodeID) + { + local_node_id_ = node_id; + } + } + + /// Resets the state of the frame parser. Call it when the communication channel is reinitialized. + void reset() noexcept { stream_parser_.reset(); } + +private: + void poll(IReactor& reactor, const std::chrono::microseconds uptime) override + { + for (auto i = 0U; i < MaxBytesToProcessPerPoll; i++) + { + if (auto bt = port_.receive()) + { + if (const auto tr = stream_parser_.update(*bt)) + { + processReceivedTransfer(reactor, *tr, uptime); + } + } + else + { + break; + } + } + if ((!local_node_id_) && (uptime >= pnp_next_request_at_)) + { + using kocherga::detail::dsdl::PnPNodeIDAllocation; + constexpr std::int64_t interval_usec = + std::chrono::duration_cast(PnPNodeIDAllocation::MaxRequestInterval).count(); + const std::chrono::microseconds delay{(std::rand() * interval_usec) / RAND_MAX}; // NOSONAR rand() ok + pnp_next_request_at_ = uptime + delay; + std::array buf{}; + std::uint8_t* ptr = buf.data(); + *ptr++ = std::numeric_limits::max(); + *ptr++ = std::numeric_limits::max(); + (void) std::memcpy(ptr, unique_id_.data(), unique_id_.size()); + (void) publishMessageImpl(SubjectID::PnPNodeIDAllocationData_v2, pnp_transfer_id_, buf.size(), buf.data()); + ++pnp_transfer_id_; + } + } + + void processReceivedTransfer(IReactor& reactor, const detail::Transfer& tr, const std::chrono::microseconds uptime) + { + if (const auto resp_id = tr.meta.isResponse()) + { + if (pending_request_meta_ && local_node_id_) + { + // Observe that we don't need to perform deduplication explicitly because it is naturally addressed by + // the pending metadata struct: as soon as the response is received, it is invalidated immediately. + const bool match = (resp_id == pending_request_meta_->service_id) && + (tr.meta.source == pending_request_meta_->server_node_id) && + (tr.meta.destination == *local_node_id_) && + (tr.meta.transfer_id == pending_request_meta_->transfer_id); + if (match) + { + pending_request_meta_.reset(); // Reset first in case if the reactor initiates another request. + reactor.processResponse(tr.payload_len, tr.payload); + } + } + } + else if (const auto req_id = tr.meta.isRequest()) + { + if (local_node_id_ && (tr.meta.destination == *local_node_id_)) + { + // This implementation of deduplication is grossly oversimplified. It is considered to be acceptable + // for this specific library because double acceptance is not expected to cause adverse effects at the + // application layer. The difference compared to a fully conformant implementation is that it will + // accept duplicate request if there was another accepted request between the duplicates: + // - The normal case where duplicates removed is like: (A, A, A, B, B) --> (A, B) + // - The edge case where they are not removed is like: (A, A, B, B, A) --> (A, B, A) + // Full-fledged implementations are obviously immune to this because they keep separate state per + // session specifier, which does come with certain complexity (e.g., see libserard). + const auto [last_meta, last_ts] = last_received_request_meta_; + const bool duplicate = // + (last_ts + ::kocherga::detail::DefaultTransferIDTimeout >= uptime) && // + (last_meta.data_spec == tr.meta.data_spec) && // + (last_meta.source == tr.meta.source) && // + (last_meta.transfer_id == tr.meta.transfer_id); + if (!duplicate) + { + last_received_request_meta_ = {tr.meta, uptime}; + std::array buf{}; + if (const auto size = + reactor.processRequest(*req_id, tr.meta.source, tr.payload_len, tr.payload, buf.data())) + { + detail::Transfer::Metadata meta{}; + meta.priority = tr.meta.priority; + meta.source = *local_node_id_; + meta.destination = tr.meta.source; + meta.data_spec = static_cast(*req_id) | + static_cast(detail::Transfer::Metadata::DataSpecServiceFlag | + detail::Transfer::Metadata::DataSpecResponseFlag); + meta.transfer_id = tr.meta.transfer_id; + for (auto i = 0U; i < service_transfer_multiplication_factor_; i++) + { + (void) transmit({meta, *size, buf.data()}); + } + } + } + } + } + else + { + if ((!local_node_id_) && // If node-ID is not yet allocated, check if this is an allocation response. + (tr.meta.data_spec == static_cast(SubjectID::PnPNodeIDAllocationData_v2)) && + (tr.meta.source != detail::Transfer::Metadata::AnonymousNodeID) && + (tr.meta.destination == detail::Transfer::Metadata::AnonymousNodeID)) + { + const std::uint8_t* ptr = tr.payload; + NodeID node_id = *ptr; + ++ptr; + node_id |= static_cast(static_cast(*ptr) << detail::BitsPerByte); + ++ptr; + const bool uid_match = std::equal(std::begin(unique_id_), std::end(unique_id_), ptr); + if (uid_match) + { + local_node_id_ = node_id; + } + } + } + } + + [[nodiscard]] auto sendRequest(const ServiceID service_id, + const NodeID server_node_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + if (local_node_id_) + { + detail::Transfer::Metadata meta{}; + meta.source = *local_node_id_; + meta.destination = server_node_id; + meta.data_spec = static_cast(service_id) | detail::Transfer::Metadata::DataSpecServiceFlag; + meta.transfer_id = transfer_id; + bool transmit_ok = false; // Optimistic aggregation: one successful transmission is considered a success. + for (auto i = 0U; i < service_transfer_multiplication_factor_; i++) + { + transmit_ok = transmit({meta, payload_length, payload}) || transmit_ok; + } + if (transmit_ok) + { + pending_request_meta_ = PendingRequestMetadata{ + server_node_id, + static_cast(service_id), + transfer_id, + }; + return true; + } + } + return false; + } + + void cancelRequest() override { pending_request_meta_.reset(); } + + auto publishMessage(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + if (local_node_id_) + { + return publishMessageImpl(subject_id, transfer_id, payload_length, payload); + } + return false; + } + + auto publishMessageImpl(const SubjectID subject_id, + const TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool + { + detail::Transfer::Metadata meta{}; + meta.source = local_node_id_ ? *local_node_id_ : detail::Transfer::Metadata::AnonymousNodeID; + meta.data_spec = static_cast(subject_id); + meta.transfer_id = transfer_id; + return transmit({meta, payload_length, payload}); + } + + [[nodiscard]] auto transmit(const detail::Transfer& tr) -> bool + { + return detail::transmit([this](const std::uint8_t b) { return port_.send(b); }, tr); + } + + struct PendingRequestMetadata + { + NodeID server_node_id{}; + PortID service_id{}; + TransferID transfer_id{}; + }; + + static constexpr std::size_t MaxBytesToProcessPerPoll = 1024; + + const SystemInfo::UniqueID unique_id_; + + ISerialPort& port_; + detail::StreamParser stream_parser_; + std::optional local_node_id_; + std::optional pending_request_meta_; + std::pair last_received_request_meta_{}; + + std::chrono::microseconds pnp_next_request_at_{0}; + std::uint64_t pnp_transfer_id_ = 0; + + /// Controls deterministic data loss mitigation for outgoing service transfers. Messages are never duplicated. + const std::uint8_t service_transfer_multiplication_factor_ = 1; +}; + +} // namespace kocherga::serial diff --git a/kocherga_popcop.hpp b/kocherga_popcop.hpp deleted file mode 100644 index 116b83b..0000000 --- a/kocherga_popcop.hpp +++ /dev/null @@ -1,534 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2018 Zubax Robotics - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of - * this software and associated documentation files (the "Software"), to deal in - * the Software without restriction, including without limitation the rights to - * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - * the Software, and to permit persons to whom the Software is furnished to do so, - * subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - * Author: Pavel Kirienko - */ - -#pragma once - -#include - -// Third-party dependencies: -#include // Popcop protocol implementation in C++ - -#include - - -namespace kocherga_popcop -{ -/** - * Error codes specific to this protocol. - */ -static constexpr std::int16_t ErrTimeout = 4001; -static constexpr std::int16_t ErrCancelled = 4002; - -/** - * Platform abstraction interface for the Popcop protocol. - */ -class IPopcopPlatform -{ -public: - /** - * Serial port input/output methods should return if the IO operation could not be completed in this amount of time. - */ - static constexpr std::chrono::microseconds IOByteTimeout{1'000}; // NOLINT - - /** - * This constant is implicitly defined by Popcop. The protocol does not support CoA longer than this. - */ - static constexpr std::uint8_t CertificateOfAuthenticityMaxLength = 255; - - virtual ~IPopcopPlatform() = default; - - /** - * Sends one byte to the opposite endpoint. - * If timed out, do nothing. @ref IOByteTimeout - */ - virtual void emit(std::uint8_t byte) = 0; - - /** - * Receives one byte from the serial port input buffer. - * If timed out, returns an empty option. @ref IOByteTimeout - */ - virtual std::optional receive() = 0; - - /** - * This method is invoked when the endpoint encounters a frame that it doesn't know how to process. - * The application may opt to handle such frames itself. - * The default implementation does nothing. - */ - virtual void processUnhandledFrame(const popcop::transport::ParserOutput::Frame& frame) - { - (void) frame; - } - - /** - * This method is invoked when the local endpoint encounters unparsed data in the stream. - * The application may opt to handle it in some way or print it. - * The default implementation does nothing. - */ - virtual void processExtraneousData(const std::uint8_t* data, std::size_t length) - { - (void) data; - (void) length; - } - - /** - * This method is invoked when the local endpoint receives a device management command that it can't handle. - * Currently, only the following device management commands are handled by the endpoint, all others are delegated - * to the application via this method: - * - LaunchBootloader (does nothing) - */ - virtual popcop::standard::DeviceManagementCommandResponseMessage::Status processUnhandledDeviceManagementCommand( - const popcop::standard::DeviceManagementCommandRequestMessage& request) = 0; - - /** - * This method, if implemented, must atomically write the certificate of authenticity into some kind of ROM, - * and then read it back. The supplied data to write is pointed to by in_data, and the output data is pointed to - * by out_data. - * The return value is the length of out_data or a negative error code. - * The length of CoA can never exceed 255 bytes. - * If the target platform does not support CoA storage, leave this method unimplemented. - */ - virtual std::int16_t writeAndReadBackCertificateOfAuthenticity(const std::uint8_t* in_data, - std::uint8_t* out_data, - std::uint8_t length) - { - (void) in_data; - (void) out_data; - (void) length; - return 0; - } - - /** - * This method is invoked by the endpoint's thread periodically as long as it functions properly. - * The application can use it to reset a watchdog, but it is not mandatory. - * The minimal watchdog timeout is 3 seconds! Lower values may trigger spurious resets. - */ - virtual void resetWatchdog() = 0; - - /** - * This method is invoked by the endpoint periodically to check if it should terminate. - */ - virtual bool shouldExit() const = 0; -}; - -/** - * Popcop bootloader endpoint implementation. - * Either instantiate one instance per available port, or switch the same instance between available ports. - */ -class PopcopProtocol final : private kocherga::IProtocol -{ - static constexpr std::chrono::microseconds ImageDataTimeout{10'000'000}; // NOLINT - - ::kocherga::BootloaderController& blc_; - IPopcopPlatform& platform_; - const popcop::standard::EndpointInfoMessage endpoint_info_prototype_; - - popcop::transport::Parser<> parser_{}; - - kocherga::IDownloadSink* download_sink_ = nullptr; - std::int16_t upgrade_status_code_ = 0; - std::chrono::microseconds last_application_image_data_request_at_{}; - - - // Sends out one frame, ignores errors - template - void send(const M& message) - { - struct - { - IPopcopPlatform* pl_; - void operator()(std::uint8_t byte) - { - pl_->emit(byte); - } - } - sender - { - &platform_ - }; - (void) message.encode(popcop::transport::StreamEmitter(popcop::presentation::StandardFrameTypeCode, - sender).begin()); - } - - void processEndpointInfoRequest() - { - popcop::standard::EndpointInfoMessage m = endpoint_info_prototype_; - if (const auto ai = blc_.getAppInfo()) - { - auto& sw = m.software_version; - sw.major = ai->major_version; - sw.minor = ai->minor_version; - sw.vcs_commit_id = ai->vcs_commit; - sw.image_crc = ai->image_crc; - sw.release_build = ai->isReleaseBuild(); - sw.dirty_build = ai->isDirtyBuild(); - sw.build_timestamp_utc = ai->isBuildTimestampValid() ? ai->build_timestamp_utc : 0; - } - - send(m); - } - - void processDeviceManagementCommandRequest(const popcop::standard::DeviceManagementCommandRequestMessage& req) - { - popcop::standard::DeviceManagementCommandResponseMessage resp{}; - resp.command = req.command; - - if (req.command == popcop::standard::DeviceManagementCommand::LaunchBootloader) - { - // Do nothing - we're already in the bootloader! - resp.status = popcop::standard::DeviceManagementCommandResponseMessage::Status::Ok; - } - else - { - resp.status = platform_.processUnhandledDeviceManagementCommand(req); - } - - send(resp); - } - - void sendBootloaderStatusResponse() - { - popcop::standard::BootloaderStatusResponseMessage resp{}; - resp.timestamp = blc_.getMonotonicUptime(); - - switch (blc_.getState()) - { - case kocherga::State::NoAppToBoot: - { - assert(download_sink_ == nullptr); - resp.state = popcop::standard::BootloaderState::NoAppToBoot; - break; - } - case kocherga::State::BootDelay: - { - assert(download_sink_ == nullptr); - resp.state = popcop::standard::BootloaderState::BootDelay; - break; - } - case kocherga::State::BootCancelled: - { - assert(download_sink_ == nullptr); - resp.state = popcop::standard::BootloaderState::BootCancelled; - break; - } - case kocherga::State::AppUpgradeInProgress: - { - assert(download_sink_ != nullptr); - resp.state = popcop::standard::BootloaderState::AppUpgradeInProgress; - break; - } - case kocherga::State::ReadyToBoot: - { - assert(download_sink_ == nullptr); - resp.state = popcop::standard::BootloaderState::ReadyToBoot; - break; - } - default: - { - assert(false); - break; - } - } - - send(resp); - } - - void processBootloaderStatusRequest(const popcop::standard::BootloaderStatusRequestMessage& req) - { - switch (req.desired_state) - { - case popcop::standard::BootloaderState::BootCancelled: - { - blc_.cancelBoot(); - if (download_sink_ != nullptr) - { - upgrade_status_code_ = -ErrCancelled; - // The response will be sent later - } - else - { - sendBootloaderStatusResponse(); - } - break; - } - - case popcop::standard::BootloaderState::AppUpgradeInProgress: - { - if (download_sink_ == nullptr) - { - upgrade_status_code_ = 0; - last_application_image_data_request_at_ = blc_.getMonotonicUptime(); - - // This function blocks for a long time; it will send the response itself - (void) blc_.upgradeApp(*this); - - // And then we send another response at the end regardless - sendBootloaderStatusResponse(); - } - else - { - sendBootloaderStatusResponse(); // We're already upgrading, do nothing - } - break; - } - - case popcop::standard::BootloaderState::ReadyToBoot: - { - blc_.requestBoot(); - if (download_sink_ != nullptr) - { - upgrade_status_code_ = -ErrCancelled; - // The response will be sent later - } - else - { - sendBootloaderStatusResponse(); - } - break; - } - - case popcop::standard::BootloaderState::NoAppToBoot: - case popcop::standard::BootloaderState::BootDelay: - { - sendBootloaderStatusResponse(); // Send response anyway! - break; - } - - default: - { - assert(false); - break; - } - } - } - - void processBootloaderImageDataRequest(const popcop::standard::BootloaderImageDataRequestMessage& req) - { - popcop::standard::BootloaderImageDataResponseMessage resp{}; - resp.image_offset = req.image_offset; - resp.image_type = req.image_type; - - switch (req.image_type) - { - case popcop::standard::BootloaderImageType::Application: - { - last_application_image_data_request_at_ = blc_.getMonotonicUptime(); - - if (download_sink_ != nullptr) - { - // Observe that we ignore the offset here! The protocol requires that the offset must grow sequentially. - // If it doesn't, the downloaded image will be invalid; the bootloader controller will catch that later. - if (!req.image_data.empty()) - { - const auto result = download_sink_->handleNextDataChunk(req.image_data.data(), - std::uint16_t(req.image_data.size())); - if (result >= 0) - { - resp.image_data = req.image_data; - upgrade_status_code_ = 0; - } - else - { - upgrade_status_code_ = result; - } - } - - if (req.image_data.size() < req.image_data.max_size()) - { - // Last chunk received, terminate - download_sink_ = nullptr; - } - } - - break; - } - - case popcop::standard::BootloaderImageType::CertificateOfAuthenticity: - { - if ((req.image_offset == 0) && - (req.image_data.size() <= IPopcopPlatform::CertificateOfAuthenticityMaxLength)) - { - resp.image_data.resize(resp.image_data.max_size()); - const auto result = - platform_.writeAndReadBackCertificateOfAuthenticity(req.image_data.data(), - resp.image_data.data(), - std::uint8_t(req.image_data.size())); - if (result >= 0) - { - resp.image_data.resize(std::size_t(result)); - } - else - { - resp.image_data.clear(); - } - } - else - { - ; // Invalid request, return empty - } - break; - } - - default: - { - assert(false); - break; - } - } - - send(resp); - } - - void processFrame(const popcop::transport::ParserOutput::Frame& frame) - { - if (frame.type_code == popcop::presentation::StandardFrameTypeCode) - { - const auto& payload = frame.payload; - - if (popcop::standard::EndpointInfoMessage::tryDecode(payload.begin(), - payload.end())) - { - KOCHERGA_TRACE("Popcop: EP info req\n"); - processEndpointInfoRequest(); - } - else if (auto dmcr = popcop::standard::DeviceManagementCommandRequestMessage::tryDecode(payload.begin(), - payload.end())) - { - KOCHERGA_TRACE("Popcop: Device mgmt cmd\n"); - processDeviceManagementCommandRequest(*dmcr); - } - else if (auto bsr = popcop::standard::BootloaderStatusRequestMessage::tryDecode(payload.begin(), - payload.end())) - { - KOCHERGA_TRACE("Popcop: Bootloader status req\n"); - processBootloaderStatusRequest(*bsr); - } - else if (auto bidr = popcop::standard::BootloaderImageDataRequestMessage::tryDecode(payload.begin(), - payload.end())) - { - processBootloaderImageDataRequest(*bidr); - } - else - { - KOCHERGA_TRACE("Popcop: Unhandled std frame\n"); - platform_.processUnhandledFrame(frame); - } - } - else - { - KOCHERGA_TRACE("Popcop: Unhandled app frame type %u\n", frame.type_code); - platform_.processUnhandledFrame(frame); - } - } - - void processByte(std::uint8_t byte) - { - const auto out = parser_.processNextByte(byte); - if (auto frame = out.getReceivedFrame()) - { - platform_.resetWatchdog(); - processFrame(*frame); - platform_.resetWatchdog(); - } - else if (auto ed = out.getExtraneousData()) - { - platform_.resetWatchdog(); - platform_.processExtraneousData(ed->data(), ed->size()); - platform_.resetWatchdog(); - } - else - { - ; - } - } - - void loopOnce() - { - platform_.resetWatchdog(); - if (const auto res = platform_.receive()) - { - processByte(*res); - } - } - - std::int16_t downloadImage(kocherga::IDownloadSink& sink) final - { - assert(download_sink_ == nullptr); - assert(upgrade_status_code_ == 0); - - download_sink_ = &sink; - - sendBootloaderStatusResponse(); - - while (!platform_.shouldExit() && - (download_sink_ != nullptr) && - (upgrade_status_code_ >= 0)) - { - loopOnce(); - - if ((blc_.getMonotonicUptime() - last_application_image_data_request_at_) > ImageDataTimeout) - { - KOCHERGA_TRACE("Popcop: Timeout\n"); - upgrade_status_code_ = -ErrTimeout; - break; - } - } - - download_sink_ = nullptr; - return upgrade_status_code_; - } - - static popcop::standard::EndpointInfoMessage prepareEndpointInfoMessage( - popcop::standard::EndpointInfoMessage prototype) - { - prototype.software_version = popcop::standard::EndpointInfoMessage::SoftwareVersion(); - prototype.mode = popcop::standard::EndpointInfoMessage::Mode::Bootloader; - return prototype; - } - -public: - PopcopProtocol(::kocherga::BootloaderController& bootloader_controller, - IPopcopPlatform& popcop_platform, - const popcop::standard::EndpointInfoMessage& epi) : - blc_(bootloader_controller), - platform_(popcop_platform), - endpoint_info_prototype_(prepareEndpointInfoMessage(epi)) - { } - - /** - * Runs the endpoint thread. - * This function never returns unless IPopcopPlatform::shouldExit() returns true. - * If an RTOS is available, it is advisable to run this method from a separate thread. - * Otherwise, it is possible to perform other tasks by hijacking certain platform API functions. - */ - void run() - { - while (!platform_.shouldExit()) - { - loopOnce(); - } - } -}; - -} // namespace kocherga_popcop diff --git a/kocherga_uavcan.hpp b/kocherga_uavcan.hpp deleted file mode 100644 index e1aa0fb..0000000 --- a/kocherga_uavcan.hpp +++ /dev/null @@ -1,1284 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2018 Zubax Robotics - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of - * this software and associated documentation files (the "Software"), to deal in - * the Software without restriction, including without limitation the rights to - * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - * the Software, and to permit persons to whom the Software is furnished to do so, - * subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - * Author: Pavel Kirienko - */ - -#pragma once - -#include - -// Third-party dependencies: -#include // Lightweight UAVCAN protocol stack implementation -#include // Utility library for embedded systems -#include // Utility library for embedded systems - -#include - -/** - * This macro can be defined by the application to provide log output from the UAVCAN node. - * By default resolves to KOCHERGA_TRACE(). - * The expected signature is that of std::printf(). - */ -#ifndef KOCHERGA_UAVCAN_LOG -# define KOCHERGA_UAVCAN_LOG(...) KOCHERGA_TRACE(__VA_ARGS__) -#endif - - -namespace kocherga_uavcan -{ -/** - * Error codes specific to this protocol. - */ -static constexpr std::int16_t ErrTimeout = 3001; -static constexpr std::int16_t ErrInterrupted = 3002; -static constexpr std::int16_t ErrFileReadFailed = 3003; - -/** - * Abstractions needed to run the UAVCAN node. - */ -class IUAVCANPlatform -{ -public: - /** - * CAN controller operating mode. - * The driver must support silent mode, because it is required for automatic bit rate detection. - * The automatic transmission abort on error feature is required for dynamic node ID allocation - * (read the UAVCAN specification for more info). - */ - enum class CANMode - { - Normal, - Silent, - AutomaticTxAbortOnError - }; - - /** - * CAN acceptance filter configuration. - * Acceptance filters may be not supported in the underlying driver, this feature is optional. - * Bit flags used here are the same as in libcanard. - * The default constructor makes a filter that accepts all frames. - */ - struct CANAcceptanceFilterConfig - { - std::uint32_t id = 0; - std::uint32_t mask = 0; - }; - - virtual ~IUAVCANPlatform() = default; - - /** - * This method is invoked by the node's thread periodically as long as it functions properly. - * The application can use it to reset a watchdog, but it is not mandatory. - * The minimal watchdog timeout is 3 seconds! Lower values may trigger spurious resets. - */ - virtual void resetWatchdog() = 0; - - /** - * This method is invoked by the node's thread when it has nothing to do. - * It can be used by the application to perform other tasks, or to sleep the current thread - * if there is an OS available. - */ - virtual void sleep(std::chrono::microseconds duration) const = 0; - - /** - * Returns a pseudo-random unsigned integer within the specified range [lower_bound, upper_bound). - * Possible implementation: - * const auto rnd = std::uint64_t(std::rand()) * std::uint64_t(std::rand()); - * return lower_bound + rnd % (upper_bound - lower_bound); - */ - virtual std::uint64_t getRandomUnsignedInteger(std::uint64_t lower_bound, - std::uint64_t upper_bound) const = 0; - - /** - * Initializes the CAN hardware in the specified mode. - * Observe that this implementation needs only one acceptance filter. - * @retval 0 Success - * @retval negative Error - */ - virtual std::int16_t configure(std::uint32_t bitrate, - CANMode mode, - const CANAcceptanceFilterConfig& acceptance_filter) = 0; - - /** - * Transmits one CAN frame. - * - * @retval 1 Transmitted successfully - * @retval 0 Timed out - * @retval negative Error - */ - virtual std::int16_t send(const ::CanardCANFrame& frame, std::chrono::microseconds timeout) = 0; - - /** - * Reads one CAN frame from the RX queue. - * Return integer values: - * @retval 1 Read successfully; the second value contains a valid CAN frame object. - * @retval 0 Timed out - * @retval negative Error - */ - virtual std::pair receive(std::chrono::microseconds timeout) = 0; - - /** - * This method is invoked by the node periodically to check if it should terminate. - */ - virtual bool shouldExit() const = 0; - - /** - * Invoked by the node when it is requested to reboot by a remote node. - * Returns true on success, false if reboot cannot be performed. - */ - virtual bool tryScheduleReboot() = 0; -}; - - -using NodeName = senoval::String<80>; - -struct HardwareInfo -{ - std::uint8_t major = 0; ///< Required field - std::uint8_t minor = 0; ///< Required field - - typedef std::array UniqueID; - UniqueID unique_id{}; ///< Required field - - typedef senoval::Vector CertificateOfAuthenticity; - CertificateOfAuthenticity certificate_of_authenticity; ///< Optional, empty if not defined -}; - -/** - * Implementation details, please do not touch this. - */ -namespace impl_ -{ -/** - * This is the default defined by the UAVCAN specification. - */ -static constexpr std::chrono::microseconds DefaultServiceRequestTimeout{1'000'000}; // NOLINT - -/** - * A sensible default value that should suit most applications. - */ -static constexpr std::chrono::microseconds DefaultProgressReportInterval{10'000'000}; // NOLINT - - -namespace dsdl -{ - -static inline constexpr std::uint16_t bitlen2bytelen(std::uint32_t x) noexcept -{ - return std::uint16_t((x + 7) / 8); -} - -template -struct MessageTypeInfo -{ - static constexpr std::uint32_t DataTypeID = DataTypeID_; - static constexpr std::uint64_t DataTypeSignature = DataTypeSignature_; - - static constexpr std::uint16_t MaxSizeBytes = bitlen2bytelen(MaxEncodedBitLength_); -}; - -template -struct ServiceTypeInfo -{ - static constexpr std::uint32_t DataTypeID = DataTypeID_; - static constexpr std::uint64_t DataTypeSignature = DataTypeSignature_; - - static constexpr std::uint16_t MaxSizeBytesRequest = bitlen2bytelen(MaxEncodedBitLengthRequest_); - - static constexpr std::uint16_t MaxSizeBytesResponse = bitlen2bytelen(MaxEncodedBitLengthResponse_); -}; - -// The values have been obtained with the help of the script show_data_type_info.py from libcanard. -using NodeStatus = MessageTypeInfo< 341U, 0x0f0868d0c1a7c6f1ULL, 56U>; -using NodeIDAllocation = MessageTypeInfo< 1U, 0x0b2a812620a11d40ULL, 141U>; -using LogMessage = MessageTypeInfo<16383U, 0xd654a48e0c049d75ULL, 983U>; - -using GetNodeInfo = ServiceTypeInfo< 1U, 0xee468a8121c46a9eULL, 0U, 3015U>; -using BeginFirmwareUpdate = ServiceTypeInfo< 40U, 0xb7d725df72724126ULL, 1616U, 1031U>; -using FileRead = ServiceTypeInfo< 48U, 0x8dcdca939f33f678ULL, 1648U, 2073U>; -using RestartNode = ServiceTypeInfo< 5U, 0x569e05394a3017f0ULL, 40U, 1U>; - - -enum class NodeHealth : std::uint8_t -{ - Ok = 0, - Warning = 1, - Error = 2 -}; - -enum class NodeMode : std::uint8_t -{ - Initialization = 1, - SoftwareUpdate = 3, -}; - -} - -/** - * See uavcan.protocol.debug.LogMessage - */ -enum class LogLevel : std::uint8_t -{ - Info = 1, - Warning = 2, - Error = 3, -}; - -} // namespace impl_ - -/** - * A UAVCAN node that is useful solely for the purpose of firmware update. - * - * This class looks like a bowl of spaghetti because is has been carefully optimized for ROM footprint. - * Avoid reading this code unless you've familiarized yourself with the UAVCAN specification. - * - * The API is thread-safe. - */ -template -class BootloaderNode final : private ::kocherga::IProtocol -{ - ::kocherga::BootloaderController& bootloader_; - IUAVCANPlatform& platform_; - - const NodeName node_name_; - const HardwareInfo hw_info_; - - std::chrono::microseconds next_1hz_task_invocation_at_{}; - bool init_done_ = false; - - alignas(std::max_align_t) std::array memory_pool_{}; - ::CanardInstance canard_{}; - - std::uint32_t can_bus_bit_rate_ = 0; - std::uint8_t confirmed_local_node_id_ = 0; ///< This field is needed in order to avoid mutexes - - std::uint8_t remote_server_node_id_ = 0; - senoval::String<200> firmware_file_path_; - - std::chrono::microseconds send_next_node_id_allocation_request_at_{}; - std::uint8_t node_id_allocation_unique_id_offset_ = 0; - - std::uint16_t vendor_specific_status_ = 0; - - std::uint8_t node_status_transfer_id_ = 0; - std::uint8_t node_id_allocation_transfer_id_ = 0; - std::uint8_t log_message_transfer_id_ = 0; - std::uint8_t file_read_transfer_id_ = 0; - - std::array read_buffer_{}; - std::int16_t read_result_ = 0; - - - std::uint64_t getMonotonicUptimeInMicroseconds() const - { - const auto ut = bootloader_.getMonotonicUptime(); - return std::uint64_t(std::chrono::duration_cast(ut).count()); - } - - void delayAfterDriverError() - { - platform_.resetWatchdog(); - platform_.sleep(std::chrono::microseconds(1'000'000)); - platform_.resetWatchdog(); - } - - std::chrono::microseconds getRandomDuration(std::chrono::microseconds lower_bound, - std::chrono::microseconds upper_bound) const - { - assert(lower_bound <= upper_bound); - return std::chrono::microseconds(platform_.getRandomUnsignedInteger(std::uint64_t(lower_bound.count()), - std::uint64_t(upper_bound.count()))); - } - - void makeNodeStatusMessage(std::uint8_t* buffer) const - { - std::memset(buffer, 0, impl_::dsdl::NodeStatus::MaxSizeBytes); - - const auto uptime_sec = - std::uint32_t(std::chrono::duration_cast(bootloader_.getMonotonicUptime()).count()); - - /* - * Bootloader State Node Mode Node Health - * ---------------------------------------------------- - * NoAppToBoot SoftwareUpdate Error - * BootDelay Initialization Ok - * BootCancelled SoftwareUpdate Warning - * AppUpgradeInProgress SoftwareUpdate Ok - * ReadyToBoot Initialization Ok - */ - std::uint8_t node_health{}; - std::uint8_t node_mode{}; - switch (bootloader_.getState()) - { - case ::kocherga::State::NoAppToBoot: - { - node_health = std::uint8_t(impl_::dsdl::NodeHealth::Error); - node_mode = std::uint8_t(impl_::dsdl::NodeMode::SoftwareUpdate); - break; - } - case ::kocherga::State::AppUpgradeInProgress: - { - node_health = std::uint8_t(impl_::dsdl::NodeHealth::Ok); - node_mode = std::uint8_t(impl_::dsdl::NodeMode::SoftwareUpdate); - break; - } - case::kocherga:: State::BootCancelled: - { - node_health = std::uint8_t(impl_::dsdl::NodeHealth::Warning); - node_mode = std::uint8_t(impl_::dsdl::NodeMode::SoftwareUpdate); - break; - } - case ::kocherga::State::BootDelay: - case ::kocherga::State::ReadyToBoot: - { - node_health = std::uint8_t(impl_::dsdl::NodeHealth::Ok); - node_mode = std::uint8_t(impl_::dsdl::NodeMode::Initialization); - break; - } - } - - ::canardEncodeScalar(buffer, 0, 32, &uptime_sec); - ::canardEncodeScalar(buffer, 32, 2, &node_health); - ::canardEncodeScalar(buffer, 34, 3, &node_mode); - ::canardEncodeScalar(buffer, 40, 16, &vendor_specific_status_); - } - - void sendNodeStatus() - { - using namespace impl_; - std::uint8_t buffer[dsdl::NodeStatus::MaxSizeBytes]{}; - makeNodeStatusMessage(buffer); - const auto res = ::canardBroadcast(&canard_, - dsdl::NodeStatus::DataTypeSignature, - dsdl::NodeStatus::DataTypeID, - &node_status_transfer_id_, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - dsdl::NodeStatus::MaxSizeBytes); - if (res <= 0) - { - KOCHERGA_UAVCAN_LOG("NodeStatus bc err %d\n", res); - } - } - - void sendLog(const impl_::LogLevel level, const senoval::String<90>& txt) - { - static const senoval::String<31> SourceName("Bootloader"); - std::uint8_t buffer[1 + 31 + 90]{}; - buffer[0] = std::uint8_t(std::uint8_t(std::uint16_t(level) << 5U) | SourceName.length()); - std::copy(SourceName.begin(), SourceName.end(), &buffer[1]); - std::copy(txt.begin(), txt.end(), &buffer[1 + SourceName.length()]); - - using impl_::dsdl::LogMessage; - const auto res = ::canardBroadcast(&canard_, - LogMessage::DataTypeSignature, - LogMessage::DataTypeID, - &log_message_transfer_id_, - CANARD_TRANSFER_PRIORITY_LOWEST, - buffer, - std::uint16_t(1U + SourceName.length() + txt.length())); - if (res < 0) - { - KOCHERGA_UAVCAN_LOG("Log err %d\n", res); - } - } - - auto initCAN(const std::uint32_t bitrate, - const IUAVCANPlatform::CANMode mode, - const IUAVCANPlatform::CANAcceptanceFilterConfig& acceptance_filter = - IUAVCANPlatform::CANAcceptanceFilterConfig()) - { - const auto res = platform_.configure(bitrate, mode, acceptance_filter); - if (res < 0) - { - KOCHERGA_UAVCAN_LOG("CAN init err @%u bps: %d\n", unsigned(bitrate), res); - } - return res; - } - - auto receive(std::chrono::microseconds timeout) - { - const auto res = platform_.receive(timeout); - if (res.first < 0) - { - KOCHERGA_UAVCAN_LOG("RX err %d\n", res.first); - } - return res; - } - - auto send(const ::CanardCANFrame& frame, std::chrono::microseconds timeout) - { - const auto res = platform_.send(frame, timeout); - if (res < 0) - { - KOCHERGA_UAVCAN_LOG("TX err %d\n", res); - } - return res; - } - - void handle1HzTasks() - { - platform_.resetWatchdog(); - ::canardCleanupStaleTransfers(&canard_, getMonotonicUptimeInMicroseconds()); - - // NodeStatus broadcasting - if (init_done_ && (::canardGetLocalNodeID(&canard_) > 0)) - { - sendNodeStatus(); - } - - platform_.resetWatchdog(); - } - - void poll() - { - constexpr std::uint8_t MaxFramesPerSpin = 10; - - // Receive - for (std::uint8_t i = 0; i < MaxFramesPerSpin; i++) - { - platform_.resetWatchdog(); - - const auto res = receive(std::chrono::microseconds(1'000)); // Blocking call - if (res.first < 1) - { - break; // Error or no frames - } - - ::canardHandleRxFrame(&canard_, &res.second, getMonotonicUptimeInMicroseconds()); - } - - // Transmit - for (std::uint8_t i = 0; i < MaxFramesPerSpin; i++) - { - platform_.resetWatchdog(); - - const ::CanardCANFrame* txf = ::canardPeekTxQueue(&canard_); - if (txf == nullptr) - { - break; // Nothing to transmit - } - - const auto res = send(*txf, std::chrono::microseconds{}); // Non-blocking call - if (res == 0) - { - break; // Queue is full - } - - ::canardPopTxQueue(&canard_); // Transmitted successfully or error, either way remove the frame - } - - // 1Hz process - if (bootloader_.getMonotonicUptime() >= next_1hz_task_invocation_at_) - { - next_1hz_task_invocation_at_ += std::chrono::seconds(1); - handle1HzTasks(); - } - } - - void performCANBitRateDetection() - { - /// These are defined by the specification; 100 Kbps is added due to its popularity. - static constexpr std::array StandardBitRates - {{ - 1000000, ///< Standard, recommended by UAVCAN - 500000, ///< Standard - 250000, ///< Standard - 125000, ///< Standard - 100000 ///< Popular bit rate that is not defined by the specification - }}; - - std::uint8_t current_bit_rate_index = 0; - - // Loop forever until the bit rate is detected - while ((!platform_.shouldExit()) && (can_bus_bit_rate_ == 0)) - { - platform_.resetWatchdog(); - - const std::uint32_t br = StandardBitRates[current_bit_rate_index]; - current_bit_rate_index = std::uint8_t((current_bit_rate_index + 1U) % StandardBitRates.size()); - - if (initCAN(br, IUAVCANPlatform::CANMode::Silent) >= 0) - { - const auto res = receive(std::chrono::microseconds(1'100'000)).first; - if (res > 0) - { - can_bus_bit_rate_ = br; - } - if (res < 0) - { - delayAfterDriverError(); - } - } - else - { - delayAfterDriverError(); - } - } - - platform_.resetWatchdog(); - } - - void performDynamicNodeIDAllocation() - { - // CAN bus initialization - while (true) - { - // Accept only messages with DTID = 1 (Allocation) - // Observe that we need both responses from allocators and requests from other nodes! - IUAVCANPlatform::CANAcceptanceFilterConfig filt; - filt.id = 0b00000000000000000000100000000UL | CANARD_CAN_FRAME_EFF; - filt.mask = 0b00000000000000000001110000000UL | CANARD_CAN_FRAME_EFF | CANARD_CAN_FRAME_RTR | - CANARD_CAN_FRAME_ERR; - - if (initCAN(can_bus_bit_rate_, IUAVCANPlatform::CANMode::AutomaticTxAbortOnError, filt) >= 0) - { - break; - } - - delayAfterDriverError(); - } - - using namespace impl_; - - while ((!platform_.shouldExit()) && (::canardGetLocalNodeID(&canard_) == 0)) - { - platform_.resetWatchdog(); - - send_next_node_id_allocation_request_at_ = - bootloader_.getMonotonicUptime() + getRandomDuration(std::chrono::microseconds(600'000), - std::chrono::microseconds(1'000'000)); - - while ((bootloader_.getMonotonicUptime() < send_next_node_id_allocation_request_at_) && - (::canardGetLocalNodeID(&canard_) == 0)) - { - poll(); - } - - if (::canardGetLocalNodeID(&canard_) != 0) - { - break; - } - - // Structure of the request is documented in the DSDL definition - // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation - std::uint8_t allocation_request[7]{}; - - if (node_id_allocation_unique_id_offset_ == 0) - { - allocation_request[0] |= 1; // First part of unique ID - } - - static constexpr std::uint8_t MaxLenOfUniqueIDInRequest = 6; - std::uint8_t uid_size = std::uint8_t(hw_info_.unique_id.size() - node_id_allocation_unique_id_offset_); - if (uid_size > MaxLenOfUniqueIDInRequest) - { - uid_size = MaxLenOfUniqueIDInRequest; - } - - // Paranoia time - assert(node_id_allocation_unique_id_offset_ < hw_info_.unique_id.size()); - assert(uid_size <= MaxLenOfUniqueIDInRequest); - assert(uid_size > 0); - assert(std::uint16_t(uid_size + node_id_allocation_unique_id_offset_) <= hw_info_.unique_id.size()); - - std::memmove(&allocation_request[1], &hw_info_.unique_id[node_id_allocation_unique_id_offset_], uid_size); - - // Broadcasting the request - const auto bcast_res = ::canardBroadcast(&canard_, - dsdl::NodeIDAllocation::DataTypeSignature, - dsdl::NodeIDAllocation::DataTypeID, - &node_id_allocation_transfer_id_, - CANARD_TRANSFER_PRIORITY_LOW, - &allocation_request[0], - std::uint16_t(uid_size + 1U)); - if (bcast_res < 0) - { - KOCHERGA_UAVCAN_LOG("NID alloc bc err %d\n", bcast_res); - } - - // Preparing for timeout; if response is received, this value will be updated from the callback. - node_id_allocation_unique_id_offset_ = 0; - } - - platform_.resetWatchdog(); - } - - void runNodeThread() - { - /* - * CAN bit rate - */ - platform_.resetWatchdog(); - - if (can_bus_bit_rate_ == 0) - { - performCANBitRateDetection(); - } - - if (platform_.shouldExit()) - { - return; - } - - /* - * Node ID - */ - platform_.resetWatchdog(); - - if (::canardGetLocalNodeID(&canard_) == 0) - { - performDynamicNodeIDAllocation(); - } - - if (platform_.shouldExit()) - { - return; - } - - confirmed_local_node_id_ = ::canardGetLocalNodeID(&canard_); - - // This is the only info message we output during initialization. - // Fewer messages reduce the chances of breaking UART CLI data flow. - KOCHERGA_UAVCAN_LOG("CAN %u bps, NID %u\n", unsigned(can_bus_bit_rate_), confirmed_local_node_id_); - - using namespace impl_; - - /* - * Init CAN in proper mode now - */ - platform_.resetWatchdog(); - - while (true) - { - // Accept only correctly addressed service requests and responses - // We don't need message transfers anymore - IUAVCANPlatform::CANAcceptanceFilterConfig filt; - filt.id = 0b00000000000000000000010000000UL | - std::uint32_t(confirmed_local_node_id_ << 8U) | CANARD_CAN_FRAME_EFF; - filt.mask = 0b00000000000000111111110000000UL | - CANARD_CAN_FRAME_EFF | CANARD_CAN_FRAME_RTR | CANARD_CAN_FRAME_ERR; - - if (initCAN(can_bus_bit_rate_, IUAVCANPlatform::CANMode::Normal, filt) >= 0) - { - break; - } - - delayAfterDriverError(); - } - - init_done_ = true; - - /* - * Update loop; run forever because there's nothing else to do - */ - while (!platform_.shouldExit()) - { - assert((confirmed_local_node_id_ > 0) && (::canardGetLocalNodeID(&canard_) > 0)); - - platform_.resetWatchdog(); - - /* - * Waiting for the firmware update request - */ - if (remote_server_node_id_ == 0) - { - while ((!platform_.shouldExit()) && (remote_server_node_id_ == 0)) - { - platform_.resetWatchdog(); - poll(); - } - } - - if (platform_.shouldExit()) - { - break; - } - - KOCHERGA_UAVCAN_LOG("FW server NID %u path %s\n", - unsigned(remote_server_node_id_), firmware_file_path_.c_str()); - - /* - * Rewriting the old firmware with the new file - */ - platform_.resetWatchdog(); - const auto result = bootloader_.upgradeApp(*this); - platform_.resetWatchdog(); - - sendNodeStatus(); // Announcing the new status of the bootloader ASAP - - if (result >= 0) - { - vendor_specific_status_ = 0; - if (bootloader_.getState() == kocherga::State::NoAppToBoot) - { - sendLog(LogLevel::Error, "Downloaded image is invalid"); - } - else - { - sendLog(LogLevel::Info, "OK"); - } - } - else - { - vendor_specific_status_ = std::uint16_t(std::abs(result)); - sendLog(LogLevel::Error, - senoval::String<90>("Upgrade error ") + senoval::convertIntToString(result)); - } - - /* - * Reset everything to zero and loop again, because there's nothing else to do. - * The outer logic will request reboot if necessary. - */ - remote_server_node_id_ = 0; - firmware_file_path_.clear(); - } - - KOCHERGA_UAVCAN_LOG("Exit\n"); - platform_.resetWatchdog(); - } - - std::int16_t downloadImage(kocherga::IDownloadSink& sink) override - { - using namespace impl_; - - std::uint64_t offset = 0; - auto next_progress_report_deadline = bootloader_.getMonotonicUptime(); - - sendNodeStatus(); // Announcing the new state of the bootloader ASAP - - while (true) - { - platform_.resetWatchdog(); - - if (platform_.shouldExit()) - { - return -ErrInterrupted; - } - - /* - * Send request - */ - { - std::uint8_t buffer[dsdl::FileRead::MaxSizeBytesRequest]{}; - ::canardEncodeScalar(buffer, 0, 40, &offset); - std::copy(firmware_file_path_.begin(), firmware_file_path_.end(), &buffer[5]); - - const auto res = ::canardRequestOrRespond(&canard_, - remote_server_node_id_, - dsdl::FileRead::DataTypeSignature, - dsdl::FileRead::DataTypeID, - &file_read_transfer_id_, - CANARD_TRANSFER_PRIORITY_LOW, - ::CanardRequest, - buffer, - std::uint16_t(firmware_file_path_.size() + 5U)); - if (res < 0) - { - KOCHERGA_UAVCAN_LOG("File req err %d\n", res); - return std::int16_t(res); - } - } - - /* - * Await response. - * Note that the watchdog is not reset in the loop, since its timeout is large enough to wait for response. - */ - const std::chrono::microseconds response_deadline = - bootloader_.getMonotonicUptime() + DefaultServiceRequestTimeout; - - constexpr auto InvalidReadResult = std::numeric_limits::max(); - read_result_ = InvalidReadResult; - - while (read_result_ == InvalidReadResult) - { - poll(); - - if (bootloader_.getMonotonicUptime() > response_deadline) - { - return -ErrTimeout; - } - } - - platform_.resetWatchdog(); - - if (read_result_ < 0) - { - return read_result_; - } - - /* - * Process the response. - * Observe that we don't constrain the maximum image size - either the bootloader - * or the storage backend will return error if we exceed it. - */ - if (read_result_ > 0) - { - offset = offset + std::uint64_t(read_result_); - - const auto res = sink.handleNextDataChunk(read_buffer_.data(), std::uint16_t(read_result_)); - if (res < 0) - { - platform_.resetWatchdog(); - return res; - } - } - else - { - platform_.resetWatchdog(); - return 0; // Done - } - - /* - * Send a progress report if time is up - */ - if (bootloader_.getMonotonicUptime() > next_progress_report_deadline) - { - next_progress_report_deadline += DefaultProgressReportInterval; - sendLog(LogLevel::Info, senoval::convertIntToString(offset) + senoval::String<90>("B down...")); - } - - /* - * Wait in order to avoid bus congestion - * The magic shift ensures that the relative bus utilization does not depend on the bit rate. - */ - platform_.resetWatchdog(); - - const std::chrono::microseconds wait_deadline = - bootloader_.getMonotonicUptime() + - std::chrono::microseconds(1'000'000UL / (1UL + (can_bus_bit_rate_ >> 16U))); - - while (bootloader_.getMonotonicUptime() < wait_deadline) - { - poll(); - } - } - - assert(false); // Should never get here - return -1; - } - - void onTransferReception(::CanardRxTransfer* const transfer) - { - using namespace impl_; - - /* - * Dynamic node ID allocation protocol. - * Taking this branch only if we don't have a node ID, ignoring otherwise. - * This piece of dark magic has been carefully transplanted from the libcanard demo application. - */ - if ((::canardGetLocalNodeID(&canard_) == CANARD_BROADCAST_NODE_ID) && - (transfer->transfer_type == ::CanardTransferTypeBroadcast) && - (transfer->data_type_id == dsdl::NodeIDAllocation::DataTypeID)) - { - // Rule C - updating the randomized time interval - send_next_node_id_allocation_request_at_ = - bootloader_.getMonotonicUptime() + getRandomDuration(std::chrono::microseconds(600'000), - std::chrono::microseconds(1'000'000)); - - if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) - { - node_id_allocation_unique_id_offset_ = 0; - return; - } - - // Copying the unique ID from the message - static constexpr std::uint8_t UniqueIDBitOffset = 8; - std::uint8_t received_unique_id[std::tuple_size_v]; - std::uint8_t received_unique_id_len = 0; - for (; - received_unique_id_len < (transfer->payload_len - (UniqueIDBitOffset / 8U)); - received_unique_id_len++) - { - assert(received_unique_id_len < hw_info_.unique_id.size()); - const auto bit_offset = std::uint8_t(UniqueIDBitOffset + received_unique_id_len * 8U); - (void) ::canardDecodeScalar(transfer, - bit_offset, - 8, - false, - &received_unique_id[received_unique_id_len]); - } - - // Matching the received UID against the local one - if (std::memcmp(received_unique_id, hw_info_.unique_id.data(), received_unique_id_len) != 0) - { - node_id_allocation_unique_id_offset_ = 0; - return; // No match, return - } - - if (received_unique_id_len < hw_info_.unique_id.size()) - { - // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. - node_id_allocation_unique_id_offset_ = received_unique_id_len; - send_next_node_id_allocation_request_at_ = - bootloader_.getMonotonicUptime() + getRandomDuration(std::chrono::microseconds(0), - std::chrono::microseconds(400'000)); - } - else - { - // Allocation complete - copying the allocated node ID from the message - std::uint8_t allocated_node_id = 0; - (void) ::canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); - assert(allocated_node_id <= 127); - - ::canardSetLocalNodeID(&canard_, allocated_node_id); - } - } - - /* - * GetNodeInfo request. - * Someday this mess should be replaced with auto-generated message serialization code, like in libuavcan. - */ - if ((transfer->transfer_type == ::CanardTransferTypeRequest) && - (transfer->data_type_id == dsdl::GetNodeInfo::DataTypeID)) - { - std::uint8_t buffer[dsdl::GetNodeInfo::MaxSizeBytesResponse]{}; - - // NodeStatus - makeNodeStatusMessage(buffer); - - // SoftwareVersion (query the bootloader) - if (auto sw_success = bootloader_.getAppInfo()) - { - const ::kocherga::AppInfo sw = *sw_success; - buffer[7] = sw.major_version; - buffer[8] = sw.minor_version; - buffer[9] = 3; // Optional field flags - ::canardEncodeScalar(buffer, 80, 32, &sw.vcs_commit); - ::canardEncodeScalar(buffer, 112, 64, &sw.image_crc); - } - - // HardwareVersion - buffer[22] = hw_info_.major; - buffer[23] = hw_info_.minor; - std::memmove(&buffer[24], hw_info_.unique_id.data(), hw_info_.unique_id.size()); - buffer[40] = std::uint8_t(hw_info_.certificate_of_authenticity.size()); - std::memmove(&buffer[41], - hw_info_.certificate_of_authenticity.data(), - hw_info_.certificate_of_authenticity.size()); - - // Name - std::memcpy(&buffer[41 + hw_info_.certificate_of_authenticity.size()], - node_name_.c_str(), - node_name_.length()); - - const std::size_t total_size = 41 + hw_info_.certificate_of_authenticity.size() + node_name_.length(); - assert(total_size <= dsdl::GetNodeInfo::MaxSizeBytesResponse); - - // No need to release the transfer payload, it's empty - const auto resp_res = ::canardRequestOrRespond(&canard_, - transfer->source_node_id, - dsdl::GetNodeInfo::DataTypeSignature, - dsdl::GetNodeInfo::DataTypeID, - &transfer->transfer_id, - transfer->priority, - ::CanardResponse, - &buffer[0], - std::uint16_t(total_size)); - if (resp_res <= 0) - { - KOCHERGA_UAVCAN_LOG("GetNodeInfo resp err %d\n", resp_res); - } - } - - /* - * RestartNode request. - */ - if ((transfer->transfer_type == ::CanardTransferTypeRequest) && - (transfer->data_type_id == dsdl::RestartNode::DataTypeID)) - { - std::uint8_t response = 0; // 1 - ok, 0 - rejected - - std::uint64_t magic_number = 0; - (void) ::canardDecodeScalar(transfer, 0, 40, false, &magic_number); - - if (magic_number == 0xACCE551B1E) - { - if (platform_.tryScheduleReboot()) - { - response = 1U << 7U; - } - } - - // No need to release the transfer payload, it's single frame anyway - (void) ::canardRequestOrRespond(&canard_, - transfer->source_node_id, - dsdl::RestartNode::DataTypeSignature, - dsdl::RestartNode::DataTypeID, - &transfer->transfer_id, - transfer->priority, - ::CanardResponse, - &response, - 1U); - } - - /* - * BeginFirmwareUpdate request. - */ - if ((transfer->transfer_type == ::CanardTransferTypeRequest) && - (transfer->data_type_id == dsdl::BeginFirmwareUpdate::DataTypeID)) - { - const auto bl_state = bootloader_.getState(); - std::uint8_t error = 0; - - if ((bl_state == kocherga::State::AppUpgradeInProgress) || (remote_server_node_id_ != 0)) - { - error = 2; // Already in progress - } - else if ((bl_state == kocherga::State::ReadyToBoot)) - { - error = 1; // Invalid mode - } - else - { - // Determine the node ID of the firmware server - (void) ::canardDecodeScalar(transfer, 0, 8, false, &remote_server_node_id_); - if ((remote_server_node_id_ == 0) || - (remote_server_node_id_ >= CANARD_MAX_NODE_ID)) - { - remote_server_node_id_ = transfer->source_node_id; - } - - // Copy the path - firmware_file_path_.clear(); - for (std::uint16_t i = 0; - i < std::uint16_t(std::min(transfer->payload_len - 1U, - firmware_file_path_.capacity())); - i++) - { - char val = '\0'; - (void) ::canardDecodeScalar(transfer, i * 8U + 8U, 8, false, &val); - firmware_file_path_.push_back(val); - } - - error = 0; - } - - ::canardReleaseRxTransferPayload(&canard_, transfer); - const auto resp_res = ::canardRequestOrRespond(&canard_, - transfer->source_node_id, - dsdl::BeginFirmwareUpdate::DataTypeSignature, - dsdl::BeginFirmwareUpdate::DataTypeID, - &transfer->transfer_id, - transfer->priority, - ::CanardResponse, - &error, - 1); - if (resp_res <= 0) - { - KOCHERGA_UAVCAN_LOG("BeginFWUpdate resp err %d\n", resp_res); - } - } - - /* - * File read response. - */ - if ((transfer->transfer_type == ::CanardTransferTypeResponse) && - (transfer->data_type_id == dsdl::FileRead::DataTypeID) && - (((transfer->transfer_id + 1U) & 31U) == file_read_transfer_id_)) - { - std::int16_t error = 0; - (void) ::canardDecodeScalar(transfer, 0, 16, false, &error); - if (error != 0) - { - read_result_ = -ErrFileReadFailed; - } - else - { - read_result_ = std::min(256, std::int16_t(transfer->payload_len - 2)); - for (std::int32_t i = 0; i < read_result_; i++) - { - (void) ::canardDecodeScalar(transfer, - std::uint32_t(16 + i * 8), - 8U, - false, - &read_buffer_[std::uint32_t(i)]); - } - } - } - } - - bool shouldAcceptTransfer(std::uint64_t* out_data_type_signature, - std::uint16_t data_type_id, - ::CanardTransferType transfer_type, - std::uint8_t source_node_id) - { - using namespace impl_::dsdl; - - (void)source_node_id; - - if (::canardGetLocalNodeID(&canard_) == CANARD_BROADCAST_NODE_ID) - { - // Dynamic node ID allocation broadcast - if ((transfer_type == ::CanardTransferTypeBroadcast) && - (data_type_id == NodeIDAllocation::DataTypeID)) - { - *out_data_type_signature = NodeIDAllocation::DataTypeSignature; - return true; - } - } - else - { - // GetNodeInfo REQUEST - if ((transfer_type == ::CanardTransferTypeRequest) && - (data_type_id == GetNodeInfo::DataTypeID)) - { - *out_data_type_signature = GetNodeInfo::DataTypeSignature; - return true; - } - - // BeginFirmwareUpdate REQUEST - if ((transfer_type == ::CanardTransferTypeRequest) && - (data_type_id == BeginFirmwareUpdate::DataTypeID)) - { - *out_data_type_signature = BeginFirmwareUpdate::DataTypeSignature; - return true; - } - - // FileRead RESPONSE (we don't serve requests of this type) - if ((transfer_type == ::CanardTransferTypeResponse) && - (data_type_id == FileRead::DataTypeID)) - { - *out_data_type_signature = FileRead::DataTypeSignature; - return true; - } - - // RestartNode REQUEST - if ((transfer_type == ::CanardTransferTypeRequest) && - (data_type_id == RestartNode::DataTypeID)) - { - *out_data_type_signature = RestartNode::DataTypeSignature; - return true; - } - } - - return false; - } - - - static void onTransferReceptionTrampoline(::CanardInstance* ins, - ::CanardRxTransfer* transfer) - { - assert((ins != nullptr) && (ins->user_reference != nullptr)); - auto self = reinterpret_cast(ins->user_reference); - self->onTransferReception(transfer); - } - - static bool shouldAcceptTransferTrampoline(const ::CanardInstance* ins, - std::uint64_t* out_data_type_signature, - std::uint16_t data_type_id, - ::CanardTransferType transfer_type, - std::uint8_t source_node_id) - { - assert((ins != nullptr) && (ins->user_reference != nullptr)); - auto self = reinterpret_cast(ins->user_reference); - return self->shouldAcceptTransfer(out_data_type_signature, - data_type_id, - transfer_type, - source_node_id); - } - -public: - /** - * @param blc mutable reference to the bootloader instance - * @param platform node platform interface - * @param name product ID, UAVCAN node name; e.g., com.zubax.telega - * @param hw hardware version information per UAVCAN specification - */ - BootloaderNode(::kocherga::BootloaderController& blc, - IUAVCANPlatform& platform, - const NodeName& name, - const HardwareInfo& hw) : - bootloader_(blc), - platform_(platform), - node_name_(name), - hw_info_(hw) - { - next_1hz_task_invocation_at_ = bootloader_.getMonotonicUptime(); - } - - /** - * Runs the node thread. - * This function never returns unless IUAVCANPlatform::shouldExit() returns true. - * If an RTOS is available, it is advisable to run this method from a separate thread. - * Otherwise, it is possible to perform other tasks by hijacking certain platform API functions, - * such as sleep(), receive(), send(), and resetWatchdog(). - * - * @param can_bus_bit_rate set if known; defaults to zero, which initiates CAN bit rate autodetect - * @param node_id set if known; defaults to zero, which initiates dynamic node ID allocation - * @param remote_server_node_id set if known; defaults to zero, which makes the node wait for an update request - * @param remote_file_path set if known; defaults to an empty string, which can be a valid path too - */ - void run(const std::uint32_t can_bus_bit_rate = 0, - const std::uint8_t node_id = 0, - const std::uint8_t remote_server_node_id = 0, - const char* const remote_file_path = "") - { - this->can_bus_bit_rate_ = can_bus_bit_rate; - - if ((remote_server_node_id >= CANARD_MIN_NODE_ID) && - (remote_server_node_id <= CANARD_MAX_NODE_ID)) - { - this->remote_server_node_id_ = remote_server_node_id; - this->firmware_file_path_ = remote_file_path; - } - - ::canardInit(&canard_, - memory_pool_.data(), - memory_pool_.size(), - &BootloaderNode::onTransferReceptionTrampoline, - &BootloaderNode::shouldAcceptTransferTrampoline, - this); - - if ((node_id >= CANARD_MIN_NODE_ID) && - (node_id <= CANARD_MAX_NODE_ID)) - { - ::canardSetLocalNodeID(&canard_, node_id); - } - - runNodeThread(); - } - - /** - * Returns the CAN bus bit rate, if known, otherwise zero. - */ - std::uint32_t getCANBusBitRate() const - { - return can_bus_bit_rate_; // No thread sync is needed, read is atomic - } - - /** - * Returns the local UAVCAN node ID, if set, otherwise zero. - */ - std::uint8_t getLocalNodeID() const - { - return confirmed_local_node_id_; // No thread sync is needed, read is atomic - } -}; - -} diff --git a/kocherga_ymodem.hpp b/kocherga_ymodem.hpp deleted file mode 100644 index ec760cc..0000000 --- a/kocherga_ymodem.hpp +++ /dev/null @@ -1,640 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2018 Zubax Robotics - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of - * this software and associated documentation files (the "Software"), to deal in - * the Software without restriction, including without limitation the rights to - * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - * the Software, and to permit persons to whom the Software is furnished to do so, - * subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - * Author: Pavel Kirienko - */ - -#pragma once - -#include -#include -#include - -// Oh C, never change. -#ifdef CAN -#undef CAN -#endif - - -namespace kocherga_ymodem -{ -/** - * Error codes specific to this module. - */ -static constexpr std::int16_t ErrOK = 0; -static constexpr std::int16_t ErrPortWriteTimedOut = 2001; -static constexpr std::int16_t ErrRetriesExhausted = 2002; -static constexpr std::int16_t ErrProtocolError = 2003; -static constexpr std::int16_t ErrTransferCancelledByRemote = 2004; -static constexpr std::int16_t ErrRemoteRefusedToProvideFile = 2005; -static constexpr std::int16_t ErrPortError = 2006; - -/** - * Abstracts a platform-specific serial port and related functions for the YMODEM protocol. - * The application can use these functions to reset its watchdog also, provided that the watchdog timeout - * is not less than one second (otherwise, false-positive timeouts are possible). - */ -class IYModemPlatform -{ -public: - virtual ~IYModemPlatform() = default; - - /** - * Result of the IO operations. - */ - enum class Result : std::uint8_t - { - Success, ///< Operation was completed successfully. - Timeout, ///< Operation has timed out. - Error ///< Operation has failed. - }; - - /** - * Emits one byte into the port. - * @param byte The byte to emit. - * @param timeout The operation will be aborted if the byte could not be emitted in this amount of time. - * @return @ref Result. - */ - virtual Result emit(std::uint8_t byte, std::chrono::microseconds timeout) = 0; - - /** - * Receives one byte from the port. - * @param out_byte A reference where to store the received byte. - * @param timeout The operation will be aborted if the byte could not be received in this amount of time. - * @return @ref Result. - */ - virtual Result receive(std::uint8_t& out_byte, std::chrono::microseconds timeout) = 0; - - /** - * Returns the time since boot as a monotonic (i.e. steady) clock. - * The clock must never overflow. - * This is like @ref kocherga::IPlatform::getMonotonicUptime(). - */ - virtual std::chrono::microseconds getMonotonicUptime() const = 0; -}; - -/** - * Downloads data using YMODEM or XMODEM protocol over the specified ChibiOS channel - * (e.g. serial port, USB CDC ACM, TCP, ...). - * - * This class will request Checksum mode, in order to retain compatibility both with XMODEM and YMODEM senders - * (YMODEM-compatible senders do support checksum mode as well as CRC mode). - * Both 1K and 128-byte blocks are supported. - * Overall, the following protocols are supported: - * - YMODEM - * - XMODEM - * - XMODEM-1K - * - * Reference: http://pauillac.inria.fr/~doligez/zmodem/ymodem.txt - */ -class YModemProtocol final : public kocherga::IProtocol -{ - static constexpr std::uint16_t BlockSizeXModem = 128; - static constexpr std::uint16_t BlockSize1K = 1024; - static constexpr std::uint16_t WorstCaseBlockSizeWithCRC = BlockSize1K + 2; - - /// The timeouts are according to the YMODEM specification - static constexpr std::chrono::microseconds SendTimeout {1'000'000}; // NOLINT - static constexpr std::chrono::microseconds CharReceiveTimeout {1'000'000}; // NOLINT - static constexpr std::chrono::microseconds InitialTimeout {60'000'000}; // NOLINT - static constexpr std::chrono::microseconds NextBlockTimeout {5'000'000}; // NOLINT - static constexpr std::chrono::microseconds BlockPayloadTimeout {1'000'000}; // NOLINT - - static constexpr std::uint8_t MaxRetries = 3; - - struct ControlCharacters - { - static constexpr std::uint8_t SOH = 0x01; - static constexpr std::uint8_t STX = 0x02; - static constexpr std::uint8_t EOT = 0x04; - static constexpr std::uint8_t ACK = 0x06; - static constexpr std::uint8_t NAK = 0x15; - static constexpr std::uint8_t CAN = 0x18; - }; - - IYModemPlatform& platform_; - std::uint8_t buffer_[WorstCaseBlockSizeWithCRC]{}; - - - static std::uint8_t computeChecksum(const void* data, std::uint16_t size) - { - auto p = static_cast(data); - return std::uint8_t(std::accumulate(p, p + size, 0)); - } - - std::int16_t send(std::uint8_t byte) - { - KOCHERGA_TRACE("YMODEM TX 0x%x\n", byte); - switch (platform_.emit(byte, SendTimeout)) - { - case IYModemPlatform::Result::Success: - { - return 0; - } - case IYModemPlatform::Result::Timeout: - { - return -ErrPortWriteTimedOut; - } - case IYModemPlatform::Result::Error: - { - return -ErrPortError; - } - } - - return -ErrPortError; - } - - std::int16_t receive(void* data, const std::uint16_t size, std::chrono::microseconds timeout) - { - assert(size <= kocherga::MaxDataBlockSize); - auto* ui8 = static_cast(data); - for (std::uint16_t i = 0; i < size; i++) - { - std::uint8_t byte = 0; - switch (platform_.receive(byte, CharReceiveTimeout)) - { - case IYModemPlatform::Result::Success: - { - *ui8++ = std::uint8_t(byte); - break; - } - case IYModemPlatform::Result::Timeout: - { - /* - * Note that we may greatly overstay the timeout here, but this is by design, - * since the spec requires that each character must be received with 1 second timeout. - */ - if (timeout <= CharReceiveTimeout) - { - return std::int16_t(i); - } - else - { - timeout -= CharReceiveTimeout; - } - break; - } - case IYModemPlatform::Result::Error: - { - return -ErrPortError; - } - } - } - - return std::int16_t(size); - } - - void abort() - { - constexpr std::uint8_t Times = 5; // Multiple CAN are required! - for (std::uint8_t i = 0; i < Times; i++) - { - if (send(ControlCharacters::CAN) != 1) - { - break; - } - } - } - - enum class BlockReceptionResult : std::uint8_t - { - Success, - Timeout, - EndOfTransmission, - TransmissionCancelled, - ProtocolError, - SystemError - }; - - /** - * Reads a block from the channel. This function does not transmit anything. - * @return First component: @ref BlockReceptionResult - * Second component: system error code, if applicable - */ - std::pair receiveBlock(std::uint16_t& out_size, - std::uint8_t& out_sequence) - { - // Header byte - std::uint8_t header_byte = 0; - std::int16_t res = receive(&header_byte, 1, NextBlockTimeout); - if (res < 0) - { - return { BlockReceptionResult::SystemError, res }; - } - if (res != 1) - { - return { BlockReceptionResult::Timeout, 0 }; - } - - switch (header_byte) - { - case ControlCharacters::STX: - { - out_size = BlockSize1K; - break; - } - case ControlCharacters::SOH: - { - out_size = BlockSizeXModem; - break; - } - case ControlCharacters::EOT: - { - KOCHERGA_TRACE("YMODEM RX EOT\n"); - return { BlockReceptionResult::EndOfTransmission, 0 }; - } - case ControlCharacters::CAN: - { - KOCHERGA_TRACE("YMODEM RX CAN\n"); - return { BlockReceptionResult::TransmissionCancelled, 0 }; - } - default: - { - KOCHERGA_TRACE("YMODEM unexpected header 0x%x\n", header_byte); - return { BlockReceptionResult::ProtocolError, 0 }; - } - } - - // Sequence ID - std::uint8_t sequence_id_bytes[2] = {}; - res = receive(sequence_id_bytes, 2, BlockPayloadTimeout); - if (res < 0) - { - return { BlockReceptionResult::SystemError, res }; - } - if (res != 2) - { - return { BlockReceptionResult::Timeout, 0 }; - } - if (sequence_id_bytes[0] != static_cast(~sequence_id_bytes[1])) // Invalid sequence ID - { - KOCHERGA_TRACE("YMODEM non-inverted sequence ID: 0x%x 0x%x\n", sequence_id_bytes[0], sequence_id_bytes[1]); - return { BlockReceptionResult::ProtocolError, 0 }; - } - out_sequence = sequence_id_bytes[0]; - - // Payload - constexpr auto ChecksumSize = 1; - const auto block_size_with_checksum = std::uint16_t(out_size + ChecksumSize); - res = receive(buffer_, block_size_with_checksum, BlockPayloadTimeout); - if (res < 0) - { - return { BlockReceptionResult::SystemError, res }; - } - if (std::uint16_t(res) != block_size_with_checksum) - { - return { BlockReceptionResult::Timeout, 0 }; - } - - // Checksum validation - if (computeChecksum(buffer_, out_size) != buffer_[out_size]) - { - KOCHERGA_TRACE("YMODEM checksum error, not %d\n", buffer_[out_size]); - return { BlockReceptionResult::ProtocolError, 0 }; - } - - return { BlockReceptionResult::Success, 0 }; - } - - static bool tryParseZeroBlock(const std::uint8_t* const data, - const std::uint16_t size, - bool& out_is_null_block, - std::uint32_t& out_file_size) - { - assert(size == BlockSizeXModem || size == BlockSize1K); - - // Initializing defaults - out_is_null_block = true; // Paranoia - out_file_size = 0; // I.e. unknown - - std::uint16_t offset = 0; - - // Skipping the file name - while ((offset < size) && (data[offset] != 0)) - { - offset++; - } - if (offset >= size) - { - return false; // No null termination, invalid block - } - KOCHERGA_TRACE("YMODEM file name: '%s'\n", reinterpret_cast(data)); - - // Setting the null block indication, aborting if it is null block because it won't contain file size - out_is_null_block = offset == 0; // No filename means null block (end of session) - if (out_is_null_block) - { - return true; - } - - // Not a null block - parsing the file size - offset++; - KOCHERGA_TRACE("YMODEM all fields: '%s'\n", reinterpret_cast(&data[offset])); - while ((offset < size) && (data[offset] != 0) && (data[offset] != ' ')) - { - if (data[offset] < '0' || - data[offset] > '9') - { - out_file_size = 0; // Bad character before termination - break; - } - out_file_size *= 10; - out_file_size += std::uint32_t(data[offset] - std::uint8_t('0')); - offset++; - } - KOCHERGA_TRACE("YMODEM file size int: %u\n", unsigned(out_file_size)); - - return true; - } - - static std::int16_t processDownloadedBlock(kocherga::IDownloadSink& sink, void* data, std::uint16_t size) - { - KOCHERGA_TRACE("YMODEM received block of %d bytes\n", size); - return sink.handleNextDataChunk(data, size); - } - -public: - /** - * @param serial_port the serial port channel that will be used for downloading - */ - explicit YModemProtocol(IYModemPlatform& serial_port) : - platform_(serial_port) - { } - - std::int16_t downloadImage(kocherga::IDownloadSink& sink) override - { - // This thing will make sure there's no residual garbage in the RX buffer afterwards - struct Flusher - { - IYModemPlatform& port; - ~Flusher() - { - std::uint8_t dummy = 0; - while (port.receive(dummy, std::chrono::microseconds(1'000)) == IYModemPlatform::Result::Success) - { - KOCHERGA_TRACE("YMODEM FLUSH RX 0x%x\n", unsigned(dummy)); - } - } - } flusher_{platform_}; - - // State variables - std::uint32_t remaining_file_size = 0; - bool file_size_known{}; - std::uint8_t expected_sequence_id = 123; // Arbitrary invalid value - - enum class Mode - { - XModem, - YModem - } mode{}; - - /* - * Initiating the transfer, receiving the first block. - * The sequence ID will be 0 in case of YMODEM, and 1 in case of XMODEM. - */ - const auto started_at = platform_.getMonotonicUptime(); - for (;;) - { - KOCHERGA_TRACE("Trying to initiate X/YMODEM transfer...\n"); - - // Abort if we couldn't get it going in InitialTimeout - if ((platform_.getMonotonicUptime() - started_at) > InitialTimeout) - { - abort(); - return -ErrRetriesExhausted; - } - - // Requesting transmission in checksum mode - if (const auto res = send(ControlCharacters::NAK); res < 0) - { - abort(); - return res; - } - - // Receiving the block - std::uint16_t size = 0; - const auto block_rx_res = receiveBlock(size, expected_sequence_id); - if (block_rx_res.first == BlockReceptionResult::Success) - { - ; - } - else if (block_rx_res.first == BlockReceptionResult::Timeout || - block_rx_res.first == BlockReceptionResult::ProtocolError || - block_rx_res.first == BlockReceptionResult::EndOfTransmission) - { - continue; // EOT cannot be sent in response to the first block, it's an error; trying again... - } - else if (block_rx_res.first == BlockReceptionResult::TransmissionCancelled) - { - abort(); - return -ErrTransferCancelledByRemote; - } - else - { - assert(block_rx_res.first == BlockReceptionResult::SystemError); - abort(); - return block_rx_res.second; - } - - // Processing the block - if (expected_sequence_id == 0) - { - mode = Mode::YModem; - - bool is_null_block = true; - const bool zero_block_valid = tryParseZeroBlock(buffer_, size, is_null_block, remaining_file_size); - - KOCHERGA_TRACE("YMODEM zero block: valid=%d null=%d size=%u\n", - zero_block_valid, is_null_block, unsigned(remaining_file_size)); - - if (!zero_block_valid) - { - // Invalid zero block, that's a fatal error, it's checksum protected after all - // Retrying here would make no sense, it's not a line hit, it's badly formed packet! - abort(); - return -ErrProtocolError; - } - if (is_null_block) - { - // Null block means that the sender is refusing to transmit the file - // No point retrying too, the sender isn't going to change their mind - abort(); - return -ErrRemoteRefusedToProvideFile; - } - file_size_known = remaining_file_size > 0; - - // The zero block requires a dedicated ACK, sending it now - if (const auto res = send(ControlCharacters::ACK); res < 0) - { - abort(); - return res; - } - } - else if (expected_sequence_id == 1) - { - mode = Mode::XModem; - KOCHERGA_TRACE("YMODEM zero block skipped (XMODEM mode)\n"); - - if (const auto res = processDownloadedBlock(sink, buffer_, size); res < 0) - { - abort(); - return res; - } - file_size_known = false; - } - else // Invalid sequence number - { - abort(); - return -ErrProtocolError; - } - - // Done! - expected_sequence_id = std::uint8_t(expected_sequence_id + 1); - break; - } - - assert(file_size_known ? true : (remaining_file_size == 0)); - - /* - * Receiving the file - */ - bool ack = mode == Mode::XModem; // YMODEM requires another NAK after the zero block - auto remaining_retries = MaxRetries; - for (;;) - { - // Limiting retries - if (remaining_retries <= 0) - { - abort(); - return -ErrRetriesExhausted; - } - remaining_retries--; - - // Confirming or re-requesting - if (const auto res = send(ack ? ControlCharacters::ACK : ControlCharacters::NAK); res < 0) - { - abort(); - return res; - } - ack = false; - - // Receiving the block - std::uint16_t size = 0; - std::uint8_t sequence_id = 0; - const auto block_rx_res = receiveBlock(size, sequence_id); - if (block_rx_res.first == BlockReceptionResult::Success) - { - ; - } - else if (block_rx_res.first == BlockReceptionResult::Timeout || - block_rx_res.first == BlockReceptionResult::ProtocolError) - { - continue; - } - else if (block_rx_res.first == BlockReceptionResult::EndOfTransmission) - { - if ((file_size_known) && (remaining_file_size != 0)) - { - // The sender said that we're done, liar! - KOCHERGA_TRACE("YMODEM ended %u bytes early\n", unsigned(remaining_file_size)); - abort(); - return -ErrProtocolError; - } - // Done, exiting and sending the final ACK - KOCHERGA_TRACE("YMODEM end OK\n"); - break; - } - else if (block_rx_res.first == BlockReceptionResult::TransmissionCancelled) - { - KOCHERGA_TRACE("YMODEM cancelled\n"); - abort(); - return -ErrTransferCancelledByRemote; - } - else - { - assert(block_rx_res.first == BlockReceptionResult::SystemError); - abort(); - return block_rx_res.second; - } - remaining_retries = MaxRetries; // Reset retries on successful reception - - // Processing the block - if ((sequence_id + 1) == expected_sequence_id) // Duplicate block, acknowledge silently - { - KOCHERGA_TRACE("YMODEM duplicate block skipped\n"); - ack = true; - continue; - } - if (sequence_id != expected_sequence_id) // Totally wrong sequence, abort - { - KOCHERGA_TRACE("YMODEM wrong sequence ID\n"); - abort(); - return -ErrProtocolError; - } - expected_sequence_id = std::uint8_t(expected_sequence_id + 1); - - // Making sure we're not past the end of file - if (file_size_known) - { - if (remaining_file_size == 0) - { - KOCHERGA_TRACE("YMODEM transmission past the end of file\n"); - abort(); - return -ErrProtocolError; - } - if (size > remaining_file_size) - { - size = std::uint16_t(remaining_file_size); - } - remaining_file_size -= size; - } - - // Sending the block over - if (const auto res = processDownloadedBlock(sink, buffer_, size); res < 0) - { - abort(); - return res; - } - - // Done, continue to the next block - ack = true; - } - - /* - * Final response and then leaving. - * Errors can be ignored - we got what we wanted anyway. - */ - KOCHERGA_TRACE("YMODEM finalizing\n"); - - (void)send(ControlCharacters::ACK); // If it fails, who cares. - - if (mode == Mode::YModem) - { - // Letting the sender know we don't want any other files. Is this compliant? - abort(); - } - - return ErrOK; - } -}; - -} diff --git a/populate_app_descriptor.py b/populate_app_descriptor.py deleted file mode 100755 index 0c650db..0000000 --- a/populate_app_descriptor.py +++ /dev/null @@ -1,293 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. -# Copyright (c) 2018-2019 Zubax Robotics . All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Based on make_can_boot_descriptor.py originally created by Ben Dyer, David Sidrane and Pavel Kirienko for PX4. -# See https://github.com/PX4/Firmware/blob/nuttx_next/Tools/make_can_boot_descriptor.py -# - -import os -import sys -import struct -import optparse -import binascii -from io import BytesIO - - -class AppDescriptor(object): - """ - Kocherga application image descriptor format: - uint64_t signature (bytes [7:0] set to 'APDesc00' in source) - uint64_t image_crc (set to 0 in source, updated by this script) - uint32_t image_size (set to 0 in source, updated by this script) - uint32_t vcs_commit (set in source) - uint8_t version_major (set in source) - uint8_t version_minor (set in source) - uint8_t flags (1 - release build, 2 - dirty build) (set in source) - (uint8_t reserved) - uint32_t build_timestamp_utc (set in source) - """ - - LENGTH = 32 - SIGNATURE = b'APDesc00' - FORMAT = '<8sQLLBBBxL' - - def __init__(self, bytes=None): - self.signature = AppDescriptor.SIGNATURE - self.image_crc = 0 - self.image_size = 0 - self.vcs_commit = 0 - self.version_major = 0 - self.version_minor = 0 - self.release_build = False - self.dirty_build = False - self.build_timestamp_utc = 0 - - if bytes: - try: - self.unpack(bytes) - except Exception: - raise ValueError("Invalid AppDescriptor: {0}".format(binascii.b2a_hex(bytes))) - - def pack(self): - flags = 0 - if self.release_build: - flags |= 1 - - if self.dirty_build: - flags |= 2 - - return struct.pack(self.FORMAT, self.signature, self.image_crc, self.image_size, self.vcs_commit, - self.version_major, self.version_minor, flags, self.build_timestamp_utc) - - def unpack(self, bytes): - (self.signature, - self.image_crc, - self.image_size, - self.vcs_commit, - self.version_major, - self.version_minor, - flags, - self.build_timestamp_utc) = struct.unpack(self.FORMAT, bytes) - - self.release_build = bool(flags & 1) - self.dirty_build = bool(flags & 2) - - if not self.empty and not self.valid: - raise ValueError() - - @property - def empty(self): - return (self.signature == AppDescriptor.SIGNATURE and - self.image_crc == 0 and self.image_size == 0) - - @property - def valid(self): - return (self.signature == AppDescriptor.SIGNATURE and - self.image_crc != 0 and self.image_size > 0 and - self.build_timestamp_utc > 0) - - -class FirmwareImage(object): - # Padding to 8 bytes is required by Kocherga - PADDING = 8 - - def __init__(self, path, mode="r"): - self._file = open(path, (mode + "b").replace("bb", "b")) - self._padding = self.PADDING - - if "r" in mode: - self._contents = BytesIO(self._file.read()) - else: - self._contents = BytesIO() - self._do_write = False - - self._length = None - self._descriptor_offset = None - self._descriptor_bytes = None - self._descriptor = None - - def __enter__(self): - return self - - def __getattr__(self, attr): - if attr == "write": - self._do_write = True - return getattr(self._contents, attr) - - def __iter__(self): - return iter(self._contents) - - def __exit__(self, *args): - if self._do_write: - if getattr(self._file, "seek", None): - self._file.seek(0) - self._file.write(self._contents.getvalue()) - if self._padding: - self._file.write(b'\xff' * self._padding) - - self._file.close() - - def _write_descriptor_raw(self): - # Seek to the appropriate location, write the serialized descriptor, and seek back. - prev_offset = self._contents.tell() - self._contents.seek(self._descriptor_offset) - self._contents.write(self._descriptor.pack()) - self._contents.seek(prev_offset) - - def write_descriptor(self): - # Set the descriptor's length and CRC to the values required for CRC computation - self.app_descriptor.image_size = self.length - self.app_descriptor.image_crc = 0 - - self._write_descriptor_raw() - - # Update the descriptor's CRC based on the computed value and write it out again - self.app_descriptor.image_crc = self.crc - - self._write_descriptor_raw() - - @property - def crc(self): - MASK = 0xFFFFFFFFFFFFFFFF - POLY = 0x42F0E1EBA9EA3693 - - # Calculate the image CRC with the image_crc field in the app descriptor zeroed out. - crc_offset = self.app_descriptor_offset + len(AppDescriptor.SIGNATURE) - content = bytearray(self._contents.getvalue()) - content[crc_offset:crc_offset + 8] = bytearray(b"\x00" * 8) - if self._padding: - content += bytearray(b"\xff" * self._padding) - val = MASK - for byte in content: - val ^= (byte << 56) & MASK - for bit in range(8): - if val & (1 << 63): - val = ((val << 1) & MASK) ^ POLY - else: - val <<= 1 - - return (val & MASK) ^ MASK - - @property - def length(self): - if not self._length: - # Find the length of the file by seeking to the end and getting the offset - prev_offset = self._contents.tell() - self._contents.seek(0, os.SEEK_END) - self._length = self._contents.tell() - if self._padding: - mod = self._length % self._padding - self._padding = self._padding - mod if mod else 0 - self._length += self._padding - self._contents.seek(prev_offset) - - return self._length - - @property - def app_descriptor_offset(self): - if not self._descriptor_offset: - # Save the current position - prev_offset = self._contents.tell() - # Check each byte in the file to see if a valid descriptor starts at that location. - # Slow, but not slow enough to matter. - offset = 0 - while offset < self.length - AppDescriptor.LENGTH: - self._contents.seek(offset) - try: - # If this throws an exception, there isn't a valid descriptor at this offset - AppDescriptor(self._contents.read(AppDescriptor.LENGTH)) - except Exception: - offset += 1 - else: - self._descriptor_offset = offset - break - # Go back to the previous position - self._contents.seek(prev_offset) - - return self._descriptor_offset - - @property - def app_descriptor(self): - if not self._descriptor: - # Save the current position - prev_offset = self._contents.tell() - # Jump to the descriptor and parse it - self._contents.seek(self.app_descriptor_offset) - self._descriptor_bytes = self._contents.read(AppDescriptor.LENGTH) - self._descriptor = AppDescriptor(self._descriptor_bytes) - # Go back to the previous offset - self._contents.seek(prev_offset) - - return self._descriptor - - @app_descriptor.setter - def app_descriptor(self, value): - self._descriptor = value - - -if __name__ == "__main__": - parser = optparse.OptionParser(usage= - "Usage: %prog [options] !%;~cAio)&RhwQ11GM(tO}H7%k2zc?4~$Fuo2 zo_r~M>>!y$-?JI3(SBF55Q#Ktw%R0|fmGhV9Yq+CVX*E(@g8)AVoetxBKzX&5d&C+zGk{f2aXvlRJW1o!UXdcgLw8!Iz1A{p7rb589Nf z;Fs!m&a^kzZ|6@#J2bfoYZh#*Krr4G zDWur-8I+Ir&Hrz-HpAx}Ji>u0q1Q^MKyme}` zi%A!;G^c}%fxO)8WW=+ueH{hc*Fo$W>U~eqqlV>qQ2X8tc#~9eO)o91+XG#6O0zD>uvC}1t5d7>Kknf`TA#`!B)7(5La!a&xG;oq?gbF!%;8sabm zCNcWms&qpG=EmZx$?vzk&uMUm8Z^JnKD$`cqK>vHwh-|Ya||=x2S65vd^8Kn8=e`~ z1gUr(G_$HEH$pm^WF>uT_3LsFTI)W@5%r$C>aQ?&(Hbc41)9nNu9I~~O=DYK8UN~J zuZ=E^;p>7ALuQ&85z^#4FC~_aM@pspp*=c@y=IO*B&`)FKxFZ>A81WgllA-AYtFfb zKyq9@aO}pp`oQ{n<^APrvh`QrzQMQOd{Mr?&-*&Lw9os~@`^t1E98nk?~luu_N#LC zTD}Shk3i<15_Y43Z9$m$V@(D*f(;08Lop|Qw$H=RpLS0)1A!KhXx*M*{OPg zSG6n;u5QU!l|a{HU)>B#tgJF%2QzZ+5MU)ZnNiF-17F{|$(Ff;%gNjxEuWgGXw zQkC03ObKr|lAwOWkmJk$3LE4*CqeIF68i`FU+K@kp(p>LIYgUK@{bb!-I-R}h`1vt z?(O;@4SpDUlRzM=Zi!J!QPHZ&>PyzZZ6b^K1L) z($z}JpCX0?CFQ(?kCKBNEDx(GqXyt&QmI`scf{pD*4y7IZTmG_+oW5dA3&6kQ>JmQ zz*B{59q`vPcZ^Kpa;QB{rO$Ce@_hSH%n^mk9O0FI>}JHjwyX05Mcxef?A=7qcA&4W z!zKQ#(!G!Saa{M_bI(20zhuZG*Vw}ghYHN7h|~RmqN8eJ$G!`Uhg)U!qzpb*I43=d zQ~7yN+2sQ`U6|wp>b%wd_-nc+LVt^N3`r+l->TA`hu$^C8~9;=lBMAxPYyb8yDd>< za>CK)m*h7~t%%hOX)*)95O3e$?Qv(aK=$VK$$xK1dz?+1>Zr$#LWO?1(?)ve%@X>1 zaxrr9074v9!f!+PV!Z88-ZsM@lh`c1<~1mq9W{mGD#AHE=k9#ZdGzm|)d4>ICO{`g(fjq)SIi7Cw zTHk@r9IgLlSBVUC?8m|9TyHXT>__7X@*Tc3Xyjh4N(aT_6 zO@{V;v$O>H%=CRrKI$;>=Y(cy6yo?4T+wZK4h5Z%uEbp%DC1 z@QvGCRC=lj7GYL9qm{DXj5w(dB+!a%&LUWW_SMj@cz+dX_}*GUsbM*0KgT!JzTi^l z-|JH6-G;srgt#|$1$6PLub9wR=uGB8UkvmWy3_gXGesu`^CRey)NgCt;H!&)`P#Q$ zFk(;vbKw!}KPDMAcQPhhSgUjs5Y`F5tNiOh*?~Cay>977Z<}<+n_8G!FsPJCj4Wc( zavjO}DR~M~U}@9^+i24G0p^Tx`tD&@8$+iiV;%XnGYS1kmFSgj@u(n`rN*0An(9&Kkn3FV?Lk?@8k0wH zF7?%C#2%sus^1_SK~Uq&ay-{yg*U=1KbX*kDUsQ zC5uC9f&9^&MS1Lim~)1%g~cPz1X>L zQF|Cmq0N${>fqY8<2)Rat!n0=<1p8%aTZ+M4(cjnjI_=}JKcl1nDp4^zLj}EllPpW z&*YWHV1IL5k9sjN=JQC)f%X`Sev0(?AMKaYxy{?ZFrjovdR)V7=o-Ijj?A8$Fv}WW z(k6}XiML;&fsYr~TUOQ$wq^p^v;DxUMaaWm4Wvwh zuo5ZfSG4oy&Rb9)wAQ?cHHgM>^)J5lr+4~9kBrJoL=N8eCJK>==`Cm(>2a_1zA8lE zzSH}y5RUtHZ(?yj++X(=6^G%z+52j7DDE$Nzby{Qqu%P3s_JGUO)IXua0$5X$5n#s z5nPYsT7j!?`q(@0)V^-upSDTft~RNESGs+RkOb*hb>li<;)kI}t?686=^(w; z%l$q!*Ue~XHhc;D%5|2oW0(tb+oW^;^38qA#AC;wi`f7zJneiLcKS4<+rf9BIf~}0 zO!P&Xmkj8OD|^*3(I=`4Uje>`hF4eqBJGl2o?6DgoYyL?ZhtZFeDSsOG5*^(;(dAh zTS67?;q4~=A6WI`JBS0;EQPf-YQw~Ey(D)8KIS`bvhW`y^@Zm?8%gp?IygkqfsUqYxv&_KXi}$-b z%~-wf!IK4LV8rw?SmVl@S9rK>+L*j%PDiPb4oap?CAIStdUn@7E%3>=N)4qs0AhiaTeD5_dv&Pt0bO_<<60`SKA;%w~ib zuEhKaV!j@4QT~_@mzntScn?9$CO#8SE|ixM|4)f|+!bQppv3%_<)aEWX#jXpZbD>{@y|qe4r?RJf(Cb@21|eS*iC? zO+;z50+D9Lci4f2usp2B*TZUjJ#4pCFFmV9k6hWTW@_BtY4u`g*|g@^yrICSHj5wH zrk$6wKfKI`p_h*z6d%+dsSMN~XQDsuP-N`|+OMKNE=7*W-iKM-jQ#i`yh-i8{xouR}VQFhxEdLyMDl{5goagyT8W8*dj_3DkYl-@WK zJ#LiZ{*z4&cVBM=T^l`>+!7VHuWfrvF>9DB*TDH<32cSQCI}ZP#MV+l^)+(*B z^w_lKn4H&Y$2zGUGR{sW4E(dbeJu0)7@_zOV6%oo2l?Bw+~x}(uejcDwy*1xe%Elt z^)VO49;f_!exTYT1xMcUH|-{ub=SW_*t;4@v?Y4-IVv< z@s#Y^d09N*|6k_kfn8_Ls-UYyI?dhSw~;=B)q81gMf(PvPfBCb`Zt6XM1h~s?}W2< zgGGN75*~~h_*k0L+QK-2G|7LKW*qR{-#WmEcX=3bhKJ@LGcJlJ@B{V;;!AB9sKl3~ z#5WxAS(uu36?0C?>W<1kC*^^A_p|h^XC_8Jop1AG!A`qI+V#W8-F1G z#eVl{Pha;y{ImS=o78>N56HuHL%*K#$FN+Fy+um@l6$7&UjMu9$%?y~`MvP5in~qK z*FBK`V1NF_puY7V$bUP|hKjsiXW#k>xMRPjxI5af?0&-UUf<;x0iIyCvv z^IW#a^D5-;^^e0BFlG`Lzvlm=5!e#}}94IjB*tHGNJ$x`fs(3a09I(N%qH3CG3nlyRkYkd$Y9>*)BmRwS za67U=zs|;L^_T*r*BLz9Ab}`0A!TYr=Etbc-WT}+gO^H)a zqSPtQ_&kTa&j2mI=N)pr!66?hVV^$3kjM5<`}YvYY66aWLt^sR*%JV9#we1n=u`?Zb5(7h$UWgu4eYJyS2bh;qvvY>+7ja-y8;y`Q^0OOU_Iz9f7vZndHcRsc+2NU+YiqN zS}5c-yQP(0qIa}P6FmX`!yn5FwPSq!ck)L+VjsLUoP$2uAIX2VOV9O*?@AhK9P;;5 zXCS^nJ_F_bv;5xKC;s1;my$y{-AvTypXK-3KJoote%Eyd;=4S*x`Oig0r@V2UmjMC zwby86#gq=8?3PUV@aq8S_EHF+{OslaBRi;W{@NMvPibMkg}kaH4eD0GZF*+|%GEm$ z0r@RS3hAcQ$URV({`FW1-wBkQR>{;^io9God~+9tC-=+3_pHYUalWVZMh}Av0s2zk z^`NFhaYr5k>jB9NR@Q{`k!zot52;NPlOi~x-r%+m=DD?MoOohPIkI`=fF_bQ+^C*X zmwn!x6^9)%Np%)9T-8geGwWy+WID4sqkMIO0a_q*5?4=W39mZ&n^RHFOUU&@{gQu+ zA^A7T@y`8Pza(Wel`xyQOP8 zx~0#$0=VAY(kRqX0TPd(jx{iuFkLj4Ttsh@it376H+{iq++#!8O+ z;=$(SqaSR3ezdQC@*74t2uFR5h2&w)M-#~I3uInzNe6Pvwx}f4dLXgj4Ak^r)z0AG z)K33DsT~U8s~vjs)sCy%S3AU`?v}Q;cS|>PX(63TI>Fsib$jCP)QuVAgX-pT{S;Xm zQD=ZoMxe@w+B}f>N2!eX58GhATqua!2!zPBHeLwNncN)2odinc(>B7~TxA3LTsUkk z5mw}iyLzFK-tF{JWJT%Tu2$&*&je7p4-3~7GzQ&s{IGDl_KxH43*Q1M@))d)7`S8B z=wd=49i2a>@Fv(Y8I{K*9tN$LuvZUT*&G-1jaNUEL)N>xyL6OT6udT?RRJC6yrL|@wa zqR1k)s!fXTB)LCeuZ!2CO=If_bc}w9~xH27_%Qbth{fu^gCKE9D2wniBWDX_kXQ| zpYn&q1BY?-)wc_$6}{IF?LFB6T4O}({fget_t`&ZgSHX=x!VxJb%b;zb%eWjx(#7j z%^|F@Bg_LdAvPk28S#jDiCLG(H4TeSn^ElfL&A%{uiH&B2;BXExnwuK$<4O0{aE zCHRB|dOoYq>(JssSuWq7s8aNMhrs&xYEwSTr0MFS`J7lA-qy)kra|^7 z$dH&#gZ4AV6}amUm@?QL)~GhAwtJ<~p#1y3a^Me5RnBN#67T@(N}@QOEt1pK;2y{? z;mFTh6>VoH%qt`Oslk5#;H%a-;BPwY)-utwKKgpk+_ze#VDulU11+O)Hy(sl8{Gk_ z|9(P$k-1gn3|p*ASD%=6F($`QOSQBCcKU357=I~-A3GXkJyR~sVHY1S7vk8U<4*|# zMcZwEcc@^=l5Jw~Uj@62mB1^E=2zm$2&(!WjjTIXHAfT0mQObd;p}WCr*3)NQQ4Le zl#^3OnmUK@Y+QUJ`+>Y7W6PqZMM;YWsk57zdDcbIz@B1^AIMKD(u9`P@ zA&2jfOOaP*e2K8S7L6(!oJf^BS^ELrH-b(_cFr`w+}|RM5bhSTazcxqwSHzDwwUy0 z*+lgM;PfHHM{VZd||uNE2^%6&F$!BYNTcBNp9KJA(a+ws&z zP=DDKJn7^Ld<0}Rx}~c8pv?13fUBI+k#r4F}KmO<>41Gp0+%EW)VB#%)_9c_vDZ@fKI{Ly~VEe-3z0}%JBi?i8EEi}fX=8XW>Ow@wgBljET~Fb>KFs7L_uF1N6J1eiTCkDh?Xj4ub|n^?STA9 zXeH3eg4y}8N9CG~Xi*(O_W^Z{x;g!_fkvjPr1wo)?hDnXY|)1IlpOZLsRc*nJ!MXL z-?z9P!gUia)+xVx5Le`EVUC~^BXpBOMusur!68v$gTkkYnZZC2Y67lFJ#bAJAmiwt z0piy)awA?Xett+2W!68ew@&qn50H zBp+WRoYV}&e!P$bB5hh99N>e+)G0m}C_a6@`WSdtk7$}$g9vLU z1n$#4-~hc=`f2VPpsY`kW7DU|F%zfAdboxqq|3Ue!{neTXPk`j3CuW8plz6|a~$^+ zzHG^F&>Iu@$fUdS?VT6h_RdzIZ0Vz-_J=m9(#AN*8rbZFz}E!GFts@uw>NX&1>1we zjzT_8{awOQJ+NYfQQD?qJ^A+wIvGK)!!&rT~2 z$u2TPC&pS&7R@S(N6%UGND@x$@m60uz3pKqrWIf3n5pbQE;!fAt03{U*O}AGs`Wg0 zY*7!2rMX!m%YugxMem8{AHOM44DWf*I&Uo^x5IKw(qUPRD{RJL*^GM*t{Pl>a3xp} zHi=S(-z0yDQ6G#YP|8V`2!^?7xQfPt9v6*+0Uob63KyL>hWgjCg`i^&!#J7V|I&E` zQuq&vd$dBX5I&VjI^;H}uj){>ZFUt%wdKu`#%ttgMmO35*~l#qQ%?LcV+(@)Dz=Z|)mWyJ6#y+$lft{``>rzv;-2p;vx- zpO3mEgNM!p^*9Fv&PVdziuPAckZeqQtg)hfGi226KLzcOL>n}&VXv@jPpLlrwuX}8 zWLWZ;1F1|V`9_C);3}RO)#-Q*Qn>E{Pm!#NzK0#?IOq8|Zfsk^7K_GRp}N5#madm8 z^eZiAo$N$SQ#7<^AwM^CrR8hq7U-Z-h;&CrX=u=-<4?$&^$Uvc+ugYI33-btnP
Ywd&1|A1l^^gL9j@TKB1lq%px1Be49fc}WCL_~Uf-#M#1YA{c1h1cmcg(Wb%c6+ z(LV=He9MPj*n~Zf5ZiY>`R#cjd_U=JQ`^u_^&5NkH)F>^_Ux~W|9bg;?ecfhSJ%Dr z_HJ#;s0Qt*kqsd^Yc0v(F)f?Iidvmk)CM2Sox*3<w%@`i^PM zUxlZ41YJ;f=+6IK8!d<1ayx=ADj9-*XD3?xVZrod%0W#`{^2R z*1IG%v{7UHYeaX4bs*LV)WcTXt8n>B>ihSt=My^vXMErL*q(Q+zggjYzCCowGsW}l zEOwxzx02^vZ_%9?o@W6r{0OA?X@@syz2#!VwZ+V+Hplo4-bagtxo+RZHrMPB3s zp-b4bp^h={Xa{G4uO>;=-iboY{+On*{0sKfCB`KqV3$3mSaKSQDR&>?+~jr=I*p5l|UV%{-(jAZpL#p>a`>>JG=}! zi$)v1d?U}Sg`HVWQyKDR*c+&w5cS5z0%=r`egSDi{KuvnL%7LgNj7X#zYSG9kSUF; zfCvATycL=lmfXy`4SXmN$}1q7ZQ$nsLv^>U3-ZFJ&xIKabIAwon+6>H>fA(D#fM_W zCp>3{-Pgh}#VssW)F2~$hZF}JkZY*2X{r+8Gi;SC9g(>qs>=Hszk(hZ(zMdKb+B_- zQ6Upd+3YIRqb>KUW3eqNCclMK>hXQ(7fgW-WAB(aevB&=k`u@umjL9@ zpjAl67}cUvWw!)#8~6&;Z2()X?);2hq+f^Yx+yN~YmC*d3|_ajYVvZI!_DZ5TC#$D zt--MQNY$LHQ>--W5nC2wBa6+Z9?QvTE>+$u3O%?BGA=cHAjdVp*Z)ZGC(!>`o+&0N zt4ja5S>8)>I?>r-{8f%1wAZiRb!e|3^h45>xwbfi{}K{CnS;I3qOL_ks{JT>#xs!W zSz!r2`mlAW71HrNUuYE4?0CVs!1@=ssPgpw{Jbwyerm9;eR|n^!xf8rkI7pN!0r$d z^EL@j)RbHL@mD)l)c##0X`|SxMlV&dalP7Edkk{Av}47N`6bATi6l!rnp)t}_qEFR zzfh}b_ao0WmY`!%v4vURx*~ZMYWY7(onP|5Q1S26g9y}@xHp_%Rl}rM99p42zfiy| zt=5i>$@yNVPaoZoDSV{k1aA8=;J+Ic+;?N+3X8F!d2mq6QPm3;jRW|qiP(z{q7%nL z;8ZY`+}1JBF4MB9sBPJ^SV+rRY`sq#d} z4X_80W!d5Q8r!*R2A03YNyt_!_mhD9Z6Rh8z@ zTOHWV5D&~6ly9;hq{@J%%Z$ReP zkbr&z8M~TZXY7bV2khsm`Lz8sAAz0)+*ike0ENaa4eanj-aQDk2*@|~&^*QvA9-o9|zsb6atq zX2L)>Fu!I$2K;BB_~h=wsD9lNc2qBbZlqsn1r?#b0VhLNk*z+0Cj&IJiJI`8(=Bc5 zNsV?#KH37U$Cv?g%WIy=`&AOUo>pw%F_39eD1IE}tG@MT+f!3lPWuuo$8yV>tn?na zU$^vJyU$;J>$f&rKFy`C#om!c@{Vrlp?1b#Q%0jLp4UJg-;DbbPYPB#axL}vzKrh1$_JeQ#9%`@o*VO*!##V73wFm$ z_8)|TLZEWHnVg~4!O+UP2{L1`c9%4-s|Y!;b~Vb39(nNmAZU5=m57+zx*7Xb(0riW z{S0~kEGM+NORxjP`}Lkg=smckZ@ZcO6H&4hJh$U{IQ)-9$y>Tc;eTwG3-+u>+&Ho@ zy)@R!q%S*v$F7XxWRWosp6ZegcMEp(Gt^cS%8-JuF6<@H0%~Tz8umq^^3qFwl~n#3 zw_5IQ$z{V`qxg4R$Fb^0;syjbm7mKk#^87KbNSrjcKNMLIiP1Hy3pksw z{Z3=78h7gZG|skS4tN#UYq-|Yd@$^hd`u5n74*z3nnJSoB->hoaYy4%yz0`NAw%9f zqF3*~Q`Wdm>WXp3$LH)fjC}(LK02j|OQ zqzm05_N#jI?E92a9iFcfD(p;Rw=@$j_dstzXZ}T+@t#-O+6CML?k9&Hws*21oqZ1a z7LO^qIUead50#JRH#%3>`1jKk|GyE-oIdgAA$}I|C&T3$fBZ>^zw{5{ul?Ql*CUp+ z-^5S(m%BtM$RB?zYaTgt%qfB9ptMC|80RQo^o)P6JUo!o^LP=mf^B$=yT@$1odh^|5P zuR%PU`oynO;%D*w@BaAbA^!b;5Puua%~y_}eDPm5LfO%tr$k_WomVU5*e30WC zxnqn5mt&?fT=kSPZ;H}4-RXrVi!CDa@X4ZhpT^X;1e` zC43;3cVQO~6ryxcJBQ$|Cz_Tjsa%d;a9Fk-1RbfqT#ljVZ}HqzF88NrHWus2Bs{H% zoJA`Oo>s0sEbA)Ds4mvTc(NXN00T-rw=gqEm11% z{(g}t-QB&XW5s#KcvrBGey?!($uoX*zJ^?oE&@EtAk4^K>1*$Gf(<;qf7hb_fU6kVk7p~_ zLK2vH-Xop$66OY-NoQgn@=8NI(?mn?Du4TVyJmuCalR8Z9x8$!wMO|)A++x5APPmc znYE2fsjY9*Ch40)B9cqym&7}d)iN97Ed!jeLBz7k zNiK?3NiK6Ro%U3~CKSBBu+V>`u*T$)`$|}!9ir!a`7vy+5e9Anf#4B@gRTWDXhw0AeC?AvEMF_FM(r{9M!UtfGo|Y=(}e`= zY8NDS3W>J1wo_LWml-WusCH)BX*(0t&Vb5DTObbUasy}rh)y&#^o`_6tSs;F*KgSl{xosnYu! z)>PlGv3kVk6m#UJE9VbdgH73ve}{PsJMC@j{m?CwvDx<2qU6@}>r%a8ZHK-yQt8<_ z%dD^`fmFY?o9wy;va=c~9hFES4pwU5s~9-w%f1+B-$kj+bA779Ukp*1U-ZcfZvmcC zAhTJY5`A0FtwBpVo&zy{6SEYrsg zZ1CsYxdYILy>Y6*c$U4_pNTOdeX^u;2|n#2 zMnB$$md;TbR$4SgmL7(Uv9Pl%x|T4ZrahQb{tRX(OAzfWC#}MdN40ULTiVjRpXuV^%JA#viv6-$oT)j5)Sh^>hcl4PrI~1tx%D-qc;X;yGNVNW zKF;)WA8)#;>HepjCZU;RG{{Rl10D^@|EIK4{OsoXItI5o%C`HTVy8Rd`DH3Rzr@1x zi^=AF>iRrNHFd#Je(ojT<8>i$(|X z9+7kY!O(kH&)3hVx=07_V7!nloQ8xzWw^tfdzAQ>?G`8AlOX{H+ow3kksK!Fn|T=L zY_YMJZDLI9Ig(n`JfX@KnwjjdPc9fmyHyv*!86pwhja?lV^h93(bH%?(W5Y)=uuj; z9=`vvE_=)q+A(yK_P1mnc7aT5;Xl-uls4J+w{+Y_#M5mqozP>R$P!X)pO&6ipX_la zob0iTARlkLAN%I9Nsr(6I9XO42u-S2FOumqOHcH~8y*1KAGLe&Y**^!BJ6VG3 zfzX5CA&}k<*?F>HjSs`w@EFL7Mj2A=G+HI|XC7qq?2qJ4XutSAv?cN=;FR*vexAGd z#r>0EFC$)f>F5reX~Ln*K6@P}K6I62eygpQTEWR?C_Wh+=5AH|w zS(xnDSQm!8uOI#q;13SBNP5^>T-ZOqejJ_?`Wd~3cqgtf;HwjQGt-DK#%2h~`1g%KL3dHsH8EB_uO+p;fX7vBludMcm?heb5(hvCO1qGhB zhvA!#yn`T3eE&!OCAB!epnqfwz=>xi!v3NDh7XThtQ2b3N6+uyh5pqqLf@wMz7C~iq|?z;4o2p< z7}|w2g-Amwb`7QAV3UbX*u-o6p3aO7E73L`=52VshUHVfgFd7fs_u&|#N18&E))|M zL$@K6q0}*bYUucRP;Sk7x8p^;p&rRwEd_b2Bn zDy`kRhe+Dsd+0}REP837cRhcpdTBbi`%k{jZt|>^dCBj2WB20~S3)g)xI-;L{a0Bp zG|6!rATM_orDKnvTUVB>4XegCJ+&CO{z_;?7qP%;zk{51LBB%yE#~5_Ff&Dc?kr#@ z68f~?SqIB#c>7EL%DUsYvm;9UuT3QjtnlJmg4E@Gjj-=$(rR zvR6Wn_N^~U$2po2Z>X5pjecxtTS=`nqLt6~5r5%pXirAHMk(P*MaAj`73^qNL$mMh zbE~0|%uHV2G~728a^2s@dVjGv+N+b!TZ&DN<%=Qh%}?_!FPc@#7*a}Bl`Jh zq0xQjBD2(rV@(f&3(5+)7^`o>6Qn-9<}>40M>w-64YCI;(nrG9?&`$MMPIK;J4jX_ zNlwb>ki%{aS&gLDYIcwnG1%q8T?nAzVQ~M!bB^9fz>CXJE3+_u)Y7Y=%l%_$^@1KH z`11V*Hg|x3>=~SD+r5mwQm$hXaBuxFV0u3S$-y(gJ0uHFa_?JqG5SK}BYaEyBgw^+ zd_3{|w>`^3{GU@<5a?XOdS5F{FL@0bMOQ*5=tcY+dJ*41tF@$j({=h3ovv3TeTr9# zUqRnTL!;tlJQKsSN?*b=KAi11gM9C}=uYQ_o6k88Vpjg)dJd(^H`TzmBEN}qL)PEC zVL{sZjrjJF%W0J1wqEIWF3{~(Kd;zi*a`iK-O!&vknK*+-S~t4gqh0AYWg+lPne6K zKSA0X>qvh>`f^v(y@lz!pg+NpcO256SSW0V2E{<=;6OHTDb+KqKXEBENzzWB694I2 zS4g`RmH9=pvc_IG0RFO~<_Ijh5o+^~~@jeg8q0p<-sRzUOC+awp3wUJH+ z!R4@C2Epa9ZpN@)hI40BFXIH@lVJ77`WeHz83d=vkRd1i41#-t^Wia@*z~9b7xmUG zz{NS`t=Hh5G;7F@->`?pyBsd`;xIRAZAeTBjT}_M*gRr5g28 zjfs-xPPSw0P>tW?`{P43UPO&0SNBUb{;6{m>Z*VYqYBS|_`eGo2Z3`f@($vkek$=# zVecHW?u4)Y@8Dq=76`YIUJt>;5x|Ij1^B1HYbu1{yuA>h`K>Xq;se*@ zZ;Agehg_vBVCA0MznYP4lDl^*i@T~gXn(tJZDPD0<%*_py|AcPI1i8i8{gg*C9@bt1@US_&R) z$rlP?^NnuwzjbjQX3L3ynq8i@(Ygbk)ny*YYOEq!V9?2UJ`QNOPXj96o4OrxAV%b| zh+jGx??!8*_yxYdZ7n!;XrR02qku-f6IXig`D9)hTDb@QY#EPifW1u)J8mBrOgqJR zVZXJ~E3B~=6ihsI0#^S(H<;mxT@M?1dU#?tFE03-ka|iJ;MIlT;_t^jop87^}?3de^67b&mgQ8n409$D)8ahS)ia5A8gS;WmwWwfX04eW?xc5c832LvgcAW( z%t}EOkg207EW#7gI9PfvLNonjfREw4D}~jz6mSA4MFt@Fmr{znkR3Cbf!MSzy?SQD zTf(h=_J(=Bg%~l;`&08c12-*ORC63OOOWiGJRdW-Ak1Slf4X1KOZ|F9@T!!K1Zmze z=V?}03A%j#A)ypICJTG3wC)Hp>06;x+D)B>fEKRir{SernyJWTcBu79QtMlUuoS=H z+)8O)Q@Zr3nGMatlYZ)T3!oCvdTXZ)kS7C< z;JwuaWtShWWxTDh9!)EdEGh3Qn9$OMRfti*ow>9E^}rhD9R(i>DJ|*%558^`vPKN8 z!jZmJLcRcK7+QmXg`qV_c{-XI?^&Ct;MC9>oQHOy?4fpHe23cgcYm~Jm@V||Vk^C2PylI@)AA5r~QLcFOIs zp*#G_B&K_+>%zqZkl;D2iJinwWXH2cxVIU{j$zZ-6m~Q_l1*f9U=!GQHkOTH)vS`0 zvmDF9pxGcZ!1OU8<`UDx1ex!dZG+?xAHIK$%Yq9Grdi5!Zs^$} zJ^v;B{bDHYhdlIl8nI2xKbX&@ub=);nopR&GJj@HG9O|)yn#K=)H6q!KQV`Ke5hmg zF?*Ta%ueP#=3RW-&b-CE$!xu@1nKcql#chydCM5;8~6McknDlnr%p1f?z?&LQo-GoB+NJ!e)dO_&pQppTxTzVG)8E zArnD|@D0j1gV2bu17Sbns`0EucmTnIkcpr}XhZ#HsI3Tl5VjyZim(L1f{=xvLb!;w zRU$YMEC^A`V%@r{Q z7FH$`vC}4e5XBYOFL>g|#X|~DCTd`ZC|0_!Oz{1+N3;B7@P3$aj%iJkbHTD(m`wR4 z%SZzxl{!L)!^B-9Sp<4sM_Zy`q95ahH}_XS(hxKs^dESZs{()H_0+0#0W#C~=S*R` zMdyW!qr@r9cR%{yid7%||Kl$|^}F>)xua=ph&{`2Ug8!ZsF#CNtZ;MQadw5*ST_$g z$eH}-v2W9U617l8-|tLMao-Zo&wDT@f{~)RBhI{|A3nm;`FFZw}5Z7LP%*= zP5!t+6L?W04?H6yi|LAhRhHuZs6i1}s6cAX*wW^Pw}WOm-eu}+KFIPNEh_~jEML8K zh2+at0@k-&VQ?zx8&G;>ptist6OC8Mb`>MGI~n~3;aTA=;qEDq_zI>x6y!Ct)E;ZJcV`;(7qq?~V+505lxOH*F&qJ1fE&c`|4JVoIy>s_<&&eD}A z|DB3b{C%r}ou0CJLaBL5zmLY6mBx6HpL%$+FXhcAFbhwYnxJd?t8&1Jc$ZW*&4Jnk z`MPddIZACx@v^xo?y*uS(}bzBRoQu$AJ~`TO%b3IH!Y<~Db3o=rO3ysC&e^V90yHa zMIfIWRcf9Sjqhnu)+|d^WivfXH`p$ZTj$1kBT@J`$jFvk@@>?jvfhGy8%ds23yL`o z)S;MFPJPbn_0_tB_xuk+H%jgX2P>C%o0QfDIkclF2BY^vDRz8T+%Z7&EFVbmrY5IU zja|b`hD~6Xd5T_;H&Lrbm9l2@ls?}df>Sva{3LHHPw|>>Qn?x2L!nMJU`(zE6v!F< z*)JiVWrEEz@5cePs@g`}6ibh%A{q9CG_WVm8$Y~mw%Vl`o+-vFsLilS>}Fn5)fhmZ zP^+H0s;eYen^U3mFd0lX&S4y#wc-V>+tBsZBg#!NZrzrZ0_-=6{=sqyF5fMuIx>JM zY6Qc2?Nr&cDn@Gt$L@vp(AI&p9qBt?#@)45szD|`+LtQTu(2HV;l$Yv`7EljygUs) z&KtW&xU}Aw8V+YMO?A3=L~5$-!QQfxa_h<9&p2oIH%@2jxUB5~hGTkUF8GKrJYz|y zQh{p!uwWrz-zxk)S>qjRJ77)3FG!S{_|}sFwfb`E$$(0w^&W)X*Ofvldb%}E0IjfhFDVZ}EGQ#aFGt6CRQ9>ly7iMBnbJjR<}w-m84$d?<5jzdN{ zGey<96n73ZlBGkjqsnPMRIRfFs}%phP`w(dmSsr!bwnwR2wRRZpK#hSOZcZf7C2#F zWWTksTfc!3TPu{+vw#Vv^f3nTn(_%7cZ6KH7r56KE3LG*opnOt;?{6dBW%9FL%DGC z{330aLh*(bc8CC(#)E$ho_R2%dG2A;=E{<`Sf+hLc~^9dF0(5Z6?&kIzl zMNFws0V946zwsjhxk<2i^3OZBrjf4)naG3*YY?Vy54}G~?-bt-Suri{Nq81@9vxp~ zv``Di4UOpKD=TqcX2otiXW{uBo=S{s*OfD!@Znj-j!*HY!lDEDyJ&}hvsf>+<&k<8 zt%$3PM;qb_sE@DuV{k$+J8M$E>5MOA$2(Ky{Y-a4Rexv7>rtFo*8AAr`i|(mE5kHK zB9RO8XpJz8n1=wuwv9sG*8>p=`@v$J@gIKP3rL2putog0^TVD*4&FBLe%_g|DcYvP zMeU(uk=C#czAG$Zb;W^eZJi~J8nu(Yj;b7uwvH;818t1AgpY(MzE5nl)TGt?P0es z4^I&1FfvtNi2bj?{Y2c4)BJz+`~Ot`A1`^zn4Wak&l1=v7s5W5rN>x7V>%yJiW$u) zk&V{`Sxs@*Jqpf8{zBSe@0OjS3__i!iJ(UARR#X0+8$i-h=eO8dCvCGi&tR1IzKJ; z)`e+ux0TW{-CfSNFF8lxo}>F8<;#j-2N0Mnif@14!D1Dkbdj&rMT=<^X-z!Vc{e=x zP1sAnKSOKSxZB%PY6=R~ZMgH#+a8?9r%bMH(C50rf&OkcEVr7N>|j$7=B21K3;WYx z`5dVh-ksi&;+chfWFh^Bd>fJP6y-}}CIy>PszwS^e|_pvvTKgORVIlO)FO1y;fvD+i)$X9b?Lc9!6&!`fMdC#|8vfko{KPwH;AigB+L zQg$XSOxw+fV+M7iQXv=edS~xZOtFabUF>vtRNjnHkSHL9dun@hO|)TMYtaMH_1?XHe_rq7d$7+dM~m??Y42p81I=`TBdpmTD!D?imzR*&d*_~UH_SRe zTGVmyc;dRx$lTD}pm8U)60ARQv&KCdkY3z%uafh@|0Z%y8endKrih~Zyv9pqxZz2I zXFt3~OKV!knZQAEF^1_TzRJ=Qa21PdqHBOMdR%Jbq&)^Bjmq%+MMBH!L806U z|84bhFBe?#sC@Ih4yEUl;CRks%?n-Hje3l~SoBFSUd;zS8KT&}>Q{O{!Jdg2{*2VV z7FfD`0p|cyJ!{%xOY#-M7r^a|t3c;dL+4fv`Bs$)6bIMaO0?#AiOMbRTB!NfUnr=u zK$ED;&Zl@Ip+ofx!Fl=oKS{>U_6y;Q&a>@Q{}$lUvfi1s(YB{n54V0(XR2p*UHXn0 zpaVomztJAs-k0W80Mp&In|*s3FdfryiIb^X5t-Z5t3@u9*u|6zUYjE zZt5edsXeqZOl{J5>+2pZvIvR5btAC9j4%>3U+JS2N8bTg#;I5f#-j#fYBfR0mmR6q z7tn%e{r_&_QP(+129S0)EoS9RliUk#5pZjo{9y2(4}R97^ekVWcrF-h(^r`c|TIE#h;|W!T-fUvefs*p;~aL5%V6Dn=h(*x?@mO1!K$q4u>7 zR>BvN&?oQD$Ljw4KckG~mxXZh0|Xsd1<|&Y_5OArwGeyvzR&!;XKa@&euM8Z|NX$h z_t9>s`MwG)eG38$<@<-QlUMQtDuaodnhd*UU>VouL zjHnotqmF?+V?fPE^7DJ!pn*tG7YQZ4ACJ+X5zGrcw(nqYO<7)O(!OUY|1`35&U;cV zeAVbq{hZ;L7>nE;-%0kQ8D_~W!mog>2ExB>5%WcPUluFagY!lyBMq;r1~PL+)1L4^GS{e8^D zV}fQNh0(T|Z`^H7X?;3f?)$7DvpK`38PKj_Cl1x3c)WBJEJl3OG)m+Y@V+rc+wk)V zw@%obw%giq{)zNaK44nrr7i}Ftqbdvb!*2akTm71&sfj3Vw8SY=k%4QUzt7UxGu0~VHZ zt=~Z3g*C_ebVctwQXDyH1tKdq3+l96>?w8z=LuFyMdNIK_+Y@ zu^f}O+^({J;r|FQp@V#bk8I)7zezXyh-ae(Ho)vg;8)G*zG_ZEjPe~&$2FSKv9 zci1h&3!2A>fBm3j{&Ecu@78WL7o5r488+aA1)Et5Dy;dw+Z-7p$8NI9S|AYuo%yql zF8gleea+rVP=WmKeGpN~Y)WeNqMWpVV~4|FJ87@Bn~KmHCe7ind#o?p|9UzLI5GvR zW@6++e@gNK$0thlnHKcJOMPHk>>KS4wEUodxDU1dBvVo|7hH`N9riDDyrX0F+bpvD zh4xh8n3QVq+Ss+<*!M_%f8D>r#5X6#aKT?A{Z{|MOS{VW=F|B78~lFFZ|PpKq{7NK zyR}^KNyNP5zoLIS^9AtnX}&(Afces5-el{G65Li;B?~4gOlK3sajXK~iWPy1xJj@% zX*9hwv_OVKX>xG=+8$#*Zi4b4=QJn!y{Gx_0hvrb+W;9oe%dr! zlAW*Pr@7O-x>0S6kz30c^Vf>UA1(;DJOpo;I(})GU;U!h7Uu8hYksgRj!U!|Ng_1Y zt3z6o-zOfQCY<*Zzi#9H=LH$KCh|%~EFRz-$wE5rXjyS5tPjDOWwNJ~sWfMUf<9&u z-*9ocz!GYSmuDin!3@d8y*D3RBi-LR34?=ER@$>>qF}5W4eGfEU;3M7;^v8=^LKw^ z9dx>K7}4SSHf>EY-~!v4)+x1iv)RXm>nuf5_I| zv^Bap6x-W!pLMy#WV;bO57tBVrrOvls{6wxQyM!*X<91qt*bE=Y>;L9&&#VBaUFcO zyldrxNgQ9Pa9g_FpxzkOeDFgU?>6BiI5uAibA|Xe{S9k-K< z4jimz*t$4Axdri)?;pFWpsR@sXtW;{be^_LW21J%J6R(uj_rr9y+*Z{$!cl3TPP^# zY~BWseYmmONIVYSQ{1(aOI&Laio5btK$i~6yo~`hGuE-+%Jft_8@oSnG81K9*5ifT zUeS4aTaCB-&u&evMT);TWu8-zxM#!#E)IW3yIB04?vhvYHH?@%m_$-G_9c3&ANO1q zSaAivlvnc;U&n28bO!(4S9%b0Te<{%u$lWx@OQ>O8o^9|fep~AWyGz6e+lXp`>oFI zb|+uWLLc_YtI-y*({Kj`$`?`sQhjY#sJ`v`M0<3e+41$IN7GaMOwGDA_a+let@oL; z=L&oicAGr2X~5UB<{&(4-iY3>N(CIKKkZ~Q@2!)0&pBj(glC)_{yyu(xlVlFX%lsP zk{7-|1m2ryS9-w_!gC0s#((0sWP_icmLZ^xB6gXq8NNHmDr#0GEQQ}ab|U0uS!F*P zOt5Nv^;S#IHuUAKu~Gj8WPgX7$a*sbqYb%18^m0-`PtX1JEy1e;Ik3D+y^N}zA`P1 zb#p>eF8n@D%=K?cr?qo$XQFV2UkQ(;4~%7QSl+};=3Dvd4D9sJhn0fDb7#V`uu;#8 z<9PBVnrNMb7A7~$7iG#w=wF!c$J16jWSOkH+|PLG*BnpoUULID4tc$|*3x?GUc+Sf zH!bNL=><%3Bi$oGZRHWw4s0Iq*A~jB&p$9U6yN!|d8sYis@R7K+>DgVr%V zLuMTDbHYiLw>b(0b}A#j3d)|Z(VAnxqx7U#F<*yn?khwLBkmgbJgkFux6Z5zCaKQ& zA4yn=QC6UTuxCarxJxFxZ&SWfb?m7rcEvBRE3g-CMg!vnxZ8! z!IGomJm*MbA+yh?=kxGTUupeA3uA=Dz+BjCo7oLqyNx_o;&(T0OHzc_{gX`4Wg9Am zxc#5{*e#qFG)Vf%pkAWB^`L`eEHn;{e%2`0e})|-S}QQXj4^iNbiL7Z-p^CtQH0!q zoXUF~ihgBqQvQT~?ox)G_AFaL@99^tpVAtY=$_#}g(dS}!u~q^llCt*7$YB*Qvv<3 zi7hi=FMtXD(YX^gEDt&v@i+a4FI_1!oH|x~QKbwta_1dez>hfQy3_HcgZ4wCZI$C) zq8ViWv73dL?W;hW-QBN(W?oePSclv2S5eOLe%<)Y3(w%pZ!C zfQ!!@Z&?*h+BPQJ*;rb|LJE5k&w=x5mVL*pF=dD6DlBL8?k+ z`7Erpl0@pdm z7PR9A*DBZ?<77+I;wN>trVD2spu7G7N>vTJRH`wylVQHnET9hk3Do-3+8fvY8TQ-$ z3=5Wj4)f0SOJIYgZ@r(%)C*c;u01+pPiN%mAMw+;e(6{$J%KV7k9|+KN6cNO2_Ch|11)ku1uEuqwp^~iyQ|!i9Spbxp-|*r7i0{UOTys$Ev?; z{dUNx%glMbkJZlZOczpZOC9G3^6nsOXiHIM0A~u^7KZ0^u0~%Lb$Zk@rFQP}AF~z| z<@Lty+u~R!weem*XjEr)!IMoo)UyMn!=jwIx9JIov5V)Pu;0aOvF2U=q?Um^>R)P& z)YxZrZ`l71_q-)Ri7~)(UskPeb zC2X_Gex>tD#?$(Lcd)qaVKk>ZF*5M^3V&{WfLYzQod={d$JvXtz^cVk_O!y3-iF@|IT z`os2;;~54%1D=uwFWx1?Z4udXDmjiDyng=j!(+09DhBWfIvD%}pUwWP{VV^}x|0tq zbdaxG74|HH?#K0uI8Q?ZZGFXlHkcI0OoR@oj{W)b2h>^_tQJ8cGgy6q4J76>0dC$0 z)|;_=6?Y}(2hPu_d)x7QtMV)#cvi;#Z28&=YYA_N$JfF-1FT|#;_~2*&9I&iYT4*r z*c#uAzanU$ImAhepyP0f?{2KOZVd55JQYomm z&Va9KCSx`8MnRPFry03B!W!v*y)@duAG zyw0c-%Zm91T=(Sa#1eXAdAwQh=452KOqTMTmEq3liGN{)d|7(Q{A5_$*`aNgAx?fcj{fR%b{QBk_DgnnpD(MTw`HpH_pXS0$U)qU6u4Rs4eTB}yr1j))OsPWB! zMDccqJfKjmbyOEXFLf&H|6RqISfdF%A9J>Vt(2cy4sHrjFW!QIW_2)0tqwfP@xdfn zO@S=%jP|xupNCcZ;f>ts#XNyC_QbgU=dKbpG0rz%_yzJZZ3Wci>><`BL0Ylm8U@PKlFow}ib4UEVK%--D5e82ru zcs1uW`{1cuwIANfoy5(GXPMWA)AWum-FN(f*A9NIBpORhQA=~M6#P?i~OSabyI02+TptmnseZ)xX6Mo`9q!>$-*wgIORf{Um>*kV-`Z!88k52j51wS z2p{{|3`+Oypr!kv46^Yt4>K$mj@-rBKS0~exEDg(=Etuu?pl|!(NS!BDt)UpPDlbT zYJmgiWoD~2x@t(T{%?bHipLzY>Q=`12FYgR78O8FH2|9(feLoodIiqXT%bHINgO9L z_)Y}%YQlu+j+cbZmW5qBPx8#5VvF7=$n015cQ=0)=C{!qo$(z{Zw@aqKkQ&{r2fj+ zMth9DKF7~O4$~V2$a_OybHFcKAG7{cYGRX@Niru*O|j_?anOLShK(GgTMQma&Sq=K zr#{W3|c#(9*E&HwvQx{QV2H-DS|m26fB2)v7TAn!_=xII@`qX7IyU$0-!@kKF~ z?ZG9FFeX_mqfY@&SZz%knP@`$;m3->+)gvKHk=IRa#3HOENlyx%!f@0gSDRv0)t3A z?;lDe*oKW_j06wa7L34?aT07+3^w8fp~=d%zZ>M)(dS>XHU?u9GaGIRtI{X-D}XCT zGSeG^7AE64%gtih@_AiK8GIWg((NR{_)Av9sr|ti?Ylvhit=Rn8iS@dTY<7)3q8)b z%dljxq|&t7`N&N0;;71_3~b=Gbx*VC?8)mRp}{L^@N{gtWIMQK*z?#OxO*66y{sWg(3@wk zePS(>)9!m0H(x3bXG^prb#MlAEj51WS;oVGccIBYX<^;JTl*LP@C;!5^DDsbTaPmV z^snLfDi#=z-AVS)30f5bSsafSwqDdQBT_#}g+2O2k^+S6{L)y!{3J-e%n`l{wGK*D ze8qbK?zUn9XT_m^C5MvM%K-oEteX?Kzy_ZE*>)=vcvhi3#g4;@TfN1Kl?jgI_SBJc zqWs8Y+p|_adhVR_taYHdM3QM9E{|Kk5d2detQ;iqI#2@@+!m{z)~=SHlU$e7hPRv- zk8ZK{pN_8C_*UYvYJDp}k??_XPT$XVzi279e(f7x;S_t`&*(d)HtP;eg}*nIFQEv0 z{Sh9ks+k|c1j$EbLvL&_GEZH^UTVXA-uErw{f}c$ar%H?UbMx^`lzI+H1$`&k&rQNi>5K)X$~59u1lVT|PHEcWuFtd&ii|A1!O*V-wMlg6qAu-Zghkuo z_rTD_$wqX2HiB=U9K5*~xRPOIU99Xte|h(b;5Zo*IK@ys_?JD`XMxq6MC>Etq47;z zwy}kG#5(p^$2IY@&9HIH&KM8enj|{&CO0X^#Eyw=!=5=O&apknPCsbP1dmdKlxaRn zF&6nJVSUQMr4BT6rLY4x8?<}Vy1GZ|AmNY)-fA($T=pfWktGSjo;rcvlBK_lz6WCx z?RoUC?l94Ok&qF0TaG&HrIRIo_Uz2;#lM`*REn^|E%+Kbs1weFvvteA6C!c8Jfp;g@eR3e(M%qAR6_B7AwF@VP z#zA#hp=ZQ1VODMi+lqNTB zOF)gGG%ZRS>lhC%eXSMR??E0^63_FPGcG$BnkOP`37l=x2^anLG)n*99|XDh6dPyDXo(XZ6~>Ii zp7#Vlmz!J==F7s(wP<~&v|kx*88#VCz{}ueL1RPDi#Ay?j>Xg}@|#Y7gZ_tw@2-QX zag$~qHZ=Rf=M8kK@!(XGVXlC63oSgkD16L#_=gW?E~%bRwn9c3iISZOiwtBtgb}BP zA$JZtYILK@h$D{BciQuj;u_4#ilVo(pEgAATW20tErNW3B)e|34}{7h`RR;r=5g}7 zFyJ%UDKFVU7>0T1J6?Hm(r@ZwOZV38vBvp86SZxDwHJ~yA}fqoQ^m|=v1w!} zQJ_qY3U?r8%pt{V1ZMKrO+oPCDQ39*5|sB2sKL0>o9hmNQync)ajnFxRDuDVRZ#A? zO(EpI>>tic&yvGTwg=YebIv7!PhQ+BdxY7C52<&04pT zq?l$GrN4_kb|rX&T@|D$^DEg!?jqMD{iQehe@qsO-@R-C{x-{=_@Dp7>8^jiD7%2v z6r%q>m7X^Y{i3+V()+#F#SEvt$dz*@v>s$>F3K- z)l&uIF7m3Z=wAZMf=X|NKN=?!2Vr#scPXwqfkr%0w8Y0i+Y|lHO%~=!9AwSb2mKni z(p#;t1pieY2bm#06x9oHRX9Z(HVdDJZQC$zdZOC7b4rgvbjp-fE4$P( zi}+xLM9YhhZlK$mhb!p*=ER0sxY=ozS-TcIT-?>ds8FU0=RI>S?$x$$VC&sL-_nv*jspkq*p8vc_C=h&t2Xg7E02`ys{ye;Se`eK3uodPk+p5dS#QjP8b|WN2bgEUj1JYOW zZMqwEIYV=htDB}Yjjzgd509hRS~%3oH>Fm-T}!Dk2cAr?|GXO~&)q}yM)TZlS6`dC zXQdo3)V5+~V$cR{&5PDO4c_F{$uaJFj9yvqBeje%(>+)Cq9a96R$JgFN?AFk^hyV< zfJ9Lb>A;z;pIQTq@yX6Hr742R(AhDj^tSYD++ue;_ioV4sIy`)1B@o`^0~d(eOHq{ z2?JTO=(kHP3%k00_qkL)t)6Pdx-YJkPhOjhYVZn45l{H5tv1$J*r{>9 zS48uasghy0{c!WCQ2piZ>s z!wqqsL?O+=Pm8N6?)v(-sn7+7<|J9&;yh=I#T;!j>~F0PEP%I|;a;_31eG-pUgYc+}#mZqpa4r%|?xb#(Pdt z>3EAMWB93{B={XnDpz_}VJ>>gcz^fn`HeZ}y+Rb#)1t7;*hlsfOyziVYiWk($PBZEcnm&4V18F++S&=Sm_Il(=` zJz=+c6TG%>(z&NYUjGpINShJ1Avh6A5GoPkJQ{bR17*hO;GO8f{jgK4rX76cGla!- z)s5X<8*URwn_>;DNQlqo}^U1s({3aVGDc{UF!+_>Qpa@tA*|AS*wH>jp3dT$@g_kkD7T;jcSek(^LkF%VLjJ}ge(ltGz~!I8PV?H7IR=zNdG18|M3-uMiie%ywvF38 zT+%wb(OwpMea|pdz#cNjY7NGUAYx-XV_MXNm84wo1;K&Y2q?Jm2PlYyYU+n`&XscJ z4dq;O4bIF`eC|+uBow-q!zg{3aP7-L{r~KGm3O0ch1XiAk#fZDrnb!$=C*$xp1w`@ zdL;CV`g&nhHT=>i=69(dDeg*1fFCc}3&7D8Yhplw(SGrL(`wLm5EI49gMa1vF7 zo{QnD(560;dZzIXV7v!XTalhdoyNT4eX~jD&Fj5sA3G;i(7E4FH5+5AKq1eKgdTf8 zu7)r(?1D_Cy(AKvg%}olP4qW?i-c}?f4FUOsckBZ7PU@?IUM8F*eV6WE&`}_pECbU zBH3P%pHAJ1xgCR(skSxKgBhI`rTEZe=iTGNve53@pPI>GeDfzs;`{xb!!w!bNv>w6jjpK}qBS$@cB$T3sCSQ_XqvoBOfA;G zN5Y!M=$z2Xy%W$s{@Q$Ol*)YtDxpR0lo|eA2s7B0fXUA~-!&3ZX*K+u;eYsou z;u!h@zvJ&;4V}j7vWTtDLkkZVJu46e0Qsm&Wi^+f7oCiHld=-~RGSWaBJCKF(DtLC zHq>NeBFWu?JLZvY(0E)NX8O3QL5Bk7W;^OM5lVA$?89*K>*(P*%4qR9_&S zL{%LRPu8bGx8SKG!!bD1+5s6yQiL(^0{`Zb>&u+I`#>Ld|J>rPi@$}VHPB-4FaQ2e z;g7sfN3Cl5x7Wt%BJAds^#*oCNAZvCncm<1ANH$3r)ql6(L`_BQM4Dkm3$X=t7_V< zVgd{cYJQV}W)fJT*M&7G`<0b2iqfUaxe(s6$cl=ZExJ$5APg{-a%Q?u zV8yMl2V2Q6l5rQ-ztpZbcg&_!n77kE-dhJfh`ip{cMQ+wM)+;0JQAg5ZwGB235`6W zt&ZDdv1%(*(EphC$Ai*(AJQeUZbsbgoNtmh5fzfua2i+xCpm9^uk(=vNY@>N6{94t z+Wn^n++Z5HAhR&Xcu!6|avgOFpdkL}jP=U&1>pF%2!KiT7--6}GTElFJ2G~1m1>+{ z`AsUw3wrwR&CylLeDiZG;vQ&Yp96LNdza43LEl$hy%c;=q6`SuK=rvYU`0jIV$N8V zo>7%jLcjW&bsleOqC3_*-!uj?D*-1_6>`@-IU~@PyIhIh5m?XAc6QM!VS>-QNY^*E zDjpJj7V#6O+MVE)=vQ|_RV1|f$nZ#A#>w{S-ahbIXr)9#XY02B4_6~x!^zc_jKR?C zK}d`;pWya@XqRVQ7RX8@nc<(aVYs9wsiZR~Y0OqC>vCtYsEE1EPc+=s(8E`VhKsG_ zF#4(2_O(ArdEVa_oyo*4Y%UAyJGz?TvvGobg*`9y+8!BB=V;pzPA`qG>VL<#tFJ}S zRYu1%7Khfd-WB_Furp}}h1u>LDTr<`_%{wdv*XlS^I>Kc)X& zYiw{MEIqDSSU$i_IXn79pf)Zp@QQ3ZEE_x`n5U^t*jIACKd%8F=8pb~tFX%SZ6$Ov zz0TO+)3O-wFmVP9R9F!o=xZ|WUY$zuiMDaO0FRy&H=7Nfx%-!`&^I=Kx8WX9MWb@! z8W#C8PESwcOfwsBLF<+21XW;a#;u}EmFWE#oF`V5j}j=?d!3BQ+)C&2(V(TyDU;5O z@I>L*)0yFYs`H1H;=`Dh^(OB71a#mSceQN|O2zuSR#JJVIoE7>Ib4R*?$dhx?)M~Z zrf4mXc2W&vBx)`742yZZzmv{cEVrd;1XdWeWL;<6)-2%*aB_GvVTRjF<0?P6+zs8+ z$n2}3?~a2@1Nxp)F78x$w2-PM-tyJZFF(NOa3$bjH~vuL@7pv$+QbdcyS#<5obsZ@GLwkL)2I68;G; zF62GyA!oB>T>r+N;;ytJZa`&-8Bq0bkW{ET@J{Nu{)&NJ2Mo4P10!vGz+kI_Tm*0Z zvH;RIrsA$l8>AVm=V2jjRhTnxJj!k{j`I{~PvIVUZqdQPUDG8p-J z__T5@4-54RF^f~qF~(bPBdfwVgUfF+IVumRQb%ZO4xE6Dk2RSK7|;Uf>RIaw!2L&U z#(7<*s8^c6bKLKarjDyBh@soP$=0K)IX|vvBBb7>TgO)!{m>KsaUlQsKn?r>lZE^7 zTj$kTA7G7c%SQV``(SAQAY(M%Mc#oJQ;qO5w9aIIw@yl3k21 zLw|w>9+PGAWqMf5)&#&H{5Cp0g~@hyW{Sl3w06folIcmb!Pxwdg8nydn-2X=XUS17-8NQ<{<(3M;0zJUi@Ji?C ztq=B=-&tVW3X1@*l|Q{u-aR6JS(>_E)+h5A`Z;j{w5ZxbPWZ!RZ+t5leh7Ho@K{k> zXx5ecBt6$Q_!C$^^fvX10yi7+LK}RH+%pu{BI)o_U)w?%_#TfwKNy}RK<~o~)(zQB`C;gLGr>1qWJyJi z5ozqMba`ws_^x&vylRuFS&nnrZ=U7n7~ITwgI6{Oa{Ar3JbU3{qWBN)LvFw9KE=Eu z+rm$J60v(EkqsbWssFFg8O#^UmI7Zo&BD9gks0%PEeSMI<^I`)GyEsh3vmOu_Ab3( z*fbcbf}e#*uEC>p!>)o6Ms=$6cR@#2YK;$?mvNA^{mhv?tm*rLQTkE^&2`zmMDs+) z<)N!>p$=GeXbYXcLa}Y3o30zR4Fk$IRBviv-G!ZUV+rYGDub+iosTh$#ym_KBv_(x zZ6n`X*r#1il3D`YvaL0ozF)C9yKQa$gLl67+^M{SAKN!(z4({Bf#1HlB++&{kAa-T zGdSa%$G#c|JD<=*%Oky0-_SnGFqsy{@G)w+eC3B)eks+`R`Emo6VU!=aI4=IdUB}! z|Gq-)jP@>bfDf7#a#-Q{TR^TjSU_J+H@%IWUA14ag`^tVLP-PE13HP&O@ABgr4}Cj z;n(=c{<>7?Jtxv#%1if;Rn_oq8zE6$umO9(%SVE<;$agvuR z%7_&e%niEMslsf?F${(l4l0CIpcm==9a!JFO?rEM;H04To=vle@0IJp#TpDfdnE=M z=!|JrD+}C8sdEMmh#^jcm+aH1C7l9DCM2&P42`=2=?f+Yc5<}thdSleZJ{Fr(Q$lU z;t?&Yjjo)#Bl%^mlJhyJidRpp)r`wD7OuM-{LCYb1$K65Y^f)t@%vB{V6{^D=3;?wi;l;*feKhAAb%qVu9(e$oS07_om_7% z7Bw=olnb0wd6k|G0-H@db2fJjsAEQRo4^|26B{z)gQ3nV^Mnf2IU6S;z*!^sGv$CC z=s*p_&Tq>BJ2|tm334t&4<)r^p47^weg@VYF4RhG_Z=wUd`qSHKlevx?ZbXwXS%+^ z&lSl$tJ7q_kPmjyI{6OvKD3nwI;H}r>2bOqg#VIQFLXN5&ygEIP36b|1<(gYt6m5E z5Rc9xepg;6KoZA0T43j#PFA~RUJm=c#yt=HkA!lsOoi0fO62(Gm4!k%Muc*Qubgmj zkd82GiH1JzN@-F{!FO!qkyBby&uou*j4HagRrDsC<`$f>+k;OvF=LGj3 z1#T4XwFJM!juFQmDw2!w^3$;9-4^;}fb8iPi=67^I>JF0!iTE5l=3g=7Nadx*-wAhqdeIklS6Kfa)i z{Ad8LB0bv{IxKxXSl){I;9Uf_jk8@UaIKeYqn&{ZXw^u!dmHoyqxtVa{-xn%xT9+e z!5=qzTUovsni{FDCBRUOVJ$}C5-bGWQB*E$9K!0~_Qhk~+d?l~A-wOruGN#n=IQMR zuxg8_#OTg+GfMr~mx$3(?Y9YX_mmE@s$%rQHUVgCHkUCe-IgAWOZmpng|1Heb^)|6 zPS2tH_E^MwF!qdLUeN;7RM!`sfmv5D(oO-i6C-YP8ojEm=U@dZx^q-ycNI+;9u;OEFpn!taLvOu1+MRs*kqN26G`%yIW)v1t=H$bh-sp7@pXV znIsEn8BsYZ61yeg(-ae9eg?uq_V zc@j&W>xk~Z>)Q#gw-GD?u4S0z4~i~?7Qu!aK{aBr3f@P|ya>Sxtt;&H(03y!IRL8@ zv~rZNPSh4!GhnV0gjs$YcJ?KpJwStC-4cZy2|Zqao1n%XIvDz-uR?gKUG0tW45X&o z)UU@>F%uWST0@+%7+&}&b!I0!!T9^w-z$GlbXIrASfSs~WRJwo@&kUq+!w{~3(->= zcT3M=*D=rmjHq^Lp%r^TARP8o`prUO2j7$q+JW_5Z67IM514!3STpp}lpS*K?cnz? zuvSd?qnz-EY}g`8Tj;59P6G9o5zVeAZ>F{dfI-OSN*n0cCbHyfS%pCLuH3)0vTZWqOhz z0XuiP?@V9P-M4dZ_hcrbCJU~jxDrAAT<{ZITvvli1XmIKGz+^bk2P69L2)6wKvZxQ z7dJf3_doTxukN1dBohq3-*!?{_uhJ)sycP5&Z$$UqGukysB1Q?rQm!PTAb|T&ASHu z@%$ht%6kHoDh=U*Xa_1`uwBYN=G9F0m)-qX%GKy;z{hD7B3 z&==T?QPz&WK6>;@Xjeae`48eIV~MT-*jTkpLPwc+Ms=R$4t!H*lht6pwvb;9) z3rCOi-{I~5^PglR|G8VgaXKXYA3pNvH#io~2V~!3x1OT)6}0-uW6(jD>&(ZF+;sF! zpvjLN`L9<_15K`4{g!J8gKv(md~(&<--?EEz0nUs;`V2hHRl}ci*`MRbFCcGcfFXq z{U6vFOd4nN*3lZ6k3AZ1kDu?w8>$a9u)i0M-hdP9gysCpZQr{cGj{3@>dSvQK&yuj zAGz}zr#;&=`@2tl__^1Zev(GXzGBDs4y8s<(#@hTA9+7+&OEx*;W9o$Tn6qNx&8d-B71-MY^!qg$m%<2&fWNA zRn5^OSKjXGk}3FI{<_?4Kl#==rlENLf$3wPz%9bi^*cEK@n`l;8>?#lIJi>iZocVE z>_S&fe-8SJ^B%rpAH{pafz#d|oh8a#d z&pLoM!u@s^<2=GAQq{PNrQ^W$?Pmc)J0IA4kL z-8z4CPCvAIo1?8imQgmVTJgs)?*R~EY?%}4F9>IOedsDTk>)zQPnfK1UTWfLl zu6o`4TXFj97M#{vR}H=ITMjRbekuCo;gwh~o%8&3(8Y&fcc9LA=}7X=v(H(*b?=qR z*AGOEZqL)KYMybPY@UGpUuy2_0@{tQ(f$n`r+c*5;vFC5PwfgCqRlj_`TKO2bqX%|%rc_uX`OWp4 z?Q`hXf9jFfwLKVwZE571_T_*Ku*?BWZ)$$`xr_F1{TLvEp&dZo`Qy>QZOnq_~kf5Z$nKZ|a(@RNLDR!5bHX@PsIaqkk59XXdR84D->_IF4{Vsw+`QtT!K!+yrqUhYz+qZ5= zy>BaSPug0C)Gr;0zjF2Axx8L|xWDuEd(g5C(R*-OX3y5q!)+<(vnphxq$#qy{i0vc z3HD&ktDc@dduzw}M?UmTR{KWk5h|DW$3L>#f2F8DZr#QEV^QiITQ5yrwDoI%=|E!)z{VJBi_R$Z$^7s=!0MO5Pp*uvK1{tkee1SVcx&y$Bx5=aJ0w>hUL9SO z8jVtZ2ckSjYRk7Ae#1NI?nG)ko-4JK%oS3%?w4`@WNmcL;X14Vj=W$U-kT!r>Zen8 zZ9VS*=~m-@rD@t>(#>v&l4kQ6k>5N;THvG5WP9bvzvF!3jK;^G3Ps9sVkhIp;SNbIzN1 z%xS-WYLPJuTLd%9iG2I$95;}Rh)f&v3xU8BkvIm^tO2{RoE>DfX?x3s)r zqRe)E#K^{1yG3}C^yvANnQS+*Nr1!iiDywxDD(j>Yi7qYX2JeoDAeKR*QsTbY28}1 zSRFNuq$YxRRlWVQRn^*_s_LeO;y4FC zQ&oNA)m7EI;fhl$%ArqJRqsLi-DT3tG{N#^7r#pQlhZ6M6AG=@^D#YNIqHN${gjYn zcK}0tVI>g3?K6MjzFscb$Hno>ckcqUeHGe%PoRHz`_@Y!>QQqVZe7`7Q|Z=rj)w_p z2|G=zb#Sd5&%XF_M6xu&{04xgn#J-Iw7{2k(9*I1+RDfBTHXc{G^EUo-jLHq^>o9| z!jLxBV4K7F9h#*#*jmmU5r2mbE5kopi3ZQ`^*+}payexL-ALoQwyxf=QHYh-leKk< zk~W5TvIh6b$R^T-q%QJk$V=Gx)39#Ur;wwfY;4;UZtD%H8*L*yqN09zBcW0uYF9+9 zNzNi%4z@#bJga4l1cN1RBhj)zWi9%lQ%Oz00%mcK|$Yu0wzFxNz^#j_Du$jq#_?8Lp?Td8xuJ1NRQh8g(bo$3F%M-U<*K;yvupftwv|i69mZ(6%jk%6T$8<9RcV;+-J&fYmbl zRpBo^lP`br8@8A&=0WkaWD+&a499IPlLIW1!RN2PqNLa=H=fk=zB56& zy1(O@pII9=Sh|+ZVda zosA^BKBJ8RdE7q6+#Sz=>p)F0mB+Q@sFqFW>RNTe1jxjnK*1|IA)VLMwQE$gily6_ zap^^7R6}5x0lNhdMAA9_(f-86ESAs@5!7WUDo4Oi5HXmE*56 zoYqEYjh595SR19m2LZkj;8-?S$UASCm)N@)N{Elqr;ciAh44}}b8KA6XEmpRDdJeh zYowyhR6wUFUf7a`HbN8R&V-y0Z6&GO8n`C5iXynun1@vYxIjxXEL`!8mjm$3VHN8+ zKr6oGEImW1bzV4&r;njtE=26x4$GuPUfRsDfsOKQGws44-YI1+t%3poJH$7zjw&C8 z07SOwFe)L!gL#f)Zq==X&Kii<{h~0HR-nqLSY>E1jty{SuBxi?_hBQCv5Hq!sadkj zP*F!BR(ZXip=MrCCGl-%!b>6SxR$_z%R+iW7@jN`1)T;Gmv-|l69IVMq$mVwd~Azx ziNIY%Msi{VGC8$XmVpi!1$prVEmE7<_^%aEFnm)Ns?qMY_3>DLSMSDtwgRO{tbMQq z&+212MyY_kW=`EwV^B}m7?&(ohxD|$Lp5wPVMYf=4HZUI1AGmk(bUldEt#lKgqoU~ zLknv0yku>7pcydN*3wdGX*1SJjbn}cx3aEo>dRd*dAThoRBnbS1S5Kq^5o34ZlkNI zA6Tf{AN`oR<|tEsw0}0W2jr-$aD#T zM)6!qgWLpcC#cneHEIg%1plTkT&TJcY#l3!q(26soAbkP9@Po$wH$cvF^no2+)glW zPfQyLF@eVuLOv#IoQ&O)nug~a+7x9M_Gml9^1JlUW#NynT{|w_dtlS%>=$F8lQQVEP#puAC83lpmw)&PXu)1{SWigTeyF&xV8H zceC=S zKkORGN8#VX!e2!F4n>q*ukQ%LFQf0ds;c_X6_CL_06E(y;ogq^dbFzgaaR8u*70~Z zv<1`O;|t=zWdhjewHP2{|_R69{%HYf2g966c-FBgce{^^)ftq z40BPzR>023N#Ht$OcEKahq0#4U~Z&mOb%8)p2TRBJg6%t^77GL4offNNs$W4bim__ z5(dH`7RZZM{>>cYf@4KGm6C{D+L9*wVLCT%ITOO3r;2-;pN+l=!WrVX<)P7B=UI zI7A%fdW`bIyOv1kv^-()SzC`n9t#NMw^8%}O(O0)YYycA!|$A+3%@QJKR;yQp9Sp! z?w1%plpp;zKyJSM$*Sr$&{h70|NS8w-#anBJK;WCM89NQO26k(rF!>6iZ4CxyQ>zo z1yPIX&+{z*8vsAifESDC-!o454-wv36rN+_e=F4=;l=bj#ljKFe!I>O(wB=_IICYf zXm}C*>Sy5uAJP6?5&eiD{Qmp8*K+zxFqHlsLc8wAe6#q-M`3*Xs+ReeNRH$NI|p9qBmE(*i^ z))@?Mp>VVt!>=1)3lczje=5tL=q|NiarrVX;SW8ZiS~TkcMh)P-ziP$-$BeL3qAsz z`=_evgLeUU;aBa)`-A@W#rOh8%jF+fYmnR>6c%!7Ep6-!tX-%iue;Vm_Lv{qk(9h5jk_ljQ;A`bLO2nUtPfRm47xz1_wG~9fS1VcLB=7o*q53 zy@q%qWegdn-O#}I9-Q|aSW9sBnFc$bS=!vNL(3;pNpl2L4zIM&5L&q`6yK2?VcZNv z<)HHP06W5*f`bY!!SY4MkFAv2TQKn5VoA}0Mjgz)vseHh^U{)}9^0})9GKWHaO@aTd zuW4L4rZ&)Gzs8ge~F!1a3)v@ z5JR(CZHt73Pz3JpPZV7{kZ(Gj&^sOn4%2>H=1 zn}C*Iv1CgY65#pK`WdQ1-eiBNG&Nb!>78Jtb|~oC#5lNVDj{#0jQ3&vvEb~BbjxIo zA@e=kQ&&pQRx04i3#*YmcyhoHA^{H*iczstAXs%-1NRe`oH#&HfhWN0N!cSOXT1NL zmaT9RfEZJU52TJvf}LWcxsEuzTHhxXu`zOd-+VpAdE@fo!JceJPdYe4?M9`eaUi#l zErJG^;`%o)T?N@eZS7!JPsP-QNQ$5?&0#@Z2AaxJmnoEYpfNFKn_C*o!0ZvX_z5C| zn5f{Z3+F1(?UUHDG|r|PAC@(ig!zP8mf!?$2f-{zY?FG~Wgw&CjnTy^s{>m@kR)zZ z3$$mzYHCo{fwZx4EQ;UMwUsQVxsfJ8Y^Q?u(*_S4|0s_}2&^JTB+rQ)Ec((Z-k@J2=NIR@*yzqSyq^CL!4b$7g0}KBA0dKK+8`hKT4cCZfAq zWxSJ!DB^WxAxDE2lTUPyC?uX%B77f<@pUXd2~o3V9x!FIT-Zt)3R(t|Cen5|o<2lY zXcGviv36S8)EKf;dO9733@fi={fB9xb|G!b4I3ki)%Xq#94Nve9W-|8=5Vd^rEalW zo5%i}2{~&s(a;5f)hW-!%0M)sv_s8o#4-!nWSHgLqh(P8OX-V+)ImK1H5APnmlDh{ z6JT0fYV1owDu2O3#1%ZEte7@RbK;hanM`E!YtYvC*0!lb?HCy78=%PVsj=UKb>^h` z5Ac;T6@d1PGEoXm!L&M7^8k%>+GwxB*3oPh;v|MguqQNRn&5Ex&L$sc087auH}XlE zLK}`4aaNufa(v669r$|BVExN?dnq>m$@RBuSs;a6Fz^_6+5^m|EJ{?TF(`)99VWADeSq&DY^RE?k-Iyz*3-Tw4 z)L<&f@v@5&AUu)UtZmZ^xfp-%7|pj^Cf07)$coYq8H>j<=ka~~P9oK{VE~gm5eD!H zlwTmcCqL%lj}j^5A2UX>T3VJLh!6N6Ntb9V%auNZKbFScHv?Cx%@)#W3>5TVVw-A0 z&j;(&9WpknpPnH_Bmp2#<%Y-YA!}P4f*|orqyTT<(4`;?T|idU2k>-^8F`VgWc|9q z{W;-%8#ZIdK$i_7Gj{S1wvYdvDSDW8C?t8ZP>9joPzXda>@N(YJmLn<&u1!*meq*Tosfj%iAI`UFB+8`I9S*nV`r&31Zk@6CcEIaXxEY((M zed%juJm-Vj3i4Y(0LsEzI|W98GEqTRSq^GO8z;^nuZH_KGJP;j9oBb%Wk{o3w#Q(r zJ5oj>HH%^!WU{et!Yb<2tSg05E0+)6AEQL16 zs9i45t*o97sr<)+l%@4e^(_bsV}As~UNjZ`c-Z=ISZ%3aRlihiZfsiCxU6}VT5IWP z9jyz^-L`?A20b&RCzE<|NlSdyk|8h#i4=evHEi14Y;In$YQ+lB&o&D@IO!g6l_vs8 zLF&;JOL|Z)DBvc|GC+DTSFtdB%j;uHdX@sNemtwBg~$No;ccAPNUGadg;daL*6;m- zqxaS!HrTVYIoi}Xrec1eiOv|Mt*mY%t1ImVBh6Rjl%mF_e*{}pA5xoIsM0GHb&0Ap zEe|bSiT~7w0lwP*D)H43CUE6KuacraK|mx?GJR-iW5}BW%>|hpO&SCbn?vEBJ}XRe zj=osKz@QqViIr+XV}Gqm{`(_H2SyJ&Oylkl;Bj;eZHj|bG@TGGkZvCMV)}w@JWN>h z9lSgoq-y8$bf?csn-c6fd~m@{%6jZ9j;L5B zLFR3b+cW@_nsk8!LqLchqaS?ptd^b^1qeoY`T`1S<$Gd!qPCE=L7>4*6YniE7XD=N zFq*653Cpx?vBikF6`VOoJ>-#L@;3tC3T_3m5`y%VG-`AyvLOBy&ENR&xh@Xatk_`xlC@0CMLbt* zAil9{3fKHx{7Fs03S@i14BbdHz~5Tcp1#4pfq3u6o_K#}TdV`~+v&6V+j_gg>K3%) zB`OwyH+{c!rMSh$^qAn&e$FclKR-!NRP6Ouu^hoijgPT@y{-_sLq zqwsd_DQ)6=Y|}YSQ1ej8Cte3W?&4-XqkDK?A>Nk@A9ujBaGE|SK9Otkn}tu=DaGuR zkfV^M2xtds8DVhqC&ZsJT!Ls9vamQ#kuZriWIr?mVP4|+CG{TIUK`ufNHA!C#6pn<0P#XlLb*t~n0MApo{MHTg zCnDJ(MsaCU$Si!-#CF>yNlMMc0&Ek?&@R?Yh_<8a$QFl>$r=4m^XT(t_ zVbE|vBYo#9JklZw1UL!YSeS4Gy#1u2LJi8M6Jpk#iG-Mc{+}j#5x)(pIwp+ z#h$N1V-R1RGNAomaS|dSkfC2%z>LZiXvVV%WfKOnX*FSW#(#`|BL-}|i+y1AiM-r*O22DH(VY@ce1aVbu)+hKZDqND*hU`I4vC`8kPr zB5g2D0XhxAe;XNdOR@qHM-6v(u@yezKxnfA9ZrIOYJ1+6FaH3mO8s~p7;&rI;B@=7a)|_eMZ&n{!OA(paBZL(=mn|g=qz5*hU>j_K zFa`>GmMMdXFpj?vR$+xtDNW*aK!igOV6ojal%tSv9N7b*fvNb;9}vPHhSS6uK!^g_ zj*+8Evj4M`sHN*WQo5c7DiW*?FUOW~On0yqTUtY)XwV;IP@`&F+&W;7rm?<>9hnPL zolpwXGz1t}bJh&=V>wE1AC}aEef|9%5%xT=vA4IYcfAsY-~{V$CPL!5^VB|AAgoba z2l{)6VWelxV%p;5d3aO2k~B+E5R)tEdh0_AU>9o2314_d5vN1AppO_`XILYTupHue z?$oR#9k7z48=tWD=<4o>phW?652Fi$oFR-)2Adk%I8V4_6_hYZmue^pOhg5KcrCs$X9 zsd?6SnRu#is2$duSN_a#iaU=^xhM2$(rMlmVf2Z&L7q+M|I)L_` zS&F?Q4!H^ATX`DVv=rYHHXEP@p&`MKxI@G`#_5m{zR^x*s!aN}_Hd*ly1ujPy%%)% z^!D{{7>EsS+_d?^_g$n7C6fB^NXoc$TRM|9bK9YYSQylq_Ty2RoG zIIm&z{DP5O!|F33pBmUYhfc4Zitiwak33GNEU1NsZ>&=+;Mp@R;WrQT4-A4fFqW!J zoc{G9f*2o6lq7Z0#5YNg3WN{!Dz;#ubQw}P)M%wGLzp+0w?MWm+J^>tAT5tV4HftW z&#TJdrZ-kqKYMyr_1ts)`V>-LTKf2J1e(+ATr|#Qm5dQMMBJ4{r+}gjLY!jbfFhIw zj`NhX@B9s;aA} zr3rO~S)1x-1O4a5;7`H&%J4mJfL<-!+zzx`m)mBf?<{d(J+EWGtRg!WAA>skVs^@% zp9%*iB+U-=GtRJrSYt^|NW#NB=cKwM3bYr%{aZLL&m+6&KpA@uz{?ZAWzNjIg;bh` zFvvZMB~eJ$wInvXb^uN;zOToTRWY(iJQ$w<*-yd#2|~Zxg8?NwCzzmg&u~mV1?yY> z_~fKDFQ2qr8!-JA@UD=X$q_^Zn4d=Teh~*W2e{=0In15?ufHgMPh4%sR0NI5Q)N7| z{lXGs@f5^I3?~QLNionHxfISZd9v(;iVpqq6Pd;M<($bb4#M6^YaUq47O#@>xl|9f zKPp-#c5&r6Wemr2ogg0y@__u+59YxE=(;&_V2hj2u!R0Ods z*-PRiI*-qAllRj?w%m8ROkv=w^vSVT*RG9cuVlzpN#DS-Txr$)h zurk~gwfhuUTgy=f3oU3$pQRAey=M9}aqqf%GJNqJ=dT=Xe96jV&nYtB1&qIhLJ++Q zYc4b@5=}-hjQkCSJ(JULALk*f`X);@dl;h&+`8XfI zpO&aig*v7b4u!%P0ztv>r4I5Pw%5dICaae7hsNt7hePc*&9uN(*ngro$_8qP1S)?V zfC;)7B<(4jlSi0^p3*pW>$P@184592@^Z!D-78mRhM0F(G%a5VC2=Ph=Z7t~7(`gM z9KpfD@SP&AhD_Qii6(K+hP-!hp_YDnbX?VgZnmr-=doFvozw~jDuH?&DnPa{3VFeoFJF3wBfX&jlD0dRebm)V|CO1KIB)S5H<=loH{HG2eqm@)4ESKY?|dsMn#6v z8VYnMgbS=-mWF4{m0sKw?o(jCcC&Hv7-9k|$A>zeAxsC?N5FclysCj7=tpl&!LOBEm1w$DIKr%MZxwT#{R| z1RxT|5r(WEPfNx{X@NB~HO!%vIGef(_8Tpn^MjL(CE=-v74=1O+T>X5b7HyAfObs& zRbqUqM5Uedl7B2>G5y8JXp^y-{!}i@%q(WQbVDwKnWe}QX}l~aCuhDz9vZ)DF3z$- zzPw*wBm104FruQ zOyIvZ;LEevpV|#NB)^d_^RJ-Fm5_^*+n!TIv%K{$H$G*Ya^VLtwic8apRzrDQ>VM{ zHP!a7F~3TlneB8PgV@%aPEMCavudeGlg+8#G?mS$MJc@VX>caxJ+tRZ&`<2SFMy4R zXV1YJ2m7{FOI+OE4SNR%L0|Xa`v$nVh({@3<^B<3Od6@jzVZ=?g;bR=s)T60RHPC; zR9fC5Tf<%7cmeiFzsuM@4aWEOT&b0iEb-MFAC55p7{*31Wi1*R#ZgMMy4$W8s16{$ zcJ(k5)%ohLXcekjo187J_4vk1{_q z?XCYQqQB#|LPcAA{^*i#bJ|zP4}I+{hzV=aNH|Sr4CQ_w$i<8O1rWOfofT8rnCM@fR5OKp~rj1XRejO57_z@-v4ppv*<)!FVs!*@alZ+>aNGc#5iOSKronPfNB~n z&q$MfMD;v!L83kJGSz5RWeY+d&fk9C?X*xRYUt@CiAQiI*wKUyg#$^|X4-y|o7}k@vuofpx z1@tMM7Q@~V?+7=Q`xKZPSXSlp;dfX7;jD4bMy;Eh`&3@;i=!P->kuF*7z2K!Ks^g& zF_`4O{iY3k86^y0_(iJ2@p?Wp)~CoWPl~pv+}?zsNHL-O7|br6<0wzH?s6zFkpwTSM8i+P`DudTTG50xNqYYbCRY+-oWFYV z(nYhFMvjRYHXt%1UPF)dK^e;1_y1HNaz%2hR zQ*}jLP+80P>Mvu32g2AzPbOuBC)kr^^oi_WFjIu60G+R3ry$UVe4Cdqxu_s8t-cq} zuDJxu2TyT-Oxh2U{t_MXfE8v2cVbxJVZ)5<@BpJFd>$w!htHjz+^3>>3J1>x9dpnLQlHTxKCx%bAZ@HT+4FB z7-zFRt=1$G5Q2$AOpbjSw%ki%S2asZHC!WQ*iKma^=AMMXKf+t;C$dYvK1NDn3A;6 zh^|FjdEaRXDt1ZPS1q6GRITPjyH$1(%J{F$~^B!j^@1 zgP4xo%fJr*-Z2F&a5{SsoN<~hJQuk5Kr7CE9B{>ggl13PX28|joHh=7jmdag&(=XT z#~dE!2S51(JM6EBXRKC&DUPxKp>#Mx_9@(reP@+NNa1hI%5VaU1_P(NNT^U2H~grE zruTNC5Wqox$F2w@;YOr?iYczmP{YiVX{Xu|Vyxa=eC+<6x_ngyl7 zY=DI!Y^|zI%_}!ze3ASFoD}s_v~94h+c7#e3uo6fb~9ZLv{qfG){Nn7s)~61^v>-I z@uQs3`t*G3y8!RUJAZnKgEQ$)LNsJqUNn^R5Y>x*G>-)1$>k7nArnrK?tJW9-%tA4 z3y>R`&lQ2SjfAaF3~8BkK(KAl#t4Nb2t4k!hG;0?eC-y_?iR%gfM4$kvWA-IFLV-MvA@V9U|>M7U$Zhy#HVNE53Jpp_H6tWDb7H)k|h?AKUT-QXJ z4jz?Mr_zWHYVOg4aKQFJ+YSi?lnALZt!xJIZTR@cKP$3)1LMjWP)vUQ8FPog1tu;< zpU9SKL)n~5U?Hp3Q?=<7o310bfg##_PQywZgIU=91hL46o6 zzMdlVgPAm7npA=vs7z`?_gw{3U4lE@$SOHsoq!@0HIb+1MXX+M#O>o(&{ut8&Hq?mdca8b%HuA#X?b&q zk#}a=jJjdTS~^BZy?v~8Xj;D1e@_6vCg3>oX+c-;jRFRnILWnSEsSE&Ma1#C>^zQXAL2wm{KezOG3gZ$+SFcqB0B)D}q@i789V%f^w$Y z37yrr_r2FPie0;vYXg zH~P_eMaAe`0lpB9WMJO#c&J4HA7P1jo1Ua?mjM`lZyf~FWWeT}m2YRZ9BT(5;P1!& zI+=EMI3MSOho|t4Jj#v^D@ln(C?vVau1FWl9YRHz`K1{Wt7<<#AiGg!;@^w|ewD&w z>T_WY56lRxj3ogN?8$owpUl_C+cSK{Him-~7|PBV7mY0O%^SqYIv;4TKbW_W(E{u$ z7M)ch^hR6Y?i`HmC2JwR>cz#rR5z0S@J$0Tl|xQUE5)=vjQjOWZqSP0gJ_o(AOmQ| zVmcTIb9V;Zry@CNLd(VtlL<$e-WhCd(W(>*7TOq*t_gI}it@^fDO7CK*OC+;wKz}3 z4&6e^knJZ}DVxlnRJ?z{B$U`a2P(1yX-v53*2sM-f``lInIc2&FzIQqi({HLcB`$e zd@iY@BdR!|iZv69pm^l+kT^5#3f%v|xegdaw)3rM6gE>VT3gHe26w$AA!6YZUa9qk zL@vIv@Xq~`p8JV+fV%t1xkvUy{qMzh>MsBM9)qJmRzo(toFiB!)?&TGY?- zcdY9L@0RQoEEg0X{9dvyw=IOsKoV%DQgRT;0Acui()^r#A|~>asVB=;HkWUFoFeiS zh(TCmIA;o>nLmk)wm3;pPAeddN%QepWh#-ir{l!{lnqn5)8tC`B=47$CKtuPN}>V3 z4d?Q-kK)WWocH{?j8cX3OB+hauVwh$I_%^9HO@4^l}f)I>E|N-p4XK=FR|}D><8k0 z*!^&&(l;UfLAV7oOQ+vG4`G14sto*1IGc4N^4?u0e_4-m=QSn^-;Ht)!O?lz-MHUg z#+Uxw4uALyUezTdvfJpQ=}gW`J<6MY3F5|PyCFw(nO`cFY?25P)r<|G!bWs_1*=O;K@B&)u}(lAW;gApbdv{B_!R)(RC@|jgB-P=1s9aV=I)gxKYaH#lfb9s?mr~m+nRk(l z1_G>dx``yXSwI)Xl*rws4(?L`ZYDR%XgNj?7SwLj$4SfsEUAUUXofDQt9PYkwcGif z51Viu1T^9x&BWEQ@*_IUW>QNjH6hIAzw-E0Vut};aAKPbqW67>=SU=>aqkQ0>ho^I z2zcdq1Sn2IvBSW;z*wssVsC~MK!RP>g6@)l9xd039$AcQ_UL{>Ir0S*G@_-N2%bGG z$pkcDHP5Sp$e|D`)klAQo^`D@|-mNANC2^9l z2kl0zb>ZfogZbb=tmz-(^8qVQW(YnSa$pQ0Hyhe}#>uN(8q!6YH}iugbl2#(<6vDV zgx%act{bEC8qOmEJX?kXxcp}fecyyF7TP&P4jvP{Ov?ghiigtBW*=@9N*54$LqW^Z z(rkx=5j0?89tuz@G+A45=W=#DV-|2f%Nq3qAM~M%GG*{$hm+tf+QU4l(8FdcT~ZMx zr3T!)R8z|C*9veiYw8(Zm`Bdr=&T8!y~UpoA%>80M;PN05neqV4ZUSH{%T~rRQe2mMz@_m%v}P zr9XU$LRSOgEbV9<#zWb4m*i6i;fMRJ^o-}eU^c$}gMyD=?3?h$BiRSn3={-XoS!aX zEzC)RfzjMZ>UWGDu^lR*N5lDBo&~|f=(kkf;entOP>p9=A=s~EiY+d^k@Rzl_*0=C z$O|q}BkY02g#9z7r75<0Dk;6R0kkQGAH?sZ^4BNKVEg#fQ(V4EJJ^{}C}_xdQW2}{ zQ--os#}a0DJYf@xQA@%=f^|n4=;4_pXU!OhV$l8a_~SBeKRG!5>D4cfIK}%rAQGv> z;F9cWuS7&jT8d2ml!`QA=LZ3ryx&Wu@XF7RPjc=B+%Lut^mai0bc*+P`D;-8rk)th zs$CRh2uh2jBu2Pnv3xPU{w!l4YvR=2zV)m$Wrw+)g&Fv`A@g>!%%qYTuY$NIcfMMB z;&XHbb)W?Sowjfqc!IV$x%F*cw#;)lWCAc8kc$>2uc`XtM2D-maa`4XP0df_pDnYCeL?$eL`qO0cJbxRvt{Q-Hg~FM$hm=+0kMU&OOpf)fp} zI?Zj_mA^!&e1elW-&Ec|#IK$xaY6r7fHMv;t{`sZ@HaKDc#XsFJ?L1;@GP3kXna)A zrYEWW>Hd*pDZM24Qw{&gU4Qrwryf_I9ZQQ<5cee4_r&}7H7E}OkBKYWj)gn+2s?_H zVo%tFys(fwh{Amn`9u;BPkb-0bR2x}1ORv@tk+V1`Y!Kdm%dfrsmy9jDD(kmbLE2& zltE4{?$VQH>=vt>dFDg4vP31&pi)-5M2&oyQijJalk5Z$rcADm;ErKUxUE-(>O79h z&^3H?1H8g)72=A(5N?>qwf3HG8JsGiuikIM%%S{-eIHyL20Jz|LzoZ}d!9sPr&Q=Z zd2gJYu5DA41;YL%d~6?HDC9lzfIqhnq@rrd{ z?*EsqhjZ1t>-N0&#%=qq+W+-~|9a^6&o;ez&NRDu;OuV?T-$!c`q|K`YtFp<1GirE zci(^D`n!()_rL$=(7E9jbw<^^r;&c(~>E?g_H6VX47raE#KMsmA8Lx5pFaME5^meD>n$ty^ce&Hu*lKK05o_uv1kMPGlz_fwzj`oiZv z^pDwx-v3ooj(YD^L<|&`_#wwe0BFd`<}jW_Q56l zd!IEA?V59=@=uNbsm|!HSr^TXY`y*R|8vbtfA@=9*W5M#fe-)Z`~Q32w5#9z-)H|@ z^Ro7N13QK`S-<`EyQ}K|NI{BaODr6%y;SPu*& z4VhzIq)}kO5_3>naj{C<@~(24qmJRyb8hU6U*_Ev31;9-&_HW2 zI!iA8!+Cg8u)m77GqBHjJI;9#e`K4W*HbeXV5Ek2uryn z0ZSU?qA~S!acW%lj=O#+m+H9SA#=UHL0K;MVwgPrE$O{a#&wJ&-!;@As$5zAgXO0b z53W#_yZnE=``HqHrmRZ#;V&)NdE8*_&^9 z$66Zsk8FZ8jy2)64%RawT^$3=G6x~1h$$jAQN}}dQ+&v7j@!5!YptqTBY0yM$|KB+ z)1>t)uq^<)jVrIxI^ZRP7-3jlfyJ713|9(Z<>Ftl@cd+Qcd2{gYfG4Lf+=tV3k&W$ z%_xSWT%N*+E}sHen|k`&HI&2jZSyzpCRB9kt<=s1ik*`a*0+z8v}m<)I7Nkyqy zvdD(+SIv^9=3%PimYU(kYR&K^YHiIh^^7On;Nb?s4GK3Xyox^JHw})S$(KL*joxLL z_nV9>^PqUpHy^h*W=VgZUhzJdN=D2G>9ZDeGN07NzRN$p^E|@d3U*>josQ#2vj}L< zkM7%vbEJu9NQEAxL1(wm^GF-^E7kjcrc`f#P^sPk$7I-a0~Ip|PZIGQiOdz8`GfhP zteM9sGx92;tp?`Nb$t+j<0>(&kVmiNL6Z18mI^+ZKC?2f!vYbkMd6A+1ji|y)?doT zwoS+{q{gr)Wj7OHj8HCAt_)jG_W#wSxg$$o-QT+QH!13kuT!o*FkR{Vb(N3)6P^UN zXxzgvY63TZ*pM4ieF!d2M`HzcbQ60JxQ}B82m^}hHC4NiNWsEVT31a=P2$K@0;av_ z{Gbd2BS!{>lrAYlKm+1bM_$jc!L4*P^zrQiPnx7YY9v^W!dgAT+N=>Zjp-Yf^i6h{ z2H!{lTOBqn^q-SQ3<#v>RV_^`qj53|kp)bI@i+$q6Iu*b>_>sh5sZnHk%mG1tb&vC z^5O$k+Q_#^(uq@GlooNZfuhs>QdA#yR|mC)o;3?2DHZpGB$#betV1&f?u9V3YEsV{ zEHRZUYZ7tlX6d$BuuxC*`?wONHq_|?C|e0zrfn|~tw08{c=@cY`B#DmmGrcDr-`;P zY=#L0FE%cy;sl3m3m_4oA3?b|E7E2AlB0i^$edFmNxPeD1dNQZ+0MvUE8Mu zih|BN8KCH*p0qT^NYQ~~to@KLC<5opLN!*#1FswuY#1PmrVrF%v-!k)N0vnG^5(V0o-IfInaqeoP z{CdDq7%4M{%Qiq8a%{xXxXJJaP@-~4K=|ib22k3Fq&o~bDYY0+ph>vDC#90%VGrSO zaMB*n44G+|o#q{gI^j4N4zmk1(MBQkW0=G0OH;AZChpG|)-nbVf}n9whcF)_jMtS% zaO=LqA}gls@F7T+YBM^8_T}`7+Zw0QivKv5xI~6KZAT?}0P>7)+=fWfMBkW4r+^9) zJUS!Q4%{AMfJC6HqCSD_0oQOI78P|+Y06v@Qbtc`oKw3C*j=I^NUi%_mgbV-K8PO~ z6N!w0owyr?aExX=f&lXe?fi;1$LZTq1LrgOgacxS$ZmRn)1%pBlPOHD<nT2ju;0xG7aMfw9jcS3T?yNhG-m1homW2mxo`d3^{q?4@(&N4^FLjy)_mo; z$SZI9a`P8Hchyg;zqh3(Y~Gx#otc_Tap~6r_pdJ#ICohg7d1ab>5tZkInJ_W;j3by^Z~D+bPe;@bPeTw@Z zyNmmmKF|DJSJbyD_#Jq2g85hc{Bz9L@4ugXHGadicKE*@?OZ|rm#%$B2L8GyuW$Sk z{HxEp`_7-iZ&~=KZL`D5uE$r5-_;gYZg}`3n_EW1%8z?TGW++2l{I^|K5^lp@VcwM zJ@&3|zBSVS`~Uda6Ysbn^67_qX8!aKyCVPd-a|i{w&dH9+rIF~czEo$5tPGzr{OPk z-3R$A1ROm7w`EWJ_P4)#P4rJMod28YeJgvK-_`g+=Z?R+sPzZ-hkw=g)`E;Kfl99DYUdX=n}^_*pHYY#^{qLfeYe6!4(d~rc78_vC2uc9t`<-=9@{^P{7 z>ak0wF`Fs0Cg1mvf^+kV&yLjNADC8M1zzK}55Yg=hP75JDmltu{*E5S+6Uhn>!v$- zbiYASZ-bjZ+aFh^3Hs~KbJGUHPS!iM1^wQ*^!~jqZ_YgR%)g(eyn%oE-@pIc0{`0r zC%FZ-f@aM7__XTp?w;Ho_5EDH?go7!cb{+%!YB8ra8JM|_l$7Q!6)~!a5GL;3KxM-u3NZ4_~b4YE(xDpTDUxXa+e8r1$=T>33n}ga@PxY z6MS+v3wIlQa(4=MH+*vU3HKm;a*qo41blMO2=^R(axV)vEM!Y7v&E)SpFWx`znpWIc#T??Pw^}^i*pWMyD-3FiB zoxpWJ=IJqVxNqryD_pWHLTJqMrM%fiioNQ&HS;pV|7H($7O;gee|Tnl`1t-?j% zlj|045I(t!g-gOGmliG$pWJ1_T>+omRl;2hpWOAr-2|W9&BEOVpWL0o-3_1IeZoBm zpWLIuJprHGGr~OwpWMsB&6tDw!_5|M9(;21g*z8Mxy8b@z$e!#Tm(M3Zs7*ule<{B vBz$sd;qvgwT_)TW@X1{z+_mt@T`$~C@X6gQ+->m5-64W_*{|})C@}AOO6P;}KJN{td{`FkOQxb_MUX5tQ;UQ9yJC92O#F@WmI4|#K6IVg!7(Dd~p5$ z?>Q`Zbzgq)*L^F{b?^tzg~J(h-S)l_=-T_@LqE%hLw{L5H2h&uZvHITq2ac`ok4-~ zL&a~F4{d+gKTP?{_Ce?u%ZCj>HF##0y>jBCZ%CV7zrE6RMY*FGze)IA@`duQ|L7--(rWA{ z8SAASCHk`IN?JITfxd|bV^Z0*+C{fbTvde;R)GkJ< zMSAfbqUmLb@~`7kqmK>#NQ(N#*mqN3u;!+}t9QL6sXV8|=Y+1msdsgK7JP~gz7Zk1 z?N`!v%n}Y&5DgbRJj@0)Q8xJP2pjx$gnXR9@u*PhCtOC5<2HJY3rf#mjwME<|6jRr zL07k7ZrXoIhkC)^yH#^#U&8SVu6FZtm7Z=zo;yuqN(d5jCS?cbvI@-=#H}pMIPO80I*J9)Z$xu2+3_*vC9fBn7N{0;Y#;3uvc z|MKN$11nbi*1gfB`fzrc%g?f{evW&@PsnyZ!;F=0y=jNr(3KV>H8p-_PR)QTZ?QjZ z4(lEox~AuzX&)rhRcES{Yy1XM<6o%aFT zhioWQbuy%4nITm$e~1mKdQrokbDs=pR1C_RA=StS?oz*s*=_u5&S5u$(F)9lt|mXv zPhXgKK{YS+13r|=p9V(F5FbRJD@H#@@`v~<<$exSoerr+TUoU7@zw&t&;^Jf&G&-B z?E|h1xx011mB!3t?pWfZ>zU=ID_Y>Ww%#%iU2oox-th{{6aS~=t?tmQC^n^qot(-%?!0#hAxUfCys}Xk>^ekSfSR>%#*v9 z`nmZObLjxdH*^vA?l}KQescvRaKKg1hvxFYAwyzneP7?M#(0%(qlvXwBFTRF*mr)q;xk|&>G%N$4!i7-9qWk$87R|1Jtn;3RGb9 zxLm_bJNqi^JUA&w{gA?j=Dc_4!bY ze|?Z_s~}NTu9h55M{Thr*Fy8onRBI-C z0U;P+0TLRobTbRXo>IS@`B9v=%@rKlmlAV;<1%0o(0)hQWA@9r<_fxwDy(Ogo35tH zQ|m94)73M}y^IbZKvwwWsac*HzYLUJx3$JEzploAiZlC{Zm#j4gdXPQ zo$yl2oZ`i~Aa04k;l;1}XdP^{nP{#__o6S3yxI}qVC_J!YP_v)?!tVREWuut`Hix@ z`*JyGdGaz{VY*(*_$n_NC^!Yo6ggK1lLD2u1Im zbRKfKxW<2iUF@%IJ_h|a2DTIEUXUTp6=^|NO};lHRJ`d;H{gJ&lxn+w>C%&-ENb&q z>LC@hGCn?fP|9ljXLKC)I4TFA>f?nX< zqwbJbF~w?$xn*v8Z00`qW+Yj2Z4wT9R)hGjqiE`pEq%p~smlFhZ8_ zVg;AG(zLRaI~t6`joDlEYbt>g(?pGUpTt|TDsXb}ut-FMv8}_t6p^z^<2cS%pApSL z7k9=AYAT75Es%RSB&I-KnFXuXTp_E34JbU|<||3yB%w96Wkl&hG9Y>SINbr$bhk)hzH8%T?UI}(g zIz|HDm)9^?#MRCmF~_#)cc&Ynv$}<4rzQ36SHxVA!x@v&PxhW_JR>P*ae?xT^a0nl zGZHDtben>%rKTWRdJ3gejkJx{n}WQF)|_l)bluWCB{#%`81qSKURJOxyVF?ssN1N` zvGxP^F-d*mH2SGn$m=p@!NRg*!y~Z9C(Z6S*PBP_yCNQNW{4R!^wy)dF2j07Ocz)0 zI^CF4#w<8Ce5HSxnJdzPQredfl%|=3C(Lh2>XQF-2Kosz(8p&mrAHzq<{)o=8+r3M zUeQaQp@V2oH>L^LrRQ@^2gN-mzPl#KWSo}riVaqK{8EiQBDvOsu%W zZAM<~=B$RUESyepN9$!QtI-O)_@?;O9VtStxZlKio)sSv_nM?`&RQp$tZM5@s~WuE zEUUyiacRzO)9^W}k7NDDBx)y98q_e1jdLE9kDN`{#KM=qJljwhWd|>8dR=A7a;Pjd zU&J;Ya1iYUM^62w*OMw^VRcj^#KM!J3VbI=<(6FWeFf&9W zt&sSGCnd7`SJ0vdoV&!FvR&4XBwlluvs%n3vsh32SkV2$7}0oryDNzD*E5Jl*`e-~ zp>}(W6b!*iFkVvlg%|ZW=XwO99BbeFHSH(j?J`T<`t;6&>xrKIfCHv)$7o;5kKNz{ z^h4mmvp{TW`z0q|^0JBTP6d=Dc1a}qs~(+ms`ZliiIXY0WYsw{$__hqwmqg9ZpS*E zjp*9!y7js38gQc7m{rkC&)$_pIkuJwd7#+e1h$OM6yTkXuLL$n9Ghbln_~htHL$@R zg9pkENto@baT-w!eVBxquaBMWuI;M^?7pu_{MQYvBu~c=I1(3(~#W52o*AbdMV|Oe!z) zD?U_4VqtHL6=t>9uVN17Kd2F7;i?#S)bJI1rpEZO@a$M-e02>`-NRIBChNlQDRtP(QBRO_`XExR?HM&s5DI@7k@PcaC zC9x8ofJ@fjhR9^rqjhGOCVS5IF?Ywpq3EQ}NtK`a0?xvRi_FP|0UPgq3GE8+4Dn-M zO5rd{bI~{KeOt=fJqu&J7z^QEMliM&DPd1IaeaHv!(1Wjp?dbG2+^|E{WhxIN@Fbi zlv~;58igH$-)CaI)WUQ|azxfzYt4C>hfZGWv(-)9N6QoVIdQv*^^p8n_!Ha&KYB&k zp^1B;(8KndYm#L!o+(gY=-75{WC`A zgZag@i>a(%TA7fn`EkjbJ5I7@L(Z>{p6#0gIla-V+o(H!sWIa?Uve;Kx+lY`+o)~K zsANiX4UUZ&WuI(xG*CO!*66_Ab!jd0t5|qvloix*IU5Q zCRKNO&Z$0fLoEDutU#Dd980zpx^uL2nPSooEg zL)_;q6;*~<_(=4xJ_+-odjX}N>dTEw!i@@0%u|YYg5sT7L1yxNA=~p8NnQNDVoDDQ zu}Sj4PEcoYCaB2MjO+{`)X-_m1jAF&*C^FX085{?>CJ zf5rui=g;Tz4Z9_WdbecPm^YgIjCPMHv-_l&z6mur zSG$V4)L+q;oBFPll0YkVu)KAt0-0Up!7i$x>#fY!OvQd48OFIkSHzB?Jk=fx&m2kU zH}BPMOmDvqG~GC?-N?++c4MyIiH zTmNw-X?vlwzT^P7O>K7PP(l3U6AhpK9o@?g#m^%_^M2v6qq_wKa2SU>L$O6l_iyp5 zUs8(|{mJl3e-g)fXQBW8p(3Yj&rA!y=eTp7aHfxCs>+paWBI%@t3SNQ3>Fr@2y+Qn;Pgx!dzYxRzKg=II2>#zcOn&wuF#QGh!1Mr7 zngdJ{bMPHtnQr|sK5N>i$hps{p#0sw$JFWhE#RDh%N5`B6@%)BW1q**%O31u`MDGA zCp-;|`n42~vMR}`w3dmBt@+kcE4};jnpoI{n|I7WOZ03{dE&wO;5uPCu)Z;LNz}Dp z5*=|1o;##_=zE=Wg_5}ToQ;uL|G41k(m8Xc9=3l1S-oT@nlF6x-g?DXJG$0%ovz_~ zW&*w|SI6V~^(a2OGh=F-Jsqd(@%RkmWW2aTCiL`i=D+6bH7NzBILn)9(gGJ7Re%bo7aT=6`#|KX|nYxb; z9%}j-;O;fhuT0ug{Ym4a(poikhGN}7)M55BbayCD%!7%~gVX7ni4Itcrt}^kR5#7@ z7{6dmltVi~2Wi?i@SP)k=zdPu!02Z5@}YWG+rVn=4Q&nI_a$iCHqdZ{QfCHrM^In&J|N|+uBz_DIxkr11(2TZ{Wcf z$|qXWbcBubYMpO^Mc2UU=;`TSLA@3HRmz#*WG=aAPW@Qe)ZSbR4_p{602Fo1F zXTFt1R{%%xZX4)+mG(#rPG%Pk2uCh8rXPc!ZZZe&gxo{@Gui=?PW>~t`B2i2mh^@% z`yU(E|HcXZ`FMX5`hOJfOTT)3V>_y0nC`g=SzB=R-OG2hPZ^a$9d@=%UC5U*TF#9_ z9={xfE^81cZiek8)rtF?95lQJin-&FiZ7mSgh3V@c;Fm6{avd;d*R z8Ldurd#08RJ}(_|X5owEv%#NAfG{MyR;+%ZhtuETF;|(Z8NJHAL)a}0G_Gep$s+kD z;gta-k%iPOlmE2w5vTpQahEK#l3j{OhLRkrJuBtSN;B)q;PH|2vf#xjpkr4%WuZkJ zDKvVLckO(eW5AEzIbRI+CO_m?eY%zh=;R?FyUADlY}rhPKAjeN&1~_vNlgztgedWm6_vnS7k~YU!E!V~L)! z7E3t)U;RFAPsLqS!{{lZ@aj3bf5g9MT)ha|gYciydw~Y~8oe9rpx^P^SUS8|8Mtld zdP1M87tnnkv5QS+Y4L1Tb+>4il(XAHBKrtlU?f$iiRsRr;xo9@KH*kWE2_;>?d;|X z`mzB0a1;2I4F4d#bnm+l?{h??@#hQHa$Vv~VrZ^-9`6~bPi}m>vV(!dJ`t@Qy=NuL zk4AT!$L@o){a>QB6Utkn%@fKSqSguJ`lxF{`QE5?6hDEtITN&w(IB_ru4u)*r6|Y7 zTV&NaR$280qyzBr%>%z?AU!aND1VOBjx>?_k6uxC{G$37i!+?&V2sOvPbb5cw!SnN zW6~UH8{>Jyp{mOMEK+IxS>+C-cda!8EyhY)%|M|h#a?MYSgEuw?st*5B%a@22_H@T zSH)NM_cp&JWg?E7ZGB79@W%fAD9_-Z0Sw`pK>`fT&6U>2t$(yu+J0E6v}-k$_9ubu zNkH)PfC)*(t2MxaFAdTRBm#^t830iX2qcvf?Z`DqGmuCzGibC~fcIUi#g<~X*i$h6 zHGIi{nSc*CzZn;#YFS3T(^4~FEX}rNAQHfE8ORSRMtb&$)cd^0dbcH8I3T76RqU`g zOC-UoB+Gihnj@M9Tyt}*DdMx-NROq7_O#q@vD7u~M&G-k!+?Oy=2(|na)ec46+hCG zEh3gOC)>JA`~ZD0+5joauvS+u6ZfIK5OCSzPG+R1qUw~iLk;Ml>n=*wK{C=~w(PMI z-AU;Q1AH-#lhR&;(UKz?@$ScHxyNgS78vo~XQ|iNFH6s{7FZ5obe26}-7hjh6{9Q| z^44uAUJRN|b@g?P@qBI0I#ck3J;$E2P8-kXR1=NWR%@&5XYrx7-dtUSc;89M#lZ75 zmTMAa7ksWRhCyiu!^VF>m+_?ZD8t7~E1kxmi>$O^kB#TR1gdwW0b-823si1;*++_5 zBgrHy2tjV_(H9;roN+?sT30?rMheSXmBxa?2M{IF#2!y3yIy+fCDKfc{7ps$pT*RW zy*qwOG<a&@8C#>_*J8?$GHFWimdP4#G(106EN|*5kZ(k< zU&f;0ha+eE(%>_}tqwk&yYb7+J?yB6J& zg38G%ytHuTf2OF!`w9*7Qam>@>uEmVdccVDg)#by{Ld19=sJv49dS7KN-LM=cJleO zZEh6p7+HxkXp-0NT+j}l+GxuS(fQ&l$A0OvJDIt5o6Mh_T$iV{Z*eVfc|LrXbbiU+w)N)rnL(ps z5wFfqu3V&whW`->1NzsX#U90dr2_ZKu1o9N>VE)xPUCehq~%Cs_6z(k&0P`=JJ2Hu zy!&`08vbzPIPSp5?M1>eDQo(E(^NqLEj}RAUCqQa!J{wYhJsS4`~#L2P`_+9u0Ry_3)#Noefcr=;8mlR(>TC z4eO&vOgCFU^?r&|@?tB=p9StZhiecy%5QpGnnyjR*0-@+DKcpI7Oz2 z>+P2s)!H1}H0O0ewS3r{u8D>h4kJbtKW8IGRs6EkJYYn;SIxa_OVYg!AJW^ffsa@& z;qH6_Q7mgmBAkP(zm#$9fpd6~*>9#Ahc4;)$+u+CBNJr41GoWDyjMnC&6 zXOFX~J!hp2u8*Y;4uShASc@bbB6@3v9uGZkas@kGVR?4X#m3}yC$A)5&*J3G4g}MW z4Z3b2!HceAgT`Buz4ZKr_fB`cFJ`TyzNDX-jKOzYMI7Qtl~a3s9NR;%TCYT2z!!tB z7b8}DvG{6>&H}YL>+o0vhWkmwRUn4Kzi(>dVV#OE5 za3PY9FN&cj@`*fw!4qkdZyUVj2*vU5kwVBe_2-?yS;07Z-g^z@ON}2(_qH>8Fl!}d zovdwo2D9FAwYoeuu;mDIkPB3$d2u^n2aH-eUxSyHNa5@YG&cozo3O7IB_(mU7MB#6GNUV+f9HZfUtliH> zsGZ3hQid4i8$M;AG#xj{5)Sf=JLxS`y|hI1*%$0PlZKcpT0{e$%EP@Z@EEr`zM8eT zO(ih*k9=>`>lzIY3`fJ8Mp8md$`s$T3Z*+)ROt7)*=8=paL>BalNm#Sk1fLLv2^v8 zio2a2N!jb^Bbvhjq9R(g@Ds#Q(9Sy$citnGl=cZ}|@Q z4<+nJA%=a_O_EbXOzK77cpoa_1UMHk+%hXYm5N2%aRw#vGb`Se;hb6Aas8coaW*3F zh1L~TMw_!$PwmrCW@kFIQoK4o9&=dKZPg2-@tAYh;xQDnHtyGA&dVw<`SOKXPU_8! zgjc>nZJPn{59-^ZHcr_AdASOGACludkn4N~{)+=9?tCIz@>@hpCfdV9{4Tw7|9Z#+ zq7l*Xw5wyW2m^2`JK(R4DS_*NN#V(L?icqUddzn7fK0lYnCTeKL!g7g_k061>S{w0mc8qH^4(`b#9yW$<7b^lHt#w z`!$*6h3~N{W~83|rxMt9UA-ip^Syzn8Uv5N2RrNSu@}p3E~E2*!*_Jl=VpMf;)QYX z#@p!o!np4d?_yIZy1#jwWHq-g-7WU%~=8Cn6ZS8F$Q_^O2IP3E4wrS0nUHpsy zPawR~34cZ`JRBuAD0@wU1^&Qj_>U3Uw2K#F;b3$Y^{{Rq+g;(W#fa~XqX*Q?MJl58 zZ=rr7e8IeS0G>);dNk<>Kjasg#> zRVY>j3DJ=o+tz&#l1Sgq#fqA6M|d2s{vI*uyNQ-Yb)w`pv!Ncov%BGebwofbT!9I5a z@=kTO7CL=R+D?2_20lDIPTnTUKW%ON8u{NJ*ML}6buTM+l>(*xq z_%XIV2CUBrx*|(UvBz4`*_=(A4@Mj z^OvJ8LGdHpFGeEa1<_xN=OuOXds61^n+3g-%--aoZ*mSmpY}$6X{z!vMS5E-{ELyA z0oRu6UPfF2o=_6JJNiuZq`gGV z_q5CLDrJ0J?B~#|NH{0b=6qKCop=~`3IDm|UIyM5JkD*BK_5^ zW%^q@Rd}H|ySFV=%GT?`o+X81x6||PzKWECx8rR> z%8UCdn0b;H);MWv51GAaV8xMfEei*sKh$^E3)@EED?bEZAbkTzeR&=Drh26?Xo3nt zLP|MKu}mbKJEX+liFJH_Vi!E3LuKyv&ntJ`q{~8GwHuUUhDIfw$4Gcx4D=Kw>1QGK zmy{Iw{N0B(Sl;_Pd5BVp=q0mED%gGf=eu)~VegHQmER9_4i8D&g`0ILSXZ)dBzzm* z8KuB}vCK&LQUtpOQz?vu^G0h4YWKz>;g_zO@e(&#yHdE{r_{Fesqgrboh9p@HT}(dR#Lg% zku24GW8%EAa#IUcN!&pDX%gSlOIp99$`-K(p82ytT2dM6Tbp;|aCtpq0%BM0S z(WzhbU2a1C!$Yfc$_ z;E#^?`O^28vG5Z^yd~E;S@SIJ9^aN&STsG}V8re9xVKXj=S_XC%GWF%H95|XB_B(( z_mF&hFvjNCU`GwvwmRoZ+bUbmIy+tk=h(CDb?a8zSJ`tmz_YnDr#`1Kp0E97wOTX| zxSG}0tR^yGY(9#=L&2Nx0hb}`{_Oj4Um75rmsx*iv1*O^+aDd}uk>W;@-dEaC^JY`$c>b!&H|y#jwvZceUrym+ z%NhWay!>>ZN_)uhsH8l4x-VnicPyvzBE2Sf$UMAui>2iLEtaj-58JyOUG|70V!xyQ z4#&gw4?DW*-|1skJSLH&bykJ;Icttp9?If&O1eWkkN%-gj=#D{oQM(U*G&p3aYK(4 z5iK-vzEL2$4r_<~4}FtXY+d_@KCCb{Q=q}0$mD$}j6eA8FjFL6X$C2<KU({l?TEO#PFGpjTeRNeJHh2Vry7y`*=#nOFXc%?`S7hM5LG{iL}EtavqVW3+WPQ~&Jr3wRBdgQ&r3F@~7x%6Y6#w+Zieq$2US zDakX}c@VxAoU^;1tjzb-44m48ImYj@KS0dEt6CfXK%@kdh*d>CnumUaH)ifsF-D@u;OW%wDa==_lfEX-c2 zNket4ZFNqfLfUw~h**9@E}rE{snyG^0r%AB{E`werl z?fLnuY&5FDFxgga(69`Y-Er<;A=%EeXHA4_GeI)M-U3z=vW zXpECu2L&muEUj*{wZSIj78xyeJCkqxf>*IKPxnpc>6_creN*S|J0%P&X6{}Ef`gFgno?y4AFJNcJu$8;A| znlC3-ZLM#nO0t3S^oXpbL0+4{6uX z*sBM1X_@e|+)6ZeArDd+2kZ&#JBN@T#W`OMJy>R0j~BnpZfk?pGV_<#+bc)Hg?QbFzhG%OEoCC*wY)DL z@0@38qdQfR13my6forp}I(;*Z*HX?azvMjEU0C^pPYW$RADQuJoS(C*C`V^4fy6e` zw=s~#nUuG;S$8=8=up;c7T;#Y?(jvdMT2;D*~n?B*!8L7hWcpuqi8)&PL%Ur_&bk8 zUvQk37P@GQ_sf>kXgQ3QPtj&{;XT@yEx$p_kD`C_^_F2RX#5N7W6UH*j%ZkFugF5f ziKAEpCGz+>VZG@2aBTm-_LudEn(=!-eKq&ha`3euD9oPa(@5dBJpX=-6WHF&P;6fD z8BSvK&nYA?H(P)Hy<_Em*$tCt@)0lg>{Qv;x0?E5u8g)kEjB)3x|(S$=Lt86V{M+3o*!EUyeBE#ju8?UPP^J!ci*-Grh zMq02KOGy*Ho5&X8Bo49}y=HLBNdwt{bU$D;#DrGbz5%~B1Rfj6rY~_*d^{nXb1C(u*=oh_zKO}2W6ZSxjkgYEv!6A*%4hq$!CR{vn4`dYbEMC}AvD!`b( zSYO`a-16mkz`x9Sf1_eo4!=Xk2cqDolrr`H9}=m)B40zoc0H5o4^R@nQ80VwGErTGDEaiG>mJetKGD9 zY__kQK>N~$4Gp%<4dok~w>+4@G0F?5&qqyL?q9RXwsDOOYV82wad=z(C>j!PU+Y}k zwC1~;oLd^!tliYKag9ArNMe>-H`^!B@5FH9y1#<)rHq^pE4g*!W8u91YIzB-U&UH_g!>{24d*uP|t=|ME??#?2d@|3o1A zPvrkN-o^R%z&(^P*Z5_hSg{5JtZ8UAI2#lE8Y^Rs;uXv8))gmO68u2<_OF!ZeWiTs zSIYChQoe0`c}3%<2HOVbLk;zb+7(O6%7v;Wx8 z=+eiR$%Q&28f3_6{k5GV0fq64$gEjQ&E_@BA8c+YYgnr(EYQrFWoVgaNc>x&(Q6DX z_?}D4D+=PpqGI}4aMQQU%a&?x)Bt?W3eCcW_(1Kr>71bEx*q8>7-IEILJs|aAx6_= z;`;GELM{%<$iE*b8BZ@j$&mu`GA`T zn~$79{xtGd)aTRkSK+}w8`7Lq^g)_9{`k^Af5-m0>PuZ4uNU9+ZQ=|#!v1&uBR{5d HrbPd5YXDSl diff --git a/test/uavcan_tester/main.py b/test/uavcan_tester/main.py deleted file mode 100755 index c3b5d35..0000000 --- a/test/uavcan_tester/main.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2017-2018 Pavel Kirienko -# -# This program is distributed under the terms of the MIT software license. -# -# This is a tool used to stress test the Zubax Embedded Bootloader. -# - -import os -import sys -import argparse -import logging -import binascii - -try: - # noinspection PyUnresolvedReferences - import uavcan -except ImportError: - print('Missing PyUAVCAN, please install it: pip3 install "uavcan>=1.0.0.dev30"', file=sys.stderr) - exit(1) - -import uavcan_tester - - -def _configure_file_logging(): - log_file_path = os.path.splitext(os.path.basename(__file__))[0] + '.log' - - formatter = logging.Formatter('%(asctime)s.%(msecs)03d %(levelname)-8s %(name)s: %(message)s') - - handler = logging.FileHandler(log_file_path) - handler.setLevel(logging.DEBUG) - handler.setFormatter(formatter) - - logging.root.addHandler(handler) - logging.root.setLevel(logging.DEBUG) - - -_configure_file_logging() - - -logger = logging.getLogger('main') - - -def _configure_stderr_logging(verbosity): - level = { - 0: logging.INFO, - 1: logging.DEBUG - }.get(verbosity or 0, logging.DEBUG) - - formatter = logging.Formatter('%(asctime)s %(levelname)-.1s %(name)s: %(message)s', datefmt='%H:%M:%S') - - handler = logging.StreamHandler(sys.stderr) - handler.setLevel(level) - handler.setFormatter(formatter) - - logging.root.addHandler(handler) - - -def main() -> int: - argparser = argparse.ArgumentParser(description='Zubax Embedded Bootloader automated test script') - argparser.add_argument('iface', help='name of the CAN interface, e.g. "can0", "/dev/ttyACM0"') - argparser.add_argument('uid', help='unique ID of the device under test as a hex string, e.g. ' - '37ffdc05465430353344164300000000, or as a base64 encoded string, e.g. ' - '"N//cBUZUMDUzRBZDAAAAAA==". The string may contain spaces, they will be removed.') - argparser.add_argument('valid_fw_dir', help='path to the directory with valid firmware images') - argparser.add_argument('invalid_fw_dir', help='path to the directory with invalid firmware images') - argparser.add_argument('--verbose', '-v', action='count', help='verbosity level (-v, -vv)') - argparser.add_argument('--nid', help='local node ID used by the script (default: 127)', type=int, default=127) - argparser.add_argument('--no-application', action='store_true', - help='do not make assumption about the application; work only with the bootloader') - argparser.add_argument('--duration-min', help='minimal testing duration, in minutes (default: infinity)', - type=float, default=1e9) - args = argparser.parse_args() - - _configure_stderr_logging(args.verbose) - - iface = args.iface - duration = float(args.duration_min) * 60 - no_application = args.no_application - - try: - args.uid = args.uid.replace(' ', '') - if len(args.uid) == 32: - uid = binascii.unhexlify(args.uid) - else: - uid = binascii.a2b_base64(args.uid) - except Exception as ex: - logger.error('Could not parse UID: %r', ex, exc_info=True) - return 1 - - logger.info('Started; iface %s, UID %s', iface, binascii.hexlify(uid).decode('utf8')) - - try: - t = uavcan_tester.Tester(iface, uid, os.getcwd(), args.valid_fw_dir, args.invalid_fw_dir, args.nid) - t.run(duration, no_application=no_application) - except Exception as ex: - logger.error('Test failure: %r', ex, exc_info=True) - return 1 - else: - return 0 - - -if __name__ == '__main__': - exit(main()) diff --git a/test/uavcan_tester/uavcan_tester.py b/test/uavcan_tester/uavcan_tester.py deleted file mode 100644 index c8b5283..0000000 --- a/test/uavcan_tester/uavcan_tester.py +++ /dev/null @@ -1,395 +0,0 @@ -# -# Copyright (c) 2017-2018 Pavel Kirienko -# -# This program is distributed under the terms of the MIT software license. -# - -import os -import time -import glob -import uavcan -import random -from enum import IntEnum -from logging import getLogger -from contextlib import closing - -__all__ = ['Tester'] - -logger = getLogger(__name__) - - -CAN_BIT_RATES = [ - 1000000, - 500000, - 250000, - 125000, - 100000, -] - - -class BootloaderError(IntEnum): - """ - Error codes defined in the bootloader library and its dependencies. - There are more codes than these, but they are either implementation-specific or ambiguous. - Over UAVCAN these codes are reported via the vendor-specific status code. - """ - OK = 0 - - BOOTLOADER_INVALID_STATE = 1001 - BOOTLOADER_APP_IMAGE_TOO_LARGE = 1002 - BOOTLOADER_ROM_WRITE_FAILURE = 1003 - BOOTLOADER_INVALID_PARAMS = 1004 - - YMODEM_CHANNEL_WRITE_TIMED_OUT = 2001 - YMODEM_RETRIES_EXHAUSTED = 2002 - YMODEM_PROTOCOL_ERROR = 2003 - YMODEM_TRANSFER_CANCELLED_BY_REMOTE = 2004 - YMODEM_REMOTE_REFUSED_TO_PROVIDE_FILE = 2005 - YMODEM_PORT_ERROR = 2006 - - UAVCAN_TIMEOUT = 3001 - UAVCAN_INTERRUPTED = 3002 - UAVCAN_FILE_READ_FAILED = 3003 - - -class BootloaderState(IntEnum): - NO_APP_TO_BOOT = 0 - BOOT_DELAY = 1 - BOOT_CANCELLED = 2 - APP_UPGRADE_IN_PROGRESS = 3 - READY_TO_BOOT = 4 - - -NODE_STATUS_CONST = uavcan.protocol.NodeStatus() - - -class TestException(Exception): - pass - - -def _maybe(probability=0.5): - return random.random() <= probability - - -def _choose_random_path_from_directory(directory: str, glob_expression: str): - return random.choice(glob.glob(os.path.join(directory, glob_expression))) - - -def _enforce(condition, fmt, *args): - if not condition: - raise TestException(fmt % args) - - -class Node: - class TimeoutException(TestException): - pass - - def __init__(self, - can_iface_name: str, - bitrate: int, - can_dump_directory: str, - file_server_lookup_paths: list, - self_node_id: int): - os.makedirs(can_dump_directory, exist_ok=True) - can_dump_base_name = 'can_%s.dump' % time.strftime('%Y-%m-%d-%H-%M-%S') - can_dump_file = os.path.join(can_dump_directory, can_dump_base_name) - self._can_dump_file = open(can_dump_file, 'w', encoding='utf8') - - self._node = uavcan.make_node(can_iface_name, - bitrate=bitrate, - mode=NODE_STATUS_CONST.MODE_OPERATIONAL) - self._node.node_info.name = 'com.zubax.bootloader_tester' - self._node.node_id = self_node_id - self._node.can_driver.add_io_hook(self._can_io_hook) - - self.file_server = uavcan.app.file_server.FileServer(self._node, file_server_lookup_paths) - - self.monitor = uavcan.app.node_monitor.NodeMonitor(self._node) - - self.node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(self._node, self.monitor) - - def logging_callback(event): - logger.info('uavcan.protocol.debug.LogMessage\n%s', uavcan.to_yaml(event)) - - self._node.add_handler(uavcan.protocol.debug.LogMessage, logging_callback) - - def _can_io_hook(self, direction, frame): - # We don't flush for performance reasons - self._can_dump_file.write('%s %s\n' % (direction.upper(), frame)) - - def close(self): - self._node.close() - - def spin_for(self, duration: float): - self._node.spin(duration) - - def spin_until(self, termination_condition, check_interval=0.1): - while not termination_condition(): - self.spin_for(check_interval) - - def request_async(self, payload, dest_node_id, callback=None, timeout=None, priority=None): - callback = callback if callback is not None else lambda *_: None - self._node.request(payload, dest_node_id, callback, timeout=timeout, priority=priority) - - def request(self, payload, dest_node_id, timeout=None, priority=None): - """ - Synchronous request. - Throws a TimeoutException on timeout. - """ - result = None - - def callback(event): - nonlocal result - if event is not None: - logger.debug('Response %r from %d\n%s', - uavcan.get_uavcan_data_type(event.response), - event.transfer.source_node_id, - uavcan.to_yaml(event.response)) - assert event.transfer.source_node_id == dest_node_id - result = event.response - else: - raise Node.TimeoutException('Request to node %d with payload %r has timed out in %.3f seconds' % - (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT)) - - logger.debug('Synchronously requesting %r from %d\n%s', - uavcan.get_uavcan_data_type(payload), dest_node_id, uavcan.to_yaml(payload)) - - self.request_async(payload, dest_node_id, callback, timeout, priority) - - while result is None: - self.spin_for(0.1) - - return result - - @property - def node_id(self): - return self._node.node_id - - -class Tester: - def __init__(self, - can_iface_name: str, - device_under_test_uid: bytes, - can_dump_directory: str, - valid_firmware_dir: str, - invalid_firmware_dir: str, - node_id: int = 127): - self._can_iface_name = can_iface_name - self._device_uid = device_under_test_uid - self._can_dump_directory = can_dump_directory - - self._valid_firmware_dir = valid_firmware_dir - self._invalid_firmware_dir = invalid_firmware_dir - - self._self_node_id = int(node_id) - if not (1 <= self._self_node_id <= 127): - raise ValueError('Invalid local node ID') - - # Checking that the paths are valid - _ = _choose_random_path_from_directory(valid_firmware_dir, '*.bin') - _ = _choose_random_path_from_directory(invalid_firmware_dir, '*.bin') - - def _test_once(self, bitrate, no_application): - logger.info('Instantiating a new node with bitrate %d bps', bitrate) - node = Node(self._can_iface_name, - bitrate, - self._can_dump_directory, - [self._valid_firmware_dir, self._invalid_firmware_dir], - self._self_node_id) - - with closing(node): - node.monitor.add_update_handler(lambda e: logger.debug('Node monitor event: %s', e)) - - logger.info('Waiting for the device to appear online...') - deadline = time.monotonic() + 60 - - device_node_id = 0 - - def is_dut_online(): - nonlocal device_node_id - - def predicate(e: uavcan.app.node_monitor.NodeMonitor.Entry): - return (e.info is not None) and bytes(e.info.hardware_version.unique_id) == self._device_uid - - matches = list(node.monitor.find_all(predicate)) - if len(matches) > 0: - device_node_id = matches[0].node_id - return True - - if time.monotonic() > deadline: - raise TestException('Node did not become online') - - return False - - node.spin_until(is_dut_online) - - logger.info('Device online, NID %d', device_node_id) - - orig_node_info = node.monitor.get(device_node_id).info - - def get_node_status(): - try: - e = node.monitor.get(device_node_id) - except KeyError: - raise TestException('Node is offline') - return e.status - - def match_node_health_mode(health, mode): - try: - ns = get_node_status() - except TestException: - return False - - if health is not None: - return ns.mode == mode and ns.health == health - else: - return ns.mode == mode - - # Optionally stopping the allocator - if _maybe(): - node.node_id_allocator.close() - node.node_id_allocator = None - - # Selecting the firmware, possibly invalid - using_valid_firmware = _maybe() - fw_path = _choose_random_path_from_directory(self._valid_firmware_dir - if using_valid_firmware else - self._invalid_firmware_dir, - '*.bin') - logger.info('Using firmware %r, which is %s', fw_path, 'VALID' if using_valid_firmware else 'INVALID') - - # Requesting a firmware update; about ~50% chance of providing an incorrect server node ID - begin_fw_update_request = uavcan.protocol.file.BeginFirmwareUpdate.Request() - - begin_fw_update_request.source_node_id = random.choice(list(range(1, 128)) + [node.node_id] * 128) - using_valid_source_node_id = begin_fw_update_request.source_node_id in (0, node.node_id) - - begin_fw_update_request.image_file_remote_path.path = fw_path - - logger.info('Sending BeginFirmwareUpdate with source NID %d, which is %s', - begin_fw_update_request.source_node_id, 'VALID' if using_valid_source_node_id else 'INVALID') - - response = node.request(begin_fw_update_request, device_node_id) - - logger.info('BeginFirmwareUpdate response: %s', response) - - # Spinning for a while in order to update the node status representation locally - node.spin_for(5) - - logger.info('Latest NodeStatus\n%s', uavcan.to_yaml(get_node_status())) - - # Making sure that NodeInfo values reported by the bootloader and by the application are consistent - def check_latest_node_info(): - curr_node_info = node.monitor.get(device_node_id).info - logger.info('Comparing node info, original vs. current:\n%s\n---\n%s', - uavcan.to_yaml(orig_node_info), uavcan.to_yaml(curr_node_info)) - - _enforce(orig_node_info.hardware_version.major == - curr_node_info.hardware_version.major, - 'HW version major') - - _enforce(orig_node_info.hardware_version.minor == - curr_node_info.hardware_version.minor, - 'HW version minor') - - _enforce(orig_node_info.hardware_version.unique_id == - curr_node_info.hardware_version.unique_id, - 'HW UID') - - _enforce(orig_node_info.hardware_version.certificate_of_authenticity == - curr_node_info.hardware_version.certificate_of_authenticity, - 'HW COA') - - _enforce(orig_node_info.name == curr_node_info.name, - 'Node name') - - check_latest_node_info() - - # Checking what state the bootloader is in - if match_node_health_mode(None, NODE_STATUS_CONST.MODE_OPERATIONAL): - raise TestException('The node did not enter firmware update mode') - - if using_valid_source_node_id: - _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_OK, - NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) or - match_node_health_mode(NODE_STATUS_CONST.HEALTH_ERROR, - NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), - 'Expected update mode') - # Continuing with other tests - else: - _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_WARNING, - NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) or - match_node_health_mode(NODE_STATUS_CONST.HEALTH_ERROR, - NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), - 'Expected failure') - _enforce(get_node_status().vendor_specific_status_code == BootloaderError.UAVCAN_TIMEOUT, - 'Expected failure') - - # Maybe restart, maybe not... - if _maybe(): - logger.info('Sending RestartRequest') - r = uavcan.protocol.RestartNode.Request() - r.magic_number = r.MAGIC_NUMBER - response = node.request(r, device_node_id) - _enforce(response.ok, 'RestartNode was supposed to return ok=True') - - return # Nothing more to test at this point - - # Waiting for the bootloader to complete downloading, then check the outcome - logger.info('Waiting for the download to complete...') - - deadline = time.monotonic() + 3600 # An hour - a ridiculously large deadline, but whatever - - def poll(): - _enforce(time.monotonic() < deadline, 'Stuck. :(') - - if _maybe(): # Generating garbage on the bus, because why not - if _maybe(): - r = uavcan.protocol.RestartNode.Request() # Invalid magic, this is intentional - elif _maybe(0.01): - r = uavcan.protocol.GetNodeInfo.Request() - else: - # The bootloader doesn't support this service, there will be no response - r = uavcan.protocol.file.Read.Request() - r.offset = int(random.random() * 1024 * 1024 * 1024) - r.path.path = \ - 'Senora, pray receive with your wonted kindness Senor Don Quixote of La Mancha, '\ - 'whom you see before you, a knight-errant, and the bravest and wisest in the world.' - - node.request_async(r, device_node_id) - - return not match_node_health_mode(NODE_STATUS_CONST.HEALTH_OK, - NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) - node.spin_until(poll, check_interval=0.01) - - logger.info('Download finished, checking the outcome...') - - if using_valid_firmware: - if not no_application: - _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_OK, - NODE_STATUS_CONST.MODE_INITIALIZATION), - 'Expected successful completion') - logger.info('Waiting for the board to boot...') - node.spin_for(15) - _enforce(match_node_health_mode(None, NODE_STATUS_CONST.MODE_OPERATIONAL), - 'Application did not boot') - - check_latest_node_info() - else: - _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_ERROR, - NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), - 'Expected failure') - - def run(self, duration: float, no_application=False): - """ - Runs the tests for at least :param duration: seconds, or until first error. - Raises an exception on failure. - """ - # We can't change the bit rate at run time because the device sticks to the first detected bit rate. - bitrate = random.choice(CAN_BIT_RATES) - - deadline = time.monotonic() + float(duration) - - while time.monotonic() < deadline: - self._test_once(bitrate, no_application) - time.sleep(10) diff --git a/test/uavcan_tester/valid-images/a.bin b/test/uavcan_tester/valid-images/a.bin deleted file mode 100644 index 8b17673db3246dd4a589818e2805799fd9079a0c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10504 zcmc&)3wTpSzMu0*a@ro+^nr#_AZ-u|<&jdr3RO;$_M|VGmdE|{S|n|)1VI{kXczap zO?gNaU13F6lwA?nWnECpb3y2yQc&EBx=AZ52xwh}&=bo&NmClqNA7=;mKH7hxZhoD zzsa1L`OklT^MB5q1Syfa

6_&LWu-C`C|oP*S0kLjS<>O$2dUyL$5N?SZua>T_y5 z!8i_3OlmE~Y<`crZJmFWS0DX56K=;>opO7-kM2(O(}({)4q^|*l>DAzPX0DT=g|IZ z{O`2x|7X9hFtdC`nO0jnA~p5n#7EuCID{&WRL(_2Ao!Phm0|*4y;uL$|FJAT@<6t& zjj2h8&oKBbxWN>C=0OBw<~#^}0;S-_)Tcu|LYwIop{?Q-JW}tGV5Fv(wr|8FA@t34 zWq84jjNUnNwXH3#2G5F+-t8u(S$>l=Upp#ichM@D&5q@Vv&aeTk_?-CM;dyWB z9}p_`C{EQ=@nXP=%yuG7t*Y4i>^uLw;PcsY zOtZ$H-@Zna)4TjHd6ynv3$3-HLiSt#OYN^6D{LV(MX>fjnt>Ge87|tl9h>;UVgGvX zXuvYoPrEMzy~a9~|6mpn!t45DPXk}xjm|JW935s4)1sIa!1Pkv+#TXo{?ktF*U!yr_8e& z2{%n(+D{ZAzVyuO^P?M+q^eAebBh>zAC_vkqx=|PpSeyX=|e86A`#*~6ADPuD!CiHR28ibRH?dN3BHV+jOvXUJUH85X4t{Hz) zVuE3GGsv6n#Zx{Am&sxMT*`(t50wH@GDn03<|k>SDg)h&JsYF58e={f{sD0Et`RA0 zJ?+>ENYE$++6Q`bNb|8~s}cLmI$kFtDpsPXrySf2T@#i82j!=#xfykh+9}ch5QlO` zYa=^Fc8iXlakG)0!q^)X2|~1wn2DeeO%XMe&CFx}WyZ%gZB)E$2P>jpw4ZX*RAcxYcux_keYsK99Vhyf0`H=Kf3z-G}PT*Le z1b^G?Q%jtbV*{Pg6%Yp;El9K3$4SV|Akd5lwBiAOZQJ30sV(og1H2jc;?aR4-yURWugqC5k~%QFs9BumU|41V+53(Aef{;Dike}RvvrL&9D#$KSPq-ciBfb zLwwQDwN@jLkl$Q z(axTZQU3K^k)8L}7(~XP=w#hn^olHzy$Z9fPJvA53+G9aQxJ^j(;S6 z=K-L^dx$fLbSBe@eA}}Wy?Y=Vw`U>G5u1wN5~|cXI4<2;r<<24A9dwmgthk9a)Pnkm)Nynql1bPV$9a zu5da(tAWqs=QS1C(WJ%p{jPA?<1UJ({cKk^*Tv{qkiHAa@M%av+>opw2o;} zkQUPc#*BM`ZYW?Jm-**-(7AD@^L}2-hdC9z^QwyPysFFE(|FWHhA(zSl0k%T3Vd-0H|77fF9)SCU`mv8i-!L{R=ncry59ZI9?8n}6$8%OZh_7!Unj z$wIzFvUsdsRpk2iX+lC-igV!mk(buNG>UD?VHS#7h#VO# z4ePm|O28T&XYbdy%#QU<3&VIN z%-bwdQ6mMEh}3xJ^NM8-4&~^Q1szz8wl1#SMrQ~_yDeH%1sYTZe>ALHVIXs}U^FYkQn_PoaT_3<^=`9DG~} z>tzd^9@9JoG zSByOWr#?T3?DGR_9jwCh;N_f1B0xtRauw4J!sjklrW5HRYaPZ02qK9L)V9YK=bj$* zm4MVBX`K};718R9X<*aC`)pcS*NJlCwV-URMheE~AOqy1ScnKP{$PjQAj&DL5$ll!eX0bkP1as0 z>f~5^2D!t5<%ZV=IU#Eu9+3-5ei)BOhk-w`QKSR3Jw4We=gJ)5Gt(1-6ydk#>IV25 zNIlr9B&>CR-==o^cDZCUtRIQ}5XWUC)j&k2(oB+VewG*;YY=0~z>AWy#OR?}qO#m1 zqA`(x_1EA-uj@12-82nWL^eLkn8YdMk@#{KB?~>9yf&rDizXa#k&&xhRAiEHBz_eS zIUn`X{-D434*lEi(0>Wd7v+&EyGfJ*vU^sUMEN}?@f2kc=d3V^4d90f3H1&fnIjx< z4+usgkZ}GHC+>q7Z9wXN>z;LE-}{GM6hu3~%MOh7);id4K>_5dxI5bRrmIjkb z!&qNTP|LHtGxyNSbtUC`}Se&PH^Iu9BXshEN}PwxG%}sZx>bG58sCt zUF99J4=h1e<#oLubL(9I&f5?zGUSl2O%lQ4yp8MGKKfJ0+>oJ{7DM)iNQ9q;+lgA> z#j@DqSR8N3U`9u3t5NAjQMl(}VJql#lYWA5{@x1lTPr8w8+0G9pao_q9GEBO_*aZO zqshJi-jU#Q#5%|krTj~#E5sncm;IpqE3Are`5%Y^~ z9CC|a>Y|D|&k*h0VxC;Eh4+mF8?>>VRfGTQ(I_|(>8M|VI7N1qWg_?lt0D1u7p%r(aFMrc4Nj(TW(|!Oiuw5T(l;(% zvWEQWxns-WKK&<<1R&DMn?pD2ieP37XagN*`ft|b!Y-ne_=|y#`M2G$Lzq#@a2UxR zFcRty-3qM6P#x&F|3?)4W8(&l_03aU-xS_{y?(X(y3$X4j-;_inZwwok(A^Mt-#a` zybq-xMoI8afgi#LI^OM0hPQ6x;D((J^iz(id6^sOkTkJ(W;&c*?GW=$`ccx5JUJf) zbMEa7bhLNV@U#Vb!}0gwIsH-uw)gg(}%y;@`F!xrKvvy{Iw)aU?ZjoiK<gTWTUTp&@I$Fsm zL(jx&rjE>`a;fY~iT6ljy6u@XV||@w&O@DkSjMwp4+Oj&GkO19lqOy6rt-|@6mywn zi8-0aHf7l_WBO!dJq!{ud>7AKw&;_EI9|pl@eJ=)^>n=0ow8VynNp(WtuUvLJ{{;u zpqKGm^qd9HP-Q0ZJsm5$T^3EI%dCbK`e1&EJ=&nb~YvE&etG2{(TB<+Rs#x2(95dU;jVS?9{H zZ@g8!XhX~5>ut|&8%2G;;$ir{L^+*iD#syp;YLE;w}-odH|w9lJ9X=Vo?o<;q0Wrf z__Iwb=k?-M{RDVhZw@Zs=f4nEUYVX?etY+VGRgm|$CjNUo@qO@gxy!wQvQ3un*j3l zY45&#UmF~6`P;j1;eDpf+lTk;Lngqxs~7JZKLOqc2gfTD{wOT=_EEaF#FZd~0^p+h-S+AM2W^IPY9MDSbx9 z1<5n~{ZExy;^GwtB1MTNb1k0#%3yi=vifkEwIfPBwCBi;Pt zh$r51Eh}5?Z`2M~5cB*ux zXHb1-jjTGeYWdGD%nls*<_Vm)l3o$YRvkO>{qL$SSZe_%&Idv6nTqKT{(KV1_l;h>m3KWK zfd6;j$sR&@vA<99_?A>1Y$Dv#AAA6=rScHFH!-4&#{~#ZvAEx;KX;ZFgRLIYT~x+iSC-9rPDT%vi~%HQHxO#`Oq!`T(#$%j`_7)YMoFYuZ(d_=Fq0+vk~Jj_C1k0- zbWLePDOsj3vl^vC@8rH?id+0n_Kut#1v_UWO(y&@BCaI1ByLIYsjSggSpS>+eE1fi z0iLowD)x+zuK(Is-*CpqKz#ygw%+8e%s=CcgLW;n)llz+dUQSO{%ii;_Ih@&XDp1P z`h1T~VKi?vZGdvdob73T)>!g_Nm0sqwmxettuZOgCa=}H!IW*@X#R^iyJUSfQ<|jC zE?sAWC-eI373PEH4zqIXiV~uIdr?~HiqiYdCg~5;t4vGGd(79(OG;8omz2V7#r*Tx zJIzc908exumJlz}%w>5%_ zh59W;*SS|MkK}0J7YwhOH(FvPmg;nbTjpnz(oHfPW=2>HURzp(nXMqNzl8f=8{zpM diff --git a/test/uavcan_tester/valid-images/io.px4.sapog-1-2.2.7719d6e.application.bin b/test/uavcan_tester/valid-images/io.px4.sapog-1-2.2.7719d6e.application.bin deleted file mode 100644 index 6804a67a4b8745f9d52ffe7a249f5e4814c35912..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 193992 zcmb@u33yXg{x^Q^P14@BX_~em-ITOpDSM!(6>vz>_BKr_DT|7NlD4QR;7~+IMje|j z2ncEc7g|sY#Q{a21yL3;6^ekO(^8yam5M+i)%o3<0^c|Yf-Wti>vKJWke=XuUO z`#In9J>TtYA#$QwZ6I9zMuyu6w;HY#ZV}wnOXQ#2*suRNUVr?bB-|FW7{lE9U@#ufg`)}!ro79J|Sq-@m^olw||bueq9V!BFt~+~+AK$BnS%tj2Mp-*V@Tg>Qum zFNo2?%l^Afcjw$=UTvJ@G>GyDZ(CB%fIDLGISJ+jOSSQL&Z)>X!ijA}GvJ@fgbV3n zxWJ3Cx&0pc)F?YULzE{j&+cjq5k~sexzYcW7A_3mOE{u=7(YUYhJa3}yEanB(ezVl zqG^Sjw3}!gpr4969LnGRjk+#azZR8Ehwbn42d@bj%}-uVc*RS!~XF0)ze8Mc-7a@%+I!?uVb zS-zzxBY$O4dH#1rhw~%m$qFpFeDT_@$^xTce#NzQ1InMmN6mWbsthm9}J zCQfd6Y|F~f`X)6X+|hK{Etj24ICn?c%`Ia^cA^IE!#4SZGcC-7zE-+UMl6vPTb5)L zuUt}IEVTx#Ib4h~NsvX|-;5TO8bL;q1sRhk%13Z?W+$qeRv~?%P?BMFolt7MNSmVwUKd-bg^j1F`3q+B|4g{F4kss#iTH)sm{7~*+R~l7~a_=69O`# zYDg%~Mt@P^O=^)DK_O&@r#h>RolTqCWu!R!AyNJWZHW7mmpLP^%1 z_SiABU)pD6GkTpAgf+Z*sf$(YL0`&dTKig?)fKCq)jCNi$(_+0u>{aOHKUb&DHK+O z*eu$P)zv?XwuN@Bad)&A+Si^nOlt3H+Z>lNcT97{awZOBi)hd7izAVmG2muz55{kh}queR?nXJr3NV$t)`r`METJF>xj24L1>SD%PI~_^6 zuIRCBlirx$ZeU`ibXi)iD>jX7nr19)&ydlciLk}7+0Dp*bZb<4JlIAtUO>O^W5hIYS&>w*f)p<8jzKU`G zzMWy}Z#o7hw$$Gw1C=c~+#HvVSY4Bpa`b0)WloE^>4FZbA(0 z@~H-wg3%qEy|?rR=hURtx+7$}-5OSTRymHl zz`FY>54n{r<5ddDUe>K_MGAY`dC09%GKg!y73ZA>my)T}UryNMWYF8N4Bf$nF1dW* zq@=UT@v8H3cRzU}TGV*u0_xn*yTEB)`Hd`>3{W+?mA$zva^+I)FkW{MB?Rif4G7CS z9kC(K(oRP-GoE>HN*kTeI47OaVNL&-Z$%TGZ@Dv`SEJr)mn`ZYCrKt6I`SE_70r5* zV@P!xKsCC9{cbwslbp;=N3`xB#dVg@V=#0UGnD>x2Wk5WQaiMdBxeNA@_8<1e28e-V!&+o7bGIoyZyfV%y8`tNv?dMs{s*HGLevOyTg{(*KQembrBV`|#T?Wz;)xMEFS@nF&4$Np|H<)w*)01Co2=Bzze# zZyA`u^rjjg`vb+!&p!4_h(L_3KG_w-Lw@$j9@&6##yO`5jB2wxApiU(DVcjy|GfO< z9M=@pvj)CBB*f_YCfVS6c&gQPM5A{-nP+wVF#0ohw{p4j%bTqW?>YTdAKk0^=)U+M zdf|C+wcf=NK*xO6K3jgsd8%EhFu3GXiw{!R0D+2`L~d+z^mE@A^^=omfMG*1D3i!F zlkNo#C-%{>f`CJVbgby0ZK9+x8Ku?jEybYNTgLVf=1EtE3Y_F>DW+{m)Qx&_)KgFj z@2e-(p>pA$%WG&K9aCJa3jN$*5YBM1GRh4UD)=25^h?m+o%JY}L^Y!XuLs8mIh=Dc zlRQlZR|xZtgxlh58S{&t(XqvzHOl z+)VLC`v#s!@j;ps;#BkTJk5;<|0$!`X3X||vCXuuq8U7LUy@WJ{B~W#9sC8XOR*(P z@xlL{rnZQi+ZA&3ucCZM^u=by$|+nzq>LIt2bpN@{4Pk~pFn zc9$f1fZ^QF+h`hOat6`-gSsc*+PMST9nuZ(D({r-#RoCoDN~@E`u$hhuDqHBSl+lw zXPC|xC(ZKY&WSr|3|bkot)z4OPC9=jor8DM*&W$NVZE}zZB~Q2G&wvTM`9M3T>us(iZ

E{*tul!?)HLdF=wo61TFBlwXa|gq@>@3MMyF^-j-GQa#J4Yj_ zV@`B^DHNVv?$(S!&fPaj(s_5!?8r0V^IWT|fz5U074L#P)wh=u$TA7n(SpO8+#Ksp zf9^{s$^jAsm7cC?(+;`gsP0gzy-G+q34YX|kXT(US{ZBv%2$Bq2Ifb00{XjPdMKs3 zw0l4mJ$iko9wVh~>ZKSRH*q=;r*u;aem0VUuxJdjHK2qNSIUACmv+HC@K#L0$@bX7 zubn6340@B^px^79WsD!O92yZ?)~fG>y(pCEDLyG~p|zQ0p~Mj9%rVlICO5%eWew1H z>UJeL4DAQ4=O&m9_QxEftzA#*Zjz|jmO8hiB&0Z4*#caPPwaH$r4ylpEybTp}c9vxQs8IP5QWupmr|>R3T0W zU}eTJ6ZgP!H^}H1DQ{>OBsWn=Aof%F^_4T@qWc0rdbnYY@My`znD((#5j=};=pITqj3%@{ce_DN&kjcimitt)G`081Rt-LCf!{tJdEUs9{@-|<{T*Q+eWbPj8E={5HQ&yMLC zuIT6tmpYc>c%?DjIg1$Ge`29$l`D;doU;<>$ac3+jT+>vZJ!GK%|>js6kCnh2E>}B zSTkbhNg_k%Fe$0ExqJBJTX{QI8uj3xK6ylEh0aLHxc@vG_qYAO;`W`}xaEMOlFmQv zq*yBHEZ)iSe`up&IWRW4GZ*pSxBUa}-!>y`Ea{xR6SI&}(m8489N6anUH|EUu{(J@ z8v1XVYdW+c<;a=NUDyd&?mRXi-MP^9b1|fsB!8()V4LT-^dl9`R5Dyf*f8?vJ9He! z=ktNKDCoB1w{=_8{|~zD0`ToCzsucY zjcTz9AINsM#}q_D8xlzyszI&)stIG<`4a6auwV0U+7JyIiRvV6FXDE4VbF%b_U_b# zeeL|SHYCoM_dU0d4EqX_6_6T2S~(|4O`hpBeRowNPVhu+}Sg)OzyHT2A#Ik}T%~o(BS&eH!f8sr5vN+$(nz_+esD zo~p6N47N^46Xx~0NK#am5Izd^?(YhCx_z{TFOeE<33x_#v3vxt;~(IAJkJLLp1=FF zJV`OS2urQECaEpqK?n!~Z4>*2hL8=FJZp11?!9706tHlx0SD`nFk z%=|3@;)Sd^9ZY~GcLvOBUkl$A+kJ&qk&`i@QqJ2AsWGDwQ7b)_-t3`vRQ$0x-( znL#$XK36=|p2f`6Pt}w7A_H(xzS~>gL5Ayv6^@lgw#AV}IxaaF?UlBeeT;Uuufy|& zcbT{s7Ujg|-x95^0l5;qDSLc=LD>s+vJ$&7fI2h?^;q`gHY4dc?BI9`vHynhUm&LC z05Qt#KyTX}5j=(W)3#~Gg|3hgy6-frt#_3#1n!&G7V70Z$AG)RlC?D;cel^)rcfw- z(P@K#O@)`csq!&h!!y)MSS=YG-Y47~ z=w0O@zW&j}ys{fAFWVF9Q*>pxR4S|M9$3DNVMP|_Wkv2xbi0yKxC3e9POiw(onar$ z2OQY<@(DMHFE%>VC-V&SzS%Ry#Y}k-b9qm@Qblny;N**0{m-xyuV!Yr6dAQXZpG1d zmFkDAuoguVNx7>{&3mrK^5Jc7>oc=%on$6iBsDs zd zWQdLrhQ4uYLLptPV0&z;rrM+bpKRFTgIyqmz7sGt=uI;FC{qT}-#i7t$4# zF-e)};z0Yb%0c_H;zQ7LI%xmCGwkn1^wNHZzy@jG?c9}3IRzM#=JNjJlU2rnp#2e< z1Ng^a(}eI-Ku!HXMMFVZ#cg%^O5^7zZTFBNDQ!Al)AptD?K7XcS>?dY)om-XK0L!t ziqCw%EuFvErs9u*=7wb&Ky&o`CThy|n}$E7*-7Q;P=nj-P@0LhXwXaB=OhJoeP}dv z+`nu@=Ir7ak+CkQw~mpY$s5B6qn4?JGro!ZuV~}cN}^fyD9agcB`$6>x6IBP!E_`p z8?o<@X&65(WlV98N9jwmpF!I=O1dRJN>UEw3@@Hn{7LZv%kjsC+v8rv`pCRijBmwh zgKMW7&zlN@}N4l>vM2I3JB^2iwZ&9f! z>uQd4S17OYOv)SLRo$O0Z-_sVH^ft#wbw$)D!8xUz8C*WQu*)2v+D1~pEOZ|Cg7-_ zz(;npMHeQW+f|FP4tp($$N9))$4D068_j4-pJqQBs&`*mMznGKh~oLhkvF>+Mc%x* zSSB(ND{0?sSF_<3m%r3BVu@k)C(laPqSDwO4XAcD3-NFW@=lBdLrK=18+&1lCeYHTD;B)xuQd9F=m8 zIcUio#GSswyGY1PXB^MCcfA%T-0uxydP-S=CEt{2Pb^xOT`68xKW*wKtmsa&(-|&x z7(3K47sa}b1vzH{)uX1;yfdN1?OICVGPnnqJ&`*NG!Vzv+G-06Y;w0!CU>VZ!SOy~ z_BLBA7scmG6YVo{&e@}F8CeElMA}oS z2Bsp2mRhKxZavv7PEl8gkEk23-=!VKAA!EF5OZ5A#E0rOi!ZU@U;IF zN=!15W+$ab)oJGS>64Bc*FtY zupDpmJ(f}CMEmQ{oU>DCn@WdB&*H4d;uRHQBOpFfyjeU^Tp|9YSS3UO+VNUT7Uwi(HHxjp z^qi`0714aRjc9B*LnUfPf6WsZIP(*tIX{$W|T$e}V{Pj(iJGK>PvCG!V z^u{P6zQJ<(7@{%lAR0e>x0c}=#mmKw;x)Kmiw(lIrHuAl%%aokw4~p?aekvH=v2au z?t>OO3JKFs|ME02RT(3+cGtNH*U-^X@wsK+{782uCtJJx8po1hjPO+1x@$_Z-MVpp z`rUED2r*dlLPe>Ed)*ThEq20~B?H;O-t*2!p-ML&} zyJ;1)-3_KwNFN{s0>O5-g9pxqGN&SiC;b=2y_bH%2x0|BMuepmYpiQ?wRRedZ@ws+ z)>N90i=6Jbn68WCkBf3m80ay~ogPN%(UOHxGAqnt^9mEAojO--)7mbI3pRnS_5B1! zPK20c<(7)vIPgn7Y}cXQGKD-${4idYvM&3g*i=gKKO>B2kBh{CQGp6)Y^3vW!BGW# z%`puSiULE{hw_Q_gSIkKEi#4(Q!B`ruRE$MM(zwsmJeUMC@%l{{B7)gTYgb|Wi{N- zeHcvu4%0VIHh&8oekJsEtR_$A#)!9ADoAV=)Y`l?%w|@bL$~<>n-&;iS~Na zIlJ11JqcmnvYnC)<8(E1cbImVFN){ODkUlQ=i3rYElDuV(|xj5`_xA!$lG4omD@Uu zhrEUKngQwcD5O_i7nQX^>9ytQwx|2QYpy=m2lMeu7%2!+NWcL70^Wu9W$58 zswpi$&k^l%%nY5Qzd0z+Z=AmhvlP-pOJzZ~oW?kxS!(D|r;+MYGs#GUFt(h0__8JR z=GtkAcICKpcEy?(P3y7^9hu4CYVMUhNrK&6Gxh81?21tMTz)#%nlD;OB_#8Ng#xjB zeLZB`KpXi`zM9-zJ2iqgbj(fplD{DKYh7rXYh_pGifr*JTbyk%pUgi8%`b1w5Jy^} z+Y_sezUJAMr_F3tqCMPp&VEs}my+s?EcWI*)0j1S;m@xNZOTkFF;!#<8ALwh7UwPoG-HQ~D&4+3ri$`0$A{j0=dnylRgO=J*CbN_3b9VOT zyE8JfPSP!wb4}Hj@&|Q^SKYvKq%oU=t@}hXEhGLG>QT+)?077ed0khJhNm9QOjuHT zy16coXdBQnyFOcFDrIhYgna!>{;=4jd*CsZ;Pe_gnnpheydO(8bezzl{mK}#{6l8l z!aP@AjIhj0XP>V~fgW5AzMy<_lt;@Dtr_>^epUa!a`(0qwy2$vQ9Ek$&A4J(_IN(`W_{hOjCj+iQ(A$)UiO?kJ?LPsk!M#wu?RYu& zWl&1u5+W)eh{3rsg*N}3+`W~Y63@dz*xU6y`jNXgM>LB4#>(B7Nu#JvZWNXEw6Cwb zXnA(M=^Rr{%SXCL-tO)AQi=nb`V@NnRl@D~a~H+RdNODxe@+~a)>m}RF-HKeHEzA^ zJJGKBPMoXzPF$dm5h|pXlg=^gX~{_Uc(fZQjbhU>+Ad9YAo}-pGlX7VW zcEa{A6y%(NH=AMwF<|X{sW?QK?I-JHH`h&1O2b%3Ozuqjgskf)?CS}5?%E$ID8ztg z!2XM3zxs0`QI7#FOzIkGx=VP>cdl(+_ODkRe6pRUj_qn`TPTEbO>NNkl>^Nmz5!L_HrR{6$R24S3x9Dc|X_Mx%D99JVJjh~b((h7Ln`BOlb6l*eX% ziuyrGWhlcd>h)i2A+(Y#&T4@UW=4*tDx_w4yJbPL0Ar% zYY&=J*B-Qlx#fiF^5JVotWgPD#o_CB_F7V@u%t#*50@;dK1*TO|2IoPH()18ISM=B zCD?w)Hc=jqe2q#5+VcmP_eSq<$&yOFZAq1M&f1o$Q3!2;_bl&WWf01rvr|q#y7V0? z^WO{Kv)n%DP@S zDymzLLcXU*k8MDJ6&)@5?3X+4$g-D^YJK*_ce?jhm%LRY{>p!#BR$vLo9GwNGYHgQn zk-}dCwRZGT8MJqkVb5K%*WR}0C{&N!F-sHzYU^IJpM^blRGu-k{SBarSrkNs!SFXH9qE#ZRR*5n~YbymU zczM8H!>zFV`B<}8;T&b?R`#YaHP#e9c3Yfb_WC6>?e4A2IQ8%jtnL#E4`Y>H(%En8 zy&vm4Vq*rFx;-1c0grO8Qs@R&6ZR5k{chZ^hW)q*ZV7ln57(#f=$ek|FRJgFuwP<8 z83Nv+I&mZHN(Mal^BWBJCDJP3_JWV7y-IcC&!2)nQJ&hOdn{|a_J zysvnvII?xAII8j^aeU=c@qXyKH~l954f9*uj)-}cM?^=ZYi{+&bd}M!Q!>^b(5^Hs z6*uaZikskSb>D>^5i2xD#N}|SG)u)A&DObkcV}stx!jVH?~nv9lrRV{*e^_DAyv+cs^*9@Rkfl~``XZcxXI+Q^P&$+03chVGu1WA|Ka zUMMg`C1my}g_Q0d&&!~MdoX)X?6X;Huo5s%?woyH_A?l95Kpi#wR5Q*7shC*70Tu3 z+Ex{W+vtir;5mue@9_sb@9ei);{Mp0!b9L|C>| zW1pTQv#+;P`~9Obit~wfGD0tGUpmEN#Tr$HIjlwAi{(LkgYL7W|D43{)t%0N+@{94 zN~S0ucE(nfpCQVBt+pkUJ)SRH!>;WwZ1c10FN*4)Wz}&NwQe<;WJ~7D#S{%KH?tt% z`2igCvA@T2_N#co0j_#^XFyxF6|;a@3(i7+p|qQ%25uGa#A#U==Czn&`wf(#v-(~4 zFiX_VVL3?COtgWT#ELx3nEGWK=1fP16XDGY%FDqy8X!c7v@P>aln1pEVbW42rCc1M zN##j$q_EjL0NU1ol|QvsZFTcnCqd6pUZH%F`WDOe@d~B?RP2GwK)xMrANYdujTkQD zny@ZQE{Co{8nr2P@aa0teVpNDU{7W1CYH0qhNQZV?!HajeFopNc4L;Y`EzSx(X6bA z&7YZ?#I5Sj%n`8Q)(ub^Bzm?w4lXVK`rF!czQ2&% z6SNZI(&FsY>ReC}D|meWupH73yd!Q>TXvBV>hp9b@5@t4q0P4sHo^~WRaobI-L}qD znN48|<-cfq*Yu89tBDmp$6o3A&_zF@E*T(L#Z*2_xQv-g{)*ZlFM3b4DTPMt{I%_+ zw6>)?Qg|P(wwtYb%^WGr>8_AyEv2HuQembPMMp(B9n{8ZC2IRa#h)y%<^-vYQsUII zAho>&Y7>5rm#FQqC~r7xtIoH9=B&aDCEg?q3)0-<`L&|DLB2XjcME-#?wWp$1;5HJT}RQCyVwau-vrx9r6li9{S1N0Q@WKaHAyFy*+Iq&_aD z!YQ%mq^>(I#=#A$`&iUy`U@WKc+2>l72-I=b*MAGGFUEM-Q(Hqj1(^VBZVK~&iR?- z5rFJHf4MXtjpb)=Lk4}Pk?~X})k~>~IjsGp{dGV#t{1WdDsjU1>U{YJ3I=J#f{7I~ zvAfx?&WoKSbOdMB}{#r?)&$h3y$J6>1<)LWs1LYygQ_H~% zWCknQbd+BtUV&}5;gKMZq{EKuggrMK@g?9ZJ@_jX>n}=QFJdgu!Y%f*RhuxLF@Eg0 zq)zU9>XTFAR`MZmO6e=3nNk<5cksGi;LB-V6(rp`Nz#S(j_MuIm+jI9p3;>fnWXI1 z<$|hC=f{^-+aAvc#Z^+OF=3UiZU|DB-pj6Ek#m8@L0ygF`Gz~_D=FZ)vTvafM((=8 zu34AORx|vgSK#ye+ZaHe0I1b)clnvBKLYZTJ_e7Ua!Ij;}Zj*c9s( zH{YI?C93o${*cJk92PYV{e=x(hK|aVNtUhC6zkY(hd4x!+Bz9uR&W}+;fcKz-ZH?t z1nyP9n+A9%^uqfvuoL8{K72ibl;qp^y41TiPQHz=Z~qs3J=%4*`R;$g*Y&SttLY5N zs@UqKm%oR7yDs|zzAXReE3x4CfakvbvNgeUSvAf1_=*Pf+(*kZu_ieNdX4L)XYLo4 zdmFY>64oiH2n$FzA^+{({f9`LFGu2>4feT@N&XIFZS)4Yw5Rkxap_M|>Y>|Qx}|r- zU-fb+BnBi!s%|v)a;Y3qY9K3gGqKw12g#wXfdql0{CZm4W!`Op^r*Q*dT8j*cEGb` zU!>6BjTAnI`_juKe*(Ccc!MY1@ibFe%Gr>?`aWA;Vf7kOg6M#Z89k1$`;a zU5wo0QttPTpssl_ccCR2ng(kg@KhY#p9+JHscOwHfSm&?XHp~Rdb6lTF3$c|v#a|H z4~X;8^JceNrm6T2?N^8?m4yW;JCfSr6h;l;y9+MK%T(n9f2X^G((3uG;=&*e<8(q7 z*j|@lBf>|H zYp`ai#-g@TaAt#PL20m~7cwn$RZNQgnIq!tN)x>u88-i^kFX1JM4Vf91m|E!#F?ye|7V z+!5}Dg;ZRHblh5E_Tw~XgooA$?eR=Iur7OA?hx(rz>et9%&%S4X$jo6zO>2^kn%G1}>6+1EIT`U(^9tmX(UY2((blG4 z3V$ie5Nv2cJ1?WNrchfz-*l>%Gxy0x|I|Fnf_6#b^1}&ag!RG*^N5_7)~5?= zrCw`kuYy9REjZj3X9^W!MOwag-%^x+a-XfRy{(p(>}<#gmdr!R9c|2Lrm3}XQ(H}; z(55s!n5>lM>-Fg<|#H6t1QwBKX@s*GrqFA&Y#*(~QMWVs3(%YA<$cQQeoI9M(h zK94dK|J)v#$=bPn5o?wKYP_wuhK`pV(L9&d!F?c6*i4A(%@E z&fr3ndLgUyoomsg+89>388^Ni!n>t()$va0p>{@fbU=+y#e9D~gb7_&ouKlCGNG#r z`3s15d%pLDt=A-!8_A8=N-Hoga_mDdFAdISr71=j)y-DFWrOzvi218m}1Q-Xth(KrgF=?Q{u=P zZZ)O!r_r|4AAz2qGS5VKpP!DY(EKpMyZv+=`Q~W|Z}-!2%rRRL-r}c_KVi;Ac%wfY z;aTP!gxCA&2p%)@2*2YGLHJQ~2EuRo!w`PhtVei-pIblO9H0Ck!sY&{d^+Fl*&B{C?6Oj1Z@2N*`3WeK<)>exRPwnC7sXZw)fQw( zSZfAuVSPPThupN@8U+_d%$NB>dYXKjV+)el`s5vfK9t7ldgZ`u~J#&CNedD z@oY>xhnHSq%-Ob)_Mfg&8RY<_N5$-hHuW)1z%clcNX z!w`F3q-|4qpq>?!2Yq~R$Jmy@ZHA-#9}4-MXlK?2<+q}VSsyE~cst49>FJ$voR+1( za5J|@y&$i?W&TM>0U2&m61;Xfc#YGC2+;@SfMLxdnOP1P#)%ZaQooQrpURu6 zx)8yKlZ*kZ0XONSvmjHy+${T1d%6ANT_5b)zl+$%X0q-5jp`|h%E5M7`+$sPyK=Z$ z+%%3TU(ElXP*CBDdfPYn-ms0!ugi}*)nBOYR#-?%f-Ron@vWJVleGW1U66y&JK8^z z5r1z!1*)jR>11WrwU*=sWEc??6_@6-?MB&p@|U<{Gheo|x~il{#AMwgqK53V%QD%q zvi6q6NKwyHqEny~7% zfMs+UGe&=7MA@n9NjRZ1;Eb+zAxW794XIx9{dMJv=(TT{8#s~-zOZ&B#=_@8f`>%`4 zS+uo_&1n0Xs1L*6pTPSDe1AP$i+kEPBC5Ce$O__Pb?kasb!n!|$GU5MGOeuLAluc> zafR(+VKZG}xczgg)ap93I1Xat|u{k!XI+G7SIZ-GSSMaZ@lAqY#@ z@X2nWk8O(T8Xvcf^RZ8(w-pxtF?j|*Q;t^gTIcjw2Q+Co-jzK=cGEtXxONh5Xm5S_ zF+LA1-YwFWXusb@0aJi_jw?S55O8}@ANbf-ggVIIHjST!@(Mtp#NJ{?d+XweDO9tm z)`#cRx^r0vAA|F38m7R9@-&|L2^LcDNr>@JjRF4FA zDILPh(y%J5yKczK_T*6h3Y)*zc?_qnIRx|IUY>OVuhW@i%3T?yv>sl33I-Zl@v-%R z`FJ6sqExPKR}ngn9Pqk6%OOlD-EC*mcH4ftLfb6y4B4?2JiF5IzNMFEZE^YQt#rM2 z#z7%^*EDDznI9AX9^Uokkql^$m;_rrr7&r~ z8cJmLyvW1CyrWw9n_R2s!#aBO?qwF4)6GPg50 zooNJp(XmasW9({4+g@9QJZ5FvDe;(QanYgnLu^83^xY;W>yBVfi6=C>rL@@0iKgS~ zgu5HWV``IA=8j?UTq6=Gojy!dy8MZl3-5}ST@0S?J~W4aEK2|5>P&0 zd9@_%YFk1MNr@ah>Wc`5MM2Ar4csBIKAK9wc6H>4P zHl9kl6)E$&iwftx13Ku>)7jr|ACbub_7uxN?*8+(S{{ zM>W24Eeba~qX=C`r1RoI87xU^mqiOt`y`tP{nqu`hQXGC-{L(3EU#llui&w!uU7IW zLkIrIRJVx7NQ-#FmR0ZfEcZs`_&w*O?`dzUZ9%<`_j~^24M%E|^ey#;TjK3V^J#41 zb;SqStpU%rKy*wfOcPuLY3VZb|MgQ<6&+6RM7vf&=L8wCyfc<-J;Umo?- z_eAEB!GmoI!RQ-o!`Lfk;1v;Fc?LW`2P)tnyp|4I^zdFeMl6>`{F^t3^E>U~zBY^Z zsL);rc2-UXaMQpxIRB~qTH7zST5#1OZw2;4`F*uTOj>Qe!lE#f{hPnFv<1Ws0NE4Rmo`Vo6PG+auPOKG!^#vNo$3gIta`aR)2hIaJ% zEEi=_?|Hn43|C!{^E2ziH#UP$Vc)XTNXr1f$KVqMm%l70D?EORwPXIq; z)oa&4xW>M-*CCqh-O+U-0k0Dy0!^n5i~jF35Y+h5cjsRQ`&T=l8siyK%oY zUuvWM_u);wCq%;6ERg}7I(mX{C}qMg&LFI#t~*S7E&Fn+i`Ld@w5#*!zo5y4+1(1E z!2em!E_2Mbw~NBro3Y=5QNGE~Iu1 z$E)~trf6ZJUxO3tbnm++na~|j;C%9EBB@LVh9-$?AbGO8AmI_SV9Z2FM}OfT%eTtc zhOfo#2JqiX@%1SrRWB?m`>UvPEYH!S>>u&6G_|gbe+)SDd!7@^bD7Hgd3gvg5SjH1 zAIJMVPXwm&Ol2xUGXudizbCp&#@h-EsKsJQmm82*l}Rm36@M?Ki#{CM1F^?Opqjq`k-!~p1F zsspJ$Pk+>=b^apOKtgH6!lFmaWCVMG#i(P-a3=`&7WBeXrEiNPbrGQMFS_ZS+L1T~ z2^X2v=qBwFGs^3Q&eA43N%nhoccs@ap1-Lm(^`#}De}^Wj>%=(`GzSdbK_{~Vrl>8 zaA}XH9dD!c!dIm)%-?hKr4rc%+%sH!H@~>>e#?;gxVZysx~>$`c;x|TYd!EEl*wan z@BXfYWG>Ojg~_;ss}MT74IRtV=8oR$oIB(LSPu8_LoLjxwZ2=obo^c?GuY>OI4~&3 z=h+(YdtU20mb%q>OcTm0gjOFU=yP*NZFSBaj5}=&tWVK_w*xf^xC6#=c*A%_wx-q> zz9!twsA_$xwYL(V#hwBCuIvLvdzOOA!6WUQLW8@!Gw~ipU4T6q$e+gse9;_-Fy#@) zyK$BvuO2f}zM=Yx?jPtaxg`8=+DX)2I|Z44s6Cujw};^0EvCz;t%W;iSW#X5<0nSk ze5^U#uEJVyM>$@p3fs!*&g#VD@_2*Z4R>!(j0 zuS+=!p+wsErfuqQv!3eR3QOX0GHQPlGpc{njlvreG&d+T*mF-S_@a#&75g938{2kM zII#a6ggj4{RDULnd{d^Qb7Vq4#3*P}<%zl`oa}uCV8v$fH zzBowP3w%4XW3Rk=ir@jG?*SiI`-s#LQbL8+kl*;PCDxuQnxmS`^vNC#C z%jX##2*Ek8UhtLTHujpaygjAb6o>m@WBF?-CO%I12D)v5Hyk$VV_tQ2RMl~;Igeu% zs=?V5qo}7lXsv$TaVacuIX-)T4C`@|)BOVM*5+oNF_KU3y1CK2|oelog^p4=<`T z$FUou5bRwGr_d5)@Hf}vm^^M6`uPc`$P}8X`3l@eUMj&jAMcq?sn3X~HPOPKVcYcU zrd*>COkH#bh{{aNnvCxJ_&j`|un5{g6PIbrC&NoRx$ObZX0QB8SCI~sUEBRPQR%=5 zb28W$$gy!>ZcnS&s0S zrXaR{-;;MBN8*(|zH{P8aLGYGNiIdH`BLlsN*}b)Gxjs!*16p|*wvyvZ}SKFb-gbX zbss>=lRc#VQN9p%F&oy1yEgLHLYyoH!>RbciJ=0~`#ej1R^gSYKF>3n7H(tfl@Heidv2;$q)c@uQ&I;2l;#`Y=X zgL)@@rKW7lfQ|WQKa~fIVSoD$R8#HK*7BDJULWk0T(UU@7*+Zctp~5t)w(5`o{QtZVEr0kJg|ta-PMBMcNfaugB7@1xP`F3~sA3m*Z?<@r=+xK@rmB3VaU+HPNGXUX7z=g|IG(?iLz=6)!L z_shiLRBL1$wFmAzSEX~+^f=31#$3_!*E)nd5vJ!XO>p!qmagdqq*o%n0sblYbgl`0 z^!x+-`2701=_XcibWg+gPxyui;l8Q(mU!t`?#;t@br=2K*JZ``ut>k6cM87GO5cvI zhY&82zJt1Q5v~_=(b^;37E5kUF5c5sevPrxJHcTD)>6erDxVHxclhvi4)>@XQ@+N2 zWQGa%)^N*8*>D)Q{}XK0wh-ZncQRU=;QbxG&Mx}R?RpSjw@AOQijz!H!Zv(EaDQ(i zzJtAZ4NyA4GJ)PF3OkHbnFmZ(VIX%=jQjfjoH9_>{SV!bJhk+C`W}@7C(BkLENR^y}>!jcXc~sI zz#~2QpUon?lY|?IbDxp(@U9VeZR1jLPL8+|>2ne15ZA%)2cJVahjjb1xYrIR;9fhm z?^2OYP-Y~Y8s%E;cyEGoxp0GM9@5lkpS~-R?!wbH5T^HgPoNzQ%3OxtOGR9Xvgc8s zpp5!y%Jc7Lg#WE}lB41tavJ@8nG<1;b;+S?e^|OdHx(})Vub;){z>ZE=BSPh!|;}F z1@@q_KQl8!Xs*__B|jy9$P10w>%%D>Ed7$slASaM=ey8EC7nxlh6_D`AZ|)J7wz0a zd%&p`5z=1!Na`U*xEi26lynyCjOx($_7Tc|U}lB}`}i~Z7%KGg~bg)VmItGMG^GKHkX@%PxyVArMEcK7p2VSxW-%gZ^hm|vOlCuZ*W~8_{k|4h zjBjipT*$vYs#7}_N~2O>RL?Kf@z3VOz+Mc#fca(z|Iv^5x1Q|U7VOMNbtEO?<;FJn z(l`^g+OEgpBuTw3MkwmWKBZQ@HAa}*Jy9x6bMMNE!Fn2}DN@d(-S^(k>4zLEazdmW zbNA@mIWa469*!JN%6YIm^>&ULIrkzbT*}cQq}G0ay`R=#j?hldjd|}4adz=tYvR_% zY`=3?5O>fRitSct;F8WKcIbGT5+f{;;t%f#7tZzcQPGqgV{TK?`HIJ}cG`J^IPQMu z*S{^BU6LHp=1D5}FzSaHyL_8=r0)E9BN_2l=E5_FFXk`4@uupb1-~Vab5Aogf00zY z{oRlLo)S|By4Qh6=v`eu!g=5gdM8T`o;r=N27E%_H{)Ng43iM1Z=4A^W+eIzS%jD} zOh}{eohh%*KsX6)iD+jK_$?FlPou6Mt`+t6BAwo;oQv@Hz106qPmub5*?wTl-yr`k ziZg!+$Gdn+mD(5Z+OKs}KKSdI7-5=(i@$6S@>e^wWl86;?cu_d+nD)!`@Y+ld9dP( z-X8Aj?P1sU&AmPNFGc;w9&rC2ZGO<(!+xoU_t3-L=po2mmD@Mn?!jBG<7+^NAw3R> zL&~Dck#~qezPL+Z}kFx2-!I|EJIg=cMhgNc=*jz>9$MCBV6>JIF7ix6hN{ zoG-zr-Wr7MoUad}VsonY_y1Q)T`Bd=-G)Ug#mROD{$;3wx0$JK<8Jmmsd6h-oLi}Q zH?w~xeNUEkUG_|tD9!$Gm5<8bu{RdU3|Bw_nY;weA&!HTXYH z6^qwz8MmrlW${8e<5qRAzrNSsfU*@!GH$JUDdSenA2V*%y&RAK14CW>FF z6vK5!kt3VO8^s8O!dgG$Kj+DMUX&;CqH+?05I0FFasyR(A#@cQoQ~t(#V;)0vsiQA zpN(zJnH?Gxg)FmU7@$$6JFTRgI`}lI= zkZx|p;BPuFWxT#D&%Yz)<-yZue6n}zeQNO3R=7GiKiqP-x!^k!{LA38O7IxHpR7rR z9({&rmOO^ z|A(SU^&ebbxD{=VN$+(*aPG`7Gym_oGZ^;Pna{cB+;h+RJKOJ^^IL#R zj%BE=lL4>EeApZsF8Lg^dw`<~@RB;4$1Q$w@u!QG_dYv`z-w1nPDd?r-W~LwiBA5W zxBm2QE|nO^AP;TAQ^+lE(*qMb6qz%z&Kw7(@;z1)>~@$pO>Y|CG7Ol${CYAa?%I%l zn?FqQSMan>?1%&Rc|r!xM@K%8_lExb3@O*&y9ujIg=Zt4eT3(Z?n<~P3(xcJ4W>%{ zfQ1RzTLgL6!~X>jc~YJVJlA!T=ZNmt@oa<^itz59ac8YtdwYJkT&7{xVBFop{(#IY zi|SZm+!YJsF0$ioVV&}izppVzM05RIWDG`NeTs!ELZ$5(J&~*(OHA~TXQq<*A#Q!& zN-{%4?}$2Y&2e;8O$8Ix;>FP@3z>#q(k}dH$t)8lG)#@~m)A#dCmrs%fg8z&g(} z1@E7Clc%xUgy*+z@_f;4#534!G#X7NgK657sWZ&|%_eaqeO!1p`53s52+vP^li>bXcz)>1fxE%CrZw}o>HRvtd9PsFGymvo z41ajz(Bv}HUgr5ZC z?>xd?Cuc5rkHxp^2$MkKVZFp@uhI;67K|0^;TDX|R(!t>H=%zf@Ao>FM!d&&0|iG!!AJiYLQMJ<^m1e8rT&j*ek(Z z0aH2`-(X5HPvpU9U=%PM!n}%j&jL0oA9*T$&)_*gc*gslhWjqz84JuUl#3Fc5y0O< zxlrLL_bq~3Dm+ELg>d`4ZybIv|yYRf>eFAQ~@ch~PINU#Y z`wO&WCE)w1{{f>}a~zmKm^t1Y|1{YxRpFdb^~qe*+$oq_$BBECs zpB|zbZKN;2RG<&kqi=95_RcUB#0^tQ`V78pBmDw4hqIA9gxA0*U^slY5reP_5(i;y zBrO<%Taq>DU>Wd8^ik5+$(lvhFS3>qoRHv|c`y`ypHIW-@ABacH5fG>-NGw0kjTCmoDznD4&E)N*ICulyIk`0-?r{E%QALnL6QZjivR?d!9@HNQ8dHdoxW zBL5h87eh0Sk@bfE`54TPznF{6L(I8cdB(QfGbqQ~UCT1h;8cZ3SvZEZ4l%|d{5U!Z zJSc6*W%2iX{g2(|m&{{IpSJ8T`VL_slZtCCT4+wlUJrQyRmZ;JCE5ze!;(cDMSj-g zb>^eUF}Kq?aXGKjZZ=(l-TIC78 zs^Y}2X?5UjfvtgE4!y(c@I`5r%zhH-<-M-TzLrF@h z6?_45oUx7O@;zeU&;HZfk%pbcvg*wiN&6`(!$fV$?HsjG#Qo?RZW@7`)MXZO+p^i( z!Y6Akf?xixf-8M$waj&4en}1W?D;gK+C&ZRV;)e7d!gVp!ilh$+wHj1iK{LvzJmG54e=<9R*yc=}ZsAElHQhZfMYMQf=r z7@T9~%59H}Idr)cejel&;%sMf@zl$^y_o&iHiDCMkybXvm2 zLfrm7=(3pTr1(-LZlB2cycql6`MCSCE}mZ~<_6)u)9=67UVzXOT;w^0qEI!V!SWoyi|HjYAoNm;0q$?_QVU603g-N-cTNYKDft`wQr!Y9) z@~}me)MD;u?#Iq%M=TO^r@H~KZWWtj+^K_{`M^#0ljo?xq)iv0RIctZCUSow65;+t zu;XDT5$ZDwiSKWFZAZp%BCmt#Fb9n=B_^4b$03nmfcatbn-*}E7S6K7vqKF!oZ-Zq zL)ojc;PYyedRqEaj8ik6G!Wsc%pvB~X&LMYHFa~`${~i(C9+D=W5%yk>IY91W8}VF z5x1MrAnW*qhs)4Ix3cJ?lx(v^RXdqV$!<!7b~5Z^3P-q#unXl?=x6Zb6UWOBZZcGEoJZ(HhzKl%XLOe+VE$_x_bbA0a> z%g3$6T@cLgpY8oFGvLkyeYO^9WZ``zOAQUV`nl{y(tp>35uG-`oo5h$`J4uh`3qq zLnbOE&f3&bu{w3GKtI>d+?x7RDL*=nRJH@LkwbGeX+{qP|&sEKs1Dt#pA%c=}1jMFj5 z_g90S{gQiLOsD*H}@4(hcRhU1-ywi?1U7J8t-E@UGF6 zJ011Da>N(&+)C!nZP{90rqcjP9yk zl-pTbTCJ-?j(d<()YY9`wTn0FnmW#}qSRl5OILgI%T@7AsU9ITk>HQN3cte$`6+rF zDGyRqj3{4soz}7_W4=41{Cj>bj*BSn_f;v(T!Yim;45Ru$a&aZXd?FxRAIFR(hgu> zGGAho{5D1s9T-s<6T*lhwwT<%C6);@0&|O{kfRdmH{^2GNvCn!b)%3?I*k&fnor}V z_B0q9hC20lszBqOxon{IF-ejA!4{3)18A{R_CGirIA#}$W;br80!74YnaK zKMg4}iAlSPPJ?g1#DcruGXEVt?!uHd&{OnVmByZh9la_K^D!AWWUP=e*M#wMhRm0k zkBP3vISYU{W1mg%^T^0ZDo^gm$F&G6nJ1Ey``B36+8Jx6Sl_6kt{^ zjokU-{#zNO)Nu+C;H#`PlUk6L0?d_gYlsxt-7D3C481Q_eq%(tX!BVKG}x7j>C}Al zN;B1QU}YI@yT#lu-J}=Ha(yn$pOQdpO9HJ;Y%;GZ0{)7CKd~|Y2Q)VpeuP=|#au2T z7-KH@he>#Q9^4))A%$@2ez(&d3|joZ$5TUpZ{8))$jxHZ7=Owpm*XQoCY$u#ADiDZ z)S4RH?)C;yy!10$xt}oJ<@?`8D|R{w?zRKB99A7>Vo3A!4PVO-%wMzbG;d8LbzaMl z%9o(Wtr!k?XlPXvxVp*;MP{ib+afl9gSUW_YEJb`3CqVkzFar zob`$CRrIUN-d7EB^P-QCGiEs(?u%m;6FHg&Z#M3GC5psO?LV4SsSZWK2L# zX1elW$lgp>&e4N6TQkQn$M~=b<))B^g2?kE@kn_daRQ#IwS06wAPLw}8m#Yuyr=6! zxO;p~$4fp)yj`C=m8@dZ;5Qh4i+v=uDR>*`GXZXEJuU~$e+e37G53;75C3RTQLUWl zbiC}DgzxuwosOBl6mvGdNCRIMdM4mY6246FvF7{mh4*H`=TVQ-G2d5f9s{4>y_xWt z1D`_Qb@RROxrXqWz7)$yxLdus+$!WT#WMn-JP0+!$67S_at@&kzFNyLxWD$Q@O2En zYJJx&YWSS+4usEe_(b|r3Wvbwus0q)DV_%d=k3&2)nmzig^V>aCdoJ?ep4_8Q=Z0n zmQz{|W1OO%&mlCma!RC4UO`ij%4rZ>1W`2I7w8piY9Pg9k8MSm-(X*Xp>XBgD#R_- z*&XUN1 zleS~x#7cL^O>KuxcbQtNs{%)G9Mfi53x0o(!{|yWoQ*WDq1BLH;Y_$&J+*}eu&;Qo z7d`~L1r$KIkIHj89Nj5J`SAHagdX56E@6w(O6C{m;UxVVUoPemDtKFaLJ%YmfTMD} z^Kak=&E?7_7BDBl6M7Q#6;4MJPQ10;u4M&G4&eH&@U~-_yh4Xm-}XSxQUgW{H34s1 zJ(zPS(toSql2abXT&KKdte&dO?L22$nl#!(gMXB)Q`Tjq2d@Oxh10Q0pvio``D1MK?&R#YcKF@(JB9rtTZ33f&P5L0|$7D^)LmxhYGXO&W@FDDNX< z^*r#^1gwE_tdkX3M}LEVoqmrY0_RP*$^MB_e#aN=_=YbKp+17dq=`&)?RWf~W#94U zMc?r=HhjnD?fd%IXw}!eb{qI_U2(f6^2zI>agzF;;7)moxVPg}HJUbpBSB8jW(XgH z#J2RFR|Q@M8Dt!v`mtjY#W}2DwXYdoN`ibVug5VDxCL(HK6ph>b8B2;P6m3})o{nS zsC%>=Rkjj6CfvI+ccp1npgl!ND@+s2H1`$GNh$6(t4K+6ue#?Z5=oFA#~|m+h|%dG zZ*vnpj=P*sA+TLZ|RRmWAScCrLP*fGob-D1ZdUNr3KO)o*>duLz_D0 zgC`wz*8pOgv-W$?<0C?qwkaRU_xWp zS}CD;^kdr+_+d^Seii%fa4|(_kfvaKeEN(ZKcsv9KN#ai9PQb=KdsK^u!2{GXFN&p7Ad z`K9nY>71><2#EoY0Xn750{xoA0!Vm@;7Xhi=vRQ#zYy-ZPK$mNqz4f9U*HyTbSTAYS>2?abC?0@>`;XnT7%@{|h^~b^+wMoMY4sNpjm8HM zYYb?F&L>Vb=EI%o9HT#*m}8s{_b^Z~iMSV>mw?;i9H&^CNcIJ@EO8cbQa|%6z<(*+ z*N_t^3i8lI+_3Iyg7uB1)zXXEi1`<14B)cJj`*ccP`vuYkCb z(1B4H7jv+0I`|aUD3~UU4YIzNvA(n{!Wjw7i`YZu;hoTkf!ax4f$)U3ObNeC#Cdfw z)+PAO#rRXgq?0`t%oT)v5vCq_2)G|`FS3?lAX6f6LE`L7e2(x+;r#9Qo59PK8Ov`5 zm;2Ab;M+*T_58999A{U6yX)upjo?~|WwdM(6PLp~YWOxt5KPGV%?vJ^Uccf75hF7x zKCCIuVI|7t*JCM_Io}Kop(YVG`V%rzN+**uzn&-#X_?crka?wWYI z5~D+2gHE;5{*Lw>Dl>)ZxMltRx`Mkwbq4-_xH_rpw|zUQtAe|| zmrqZOPHH>#N31_wf5cyiSUvGcNl{9=KXiN0ZDmwXSvyfmE>ODqX4FNHJNce*}prYXpbzF#YL!f8jfo&dxMRMpEhW#so&Sgi58j zN-H^i*<__hD;c_#9xXnvfX*07?QEv{kBZM8s2py#(+}4Y@ zRXj=QQZtc~gfF_YHoyP#e1}~O%T^Ke1>~W$vBhkB@fBA7QmppJ zeP^@dE%L)=S>Fp7aq39hcG+F$f0juv%xC-SiR@jmN)3wBT!B~5=NQ|#mhBIkaLDkI zwIK#{0H|Ya5`_8LzC#u)q#?2u$-2&8v9D#X-M+AW|KhP z-%s{!GROH-_Ci_b`OmRV^vu|8v|V@IqJWL~Kwy z9aP?!ZLZXvG&dns>8H!hS&d>cx8qKP#E&pV8fHOGN;72Fo{K-mYW%6i(<3IbKgS(2 zi&&D@TAe=?l7{AvGW~Pn5}8w*@`R4mh95 zzP*q5j`v>#BILixOXB`;f#z&pn)}8( zzj*C_?0?`smI*XU#-ix<=$#s{3NFp1!ELJUJdqYImk~J$dBg2 z135U4^rSKcDHwez19Cg5g+81YJc(Pp48%)8yt@POrr*wUBtmDv7yyCE-nz$29`A>I z2ibPdPkEd`VRzB4xn;UROJYp)xl8k_=fxk#`h(ArjB_w$FwGtG z61*;98q)LDGc?Y}XzrGW#(s2vNdcdbOK0Rv+F#h;_6|#rf5&{}kP2K1zkt$8V!dgh zxdzBM|G-O5GL^*#CzF(*FAOOh@pcyP#AvhIXzne9*&3(;&9Pp8Tg^Dq)6YqN-TK)d z{aJn>(zK;`XNjaeMY>gIE|l5g**IoLA(Yz7Te`;is=A`1H;&@9^alR!I z`^WhnGKe4L?$ji<&Bc}MD{zU=`Qc3XwrTDr zu^ znT9-Z(4lxk{*<32kRRc)mLSJZ+&%ZHB;QtoOA7fxV!%N8W$-`vs0PK6FqO(ousVe> z6K``n*0IozkEm_Wn=nFDu~s)`FSPbD}0#1=vN zVJLJU0K4MeK)EwLia)h(pL3vXLB07SXn%xIdH&qrdfGn)(jQ2V`^42#AAiaHPg0+E z{#>6CK1WT!kF+=XW4(<}uu^G{*#@9?)CPa;h7*dS;C;+#0k1Klrgb@Oyk6AJnwcin_{Z5Si{;1QT?hJPS9c`~V zw-$A#YDmk!rHixC_tG`~u@Z{hqddp$v>?xZ3u1;C_y3o69_}LTJnS}Hw7^Zm0FRZu z-~sv{p?zNn_>~IdGa@iTZJ5jc)(tt6LR~4RbM0;gx zrY*0RFb9eY8hU!+izw%^ZuR__hPT)cbRuxg(A+*}668cUuhL_ZEQ=0r(RDWe0y&=J zi)ii_*Lj?G(A85WW!b*vGp!Fd(A;I08Yd`X8|1uB%4)1){*Jr)o4}Yg8* zKs}=}CtBv1e}w!2dBvB!MoV+gy0N~&WT3XqkPIJuN2^E!wH3zRaxzQJbM7$Pv9^%< zn$aG4f5I10zllnfRDeI>SNAz0P`B7_tnJ9H+(iwYV5?=-#Z*cm_AH~XuF%c1ZD*H5 zj`zT`6m!Q3Uh(^ zAWLFz*0r{%8FA}oNN`*)gxU&S471oECG?M&W6g8Sk75Ox>8fM5=)!IBsE61|CsyF5 z!-|{bcwlAuW}0hvU9qd?Z`J*ySzS!4+b|YtArXdglM~bkBQ#{)rXx`V-~wq6Wt94F z%}MiV)gJQ=v)VGa=qEd!p|-RNxV4SfVs0X^>Veap{nH96l~sU#w==zAC!2&hM{;>1 zMg=M1K7OOlWt%mSO50(Mx!iXuSxL+&j~6W+bH^MR6DW-meR5Sp*wo=nU9nU-r?8Ms z0EbPCxd?U3fK-p~>_JF(6~EIg#(j)0^KWR!Rd88cduKCxgoQ= zXHM$9{w3o35cj7IiHuxHb0VCcDp0=nU4I@kJu9n!onv@1sv_hFGjv{{VbCU3JBOnP1Jmp`&dZaT~S8JO|^T!3j-p(2?>qAQF5BM0(~{4In~# zZJ3Z}aIZWo0(k--${onaumck_eHf}ePvT1gwYYiHE+y8r$?z(Q00~Z8fACjxKT*9s=E(qVx;g zXB;ea7LA_!+}v)YgtgagpX$JBjoy zB|>b}NEkXJO9LHGxSftG{SrPt$7(0p%l=rLJQgckOCoqxZ#gDmrB(y3U*hhW(kP6k zUOhiD(ATn{yP1DIL{D*l@nr#{$ans1m*MRpA9>F31>rf}x12o`m@7Df&j9A6y-&d` z4D5r2{c%s{f-NpS23$;Po5;>eI6X&XiDOD9KMBdjJ>ibAV#dl==z?vE59hGZZ8PH% zOHRA!5tpzF3WV&H9^?lN?87Lp#}M({YXW+fvwuD~28 zuGbtHewCJ2;jTb?>qInU4i`V!>0H>Kof)@Y*QX`aNN2>DdwTYFJk~QJtrR*3y^asi z7mh%7&taYtc3*PH_<_s{ftFk-N2>U0U&CESuVYrD0wZ@>eIG*~yyZhTT&&=lCb(kZ zXMiilKwLS3D+aC!a77!4Ym6@%y5FeEXhXCy#^i+@7E!(8$oUsQ2M=={rW;1`BKXT- z%rH;Eya-bPvjwIe<`m2o80AZV8*Bya1ehL}l^zHxK7sSqxBL6gSF>@xDrYwLyXCmy zQ5TaF)^tlL>ev&W$Y)N0WG=BOjN9y?8q<+h1=_$INR!}S2d1p6S3y^j6RyBpSU;}Qi(#8K^sHfkZzJ~lH(RGSs7ZPwuEtAzS=3d*g>i- z@0Cbfv0wGa-^VU}Fcngs#fSbeTKv!3$ zKxuD5it&z;V8X54^^{87F+o-97RP+mCKByytIYkl=HLs?TXJ%XVH6RfTb-zPBKU&o}CI`$%R#L!h?U5~8d|ydr4MvkGrv+{1t{ zpoiI0F0UB!$c=CN#WsGk$8IBVnks}kkIO1mzQ zyVa@K_sBKXQiA|fd5uh{e>yZ5lKSUc3K(%EjoK&wtJ+(mQ2S^9W9=v0Qh(sD|F5!TQV1~k*e-APWi6#p@8{w;pWL?( z^{YsjwyK%HB}1rPix+n>vx7Rc;W zUX)fCJ1>xuk}@7c36ayY7lbm#SRjQ1&|B%HtM9V(CGse&L4es+q;LvSG(*&{2x(lB zF3r7pf#h2Xjk}2dOpWwq%kJ}}o@0?l7&q5@pXJ>W61Nd?Njpxs^@IS~%TnlXFY6Fu zPlpr|xu!}=;EA};p27-AVo&mt+T%@=!hC}F@m>adVmY`$*2^Ec(b7M*5m4OL1BGEi z=nNs>6u^Pxx*|~L3AaXk;IDP(1#%BAvE`X+*lOhX{rjLX8=ZSY4~SY{=*RhjA90oQ zNV8s_dq#kUza~Bp)_6z~PqE67ZozFxOt>Wt)Mwl@e{F7f_7hkv+ALjUnb4K|0j)xv zUJzA=w<~}%Kw8vTfBRXVL+R^DJ)1qGH~34ezFp#r@B2%9w~@e@w8s$-T}@i7k|pgG z&(jIVZ)#Q9HztZn8zSX#7n((z%Q#v-@i1uv(vD%=4$vszluK}L@%VG$UH-l>;nurC zE|;ZY$c5}J#kNY8v_$^G*g!e<_M}-l zwcrmOpWEreEO^1!-_+k&Q`~<_!nA~`)fUx~n}_HsDkbG|7??VZ+=^uST))2k|)yRp_#+3#BDJ&)dA0c2sY%f z2Y8wCF@SC2ozVZ4sNyN286HDs}Q4F?&KkWWej$;Tk%^OfC*c+m)V(WPXf zU|(?cvy=JqQ}lroFjruHhRH%dB{U#gfYVWM6Wr!H{N->bqmL+Hr@*AcXkbW(@$aWs z)ss^cuY>ha+=A|@hG|nm*r2*>K6dCkxC{4<0FA0yQX$?T-5}aTaZ{nqt<9~1ray^o zM}YI8G=R5j^$?*Ca`$ttAm%=}AA4Wpg3vGF8yMx1PuQw|4i#TdWqK48U4 z#^KOI`kw^04iL*kD-+5hhhw-)yWa(RL^IkW(co|law*ZeO1QFIiUybCIo~1lf&qAs zM!Z|Vt*JzZBMoj2Y0L%vcZ#pp9;xqx62)}yvBzSLxXT%U_-nG z5=K?gT~c*b5h;4=Z)I}eGx_^~lrW>L2CsOksGEvD1h1JCm<|cKv>XNup z5qKSnl}h5eWEoRVr_L&>F1l1yV4*rHt%NRqswu(jb9~{Whfy6XtVFIaR=7oa)2yN` zMOTUzeKdL6WQ?%a-TRCCH&hkN8^WO9J_kOl+hd<1!t6APa`fkK9yrjP597KLVYKos9e`q%D_Nc4m?#bskfctgwU9t@8 z#Q_-;0}b14J{9{;@j#YNT8Xke1wEKb<$2@tNje=1FUxpxVk62gQbfB6BQV=rzY+k8P-J8OzJK$93~z+2=; zh`eN^GPu+Bq+r>4?Q-fh)_bvg}csJ!17;&zuXi4um@)_y}~El4iC8Fo>zKx z7M`N3RMM;>e=phLA@xcQgzG7j{D40^jquSzNnZ2NLL7hj)BhAc3-IvQzytZ*KF9Yy z0vD+|vgP~hVlFl!A|Y7(XbIZii?fJ~I`c-HC&inSa9&QZ?4m+)I{X0Q)%iS*U4fhL ztRqDwR8oAe5IMfT2j+Eue@i+vb3T=dG6w20PN+*J%I5zba)^{(3^?8sC_Sm`IN`el z@CdXt)KWQ0PZYgV&=H6bei zrCO$2+(t|KUb4np+Qc+DtxGV44uc%FNhAja)6W!r=@KhS49UJ^o@pWHYzo{P#$W}a z2KNeQ`t9<}60BxGTndW7V+9%P;9WKMgb#*gpA` zzc&vv^)H45w8#)!x@mTaxRS`->cLi!l1jXp_}m{y^Z~>h!q^>d2@T1BQFEcw;+CV{ z<1bkpN)~5~L&?5y`%4BXU6c9}HtH13(ZbtiyjAFe&dLFyRe(?=;_P*Yn$Q}w zzYcQj?WjfqGXft4<_rNoz2FuHSQ4Cx;7(8Vf*pZhNJ`d$FB-SChe&?8iyBNNgC50~ z95;2M&vCzR7xLbwbDkq|(n(nIi`HXCVkuRWaf>bnl$(|2b7tAqY+&7rt2Rg{i^&eM zDUO8drzD0#k5nkuXSse_qQ7OQyQPqbC^gF9Zvwpn;qQ@y=Ji#S$%c8V6SihK>Hy=< zjm%eKP`npK5q)!^oW@OPW49sflt36F-=0#>u+i%KOz|bNY)OJ#*-OQKyhn_0qnqzF z&HN{+$Jwm-@bG{?!Ik1mhy>;M!l95pd>Wd9re96hryHsA=yJNPd0|tVgD9E)XcotF-^1p1S)$ykOfiLD>Uyj*hQO+bGuWF%`)s{MY z5|@X(@ebUr+OpHW`Jd!uz8ml(-$_|X_*(4VpY)+^fSdHz-zN~B(jGog(#9I}vox-t=on^Nsc^zwIvHOkeb{ByOaezJ z{As`cC3eRY!ufv*82rl~6&uN1lm#(AU#OOCol3PYElVshms|i(W;u4da%SNqE2Hgr zrR)NDH7(Fyw7M*Y3j&|ca`;FpE`T!j+N7`Af}yG9NEzXaKVJ4t8y$Ka8Wcn6Z`#5{ zp_sqO>^0oogiy<%IjpX%Y!cDW($w(=^i%!#LM?Q($lakCf@T(4-c}`V>UgQFB3OH4 zxmlY-C1Snp{%L;wKyf=Qo{X9-HcHi}8Y$JeBGq$56U+%{v=XbSj^SnBUytE#QQuLw z!0@kPA!}g$o8Vb4DRVk*KuZ)wLwd0lw|vUn8wKK~cpWDZUF4@|z`3d1T})CZQjaTj zMB4F}@V9br6D?(`Q~?^8mD_$;OeLP21c`jA#D$Q>p@1w7=*?|XOf%-1tX#MC93NJH ze3G`qX^mzf8xLA0JK-m(I0q-aGmf(Va49&rs-RbGVq5{^IA~1QE|Bw z4`cS;r=!oFf{dZ_zh_*6 zh?2}CRMF#TXF&s3eOm-Y+LneCG%8|)3>t{&qPZt{ee14b@%f2*5lfE{nb+#-T7qyJ zLCM*YiNN$q-Bd4KQqYcNJ$&t6%t>EIjWXVgIZ4i!uy0;VW4Bx@VAb{%HWieZ=PLjS z^OtQw5#l$VVH|f+ZdrGPn?6JIiITi6Ij8uX)?1FRocf%yNn@^vYnJOYmsytOQhg_PkC$oJwM(>-MN zv_wVWG>GtPhv98@RD>}KZ~J}0cU=1K`K;Tb!tV)3+ zk@Mvq9Y1s7?$Ptpg1K~0ulZDQ;FWsbkt|3_o!FdYA}1z2rzM~#^3b*GAlHX?n1nME zBELdZ>va6)mevGqIitS&%9(WNQc9+iX_XLC!#hR=LjEX({MIe0kyZz7u8ybD4mRGe zn;m~9?NlS3N&5C|-5JaRbfypT#g$A5ZcpjtOXMy|!`qW_)xH zBg(93*Y56hkfqV!B+mz3;!ToG(oH@`dv`h`-b+s1ybibbOp1)jz5-4Jymfj{rvx#R z_+)DM)kJ0tlff+2)0v^r{~=Dl*1YMuj0$>x6`n6Z2e*ciBt=4BscZ{1sr(|bTp`1+cIboj% zpD}`u*Gayd;ouIXIyhe0Xl!k&I3i?&L zHQ=lr>Q;dhCLtt;sbO|8qNLjP-B}}A4=)*0URRn6ex)>PF!(Nfj^aQ%1B7%+0_pTc zI&*|{AhV#IrptkW>J!S{)Q>wcI}+U!JK9M8xSx+3H;b3t6PmNHEjuz4GkfUO!|me8gMd+5w>UlA_9D(o{V)ot zl2=m|!8IuY>!u-}dPKkFpVr<{XLDU84G5jgOU7-+}}g?>3w-vh4DtzlcBtGZiz z%d@y-AgExG6Ws#sZ5LCSB<3!J%y-htETQj=v=hG=_!*p(a+p0@e+qi5qcKjRa85z^ z8YL@0rvOv-110|%_Tw;0V$kQ(R|4;VIRPU9o|yu_1lTMLIs41Q*wW+M?KS@}*`edi zYx(h;a&hG$uj4c9E`J6u`7$0Sn|Dy`h6c`16-RuI`3O#LbJ?G)U* z>|&%=Wv5ZH20Ml4`*szx$6n8hD6(D zQoRZ4t*ts>ni*m>kJEfYZwsw4RU?qYXojj7hMnN*;L+*qa&&m;bm%$&%}*rvn-j8khiR{7 zq2HAFsAMa%hEcR;G0?g*-03*t(%w1{0YxiU;I&}?Rha0?nka!=Wvj}C9aW*y3&@~nUdG_SF!yArqw;FVE(ryWKuz4I&t`L2Q ziE!}636KckDn27{Ugy1Lzm9afVdSs)^%$Lj`^um4BJ~yS`=%sLU51-s55~toK$CO< zn48D(qf>+W=6i#n{~{8uY2Fc#r%SMXGA3R?2`x^tFNJ&x*9SPi1pmB~^sg}jgd+6A znGo<_5dWdBCxq{ZoT*GO;y&yRHU;bZu_CheP7L8506zv9H-i2Plve|zhavdT9N46s zJwGBxS7xLWn`fMxV`?CygRHJ0+*%KK7%|GpY)p9IUmfC~Z>iMM1AVXdWy6Uyc|$D9 z_pbXOcFvJp2~Q6VV*1+57$0+SZ>RwbOSMkNYX^auJ>!4N?R;aQ)3NfPRG^lFJdY#9 z2fetF+@Y4DY-Iect(QWBg%LUkBuq>Lxy4xFO*h66N&Dh^adVhv2*>mie@uyGvsnEq(JrQqFReQ46E&RnDIN(bFChZdstjq&+_M^t1&x+9mTP9dM&0+GawDmy26Z&UQ2((#t04uS^uT*j z-}Bw00`<*CeKS$t<$?MR5$fyLhww*z_jC`wUEg=R{m>wF-v&AOlxnb0chxjP<@cY) zT^N^#+J1Js?!SU|=i}RT$F2N-Y~$U+xLbKGKX3D?@Y?p1?;W+V^2qJpA&=hcVXmIo z)|v6;i3=eF90h@6@VWJbO z*(dZOE>aZ2-RBN(mq#QriBm8csjkmNZb~d9g}nX=1TqXo6>QxqahVkd~;4;aIEyj3ERFo;^AdF8EBeh zyyR6G;>mbXdag;jvo>&5p}*eJUDbi7}9M$JIA2M>0P3TR?Qqzo7`~+U?pzlgQB8EqBdHGQdy4OT&Z0XeC(!l#Q)7XIp^#g zYVdfJEfJXgK9s25vW{;+3HJ|Q$A36Ig!`*26>Iq3q$teCgRv`>1sS2;MVu%a+1Jf^kv*6Io6taOU<>kLVPALn z2F-!J4dx(>4W&6NyCT_NOnM})_vQjA&_qlaQ++CTT@sXgV|h$S zi#VGeu}>Fei&|1c_k8WVxV_y%^@FU3H>~uC$)oq_vTxAhzc=>_i3>|9EiM%c6m3zE zi2zO;wB6GLQ3O+>@xUMpxIRz7;DXZJ{{Uk>nVbURK%4_~u0 z&qQy!#7;AO3rSvUte4maWlPUrVlRLX{4}I=D8;AEp~Z1Q;tP|FlTAjm$(&;t45>46 z<{MUPWVp;Atl?oe5s3K4ENZvPjzZHnOzTKq)c=iyD5Q5b=wYNcT{Z*Hv$9*~kFuZX z2$%E-=BT-?nP^16$Iue=;Q4dxY?%&Ef{}DKpJJ_pwVaYVWe&B3Tj(mE<3;GYO=DDs zDDclxDHA&Hc|RGgUF=M|rh@&b)8Fdb+ci{hil{|0JTyDsu1=NM?nw=A&c^7OV$bI8 z6B_~?$lZ=k=$z}|m)ggte%Jg-Je{VniPA1M*Mz7`AG9mX9LNJ#Kj8>Hxj@yM` zPUVdc>mQOQtn45H327-N|9DY&zn@Y=J&6w(C4nh#KI`f0Dp%)1Eyzg z46Kj2&HNe7|6!L63I}exb>_=-s+QYoUA}-yl46X$t00u{>-fPqpQX6h(EEaS5lY5) z!I%1i4I1cSKX2>*@=BaPlQ(I*Rz|58+rE3b=H)EJ57WrGr?C5JV5VOlVKqA#kL;N3AfHA|$ml3ns7k~ZjfFgM@0GEiS9=p+xu zySR?345|~Psf3`fgknd>m>4PvH>7nE&H^kk`cle*SfiM|3_hKhW~!?-FHU3ob%g3c zqB&jGwxH?P5%cJbw#RaXb1s}joyPhaew<7#_oFm zUbS6C%QbihzBM^?!E5#42mv%-x6XQ*PSbL03A90hw~mhjq!kKEy~K8M0rey`Y?1Al z?4b)*{kP+0llTXZ^&DApK2puY6nc;@=DQuQc%MMbGvX}&;v&TTkY$9 z*v7s0N1q0K1$qCw>m!)d^{Y$v@;Rs*W2LZ~wp-Ko$m(+VjpH$c?vkOeDe*1YDu?_( z&@o?~1gJ6AnATCYtvsWdgZr5?);_jb31|0JLP80nv~nk{Tlq|7+OE(XF>gu7&SKX( zJ~I#TyCK0${^O+#(8dti7)#j0tcZQYa27Nfujme&wcJkYAAN}yX#G%sRT_cTx}I2L z*%`(f^t%_EQz6j|vZ}1JU#2l*S6UA)K(9bq(e5cv+qxHweUeIl(soHU_rh$&G4zbs znNhnWySD{;#yJ~_KMy@)HF`#BjjUZOrPOn5m3)6|&C4l)z5(gGDVodQ^W9D8AFbha zRB}kA;{A|1%%34afqs$MgnrQ)@y1?$+_t^^ecI5C>%f&7gkEvky-laR8d4{E|Bum> z0(c}b*$nl@*S~_skUNCAaowe?1p@-(DXCX4m_f#q^aaujCeax$%zMsGwG~k{(5)DS z*~Jf+i?Y|xD{$uYG~|o=gR70~%xks+g z67DjCxead8SJun?&I@NFXlMkw)uaQJD%BDL}=h7 z?O~+U>gB)$MJVf3LYuG~I0njiXMEfL6n`k{I3yTj*{5(z2T#07=~ z&GtNh%<}&dbKI88vwBsD%6e25gDk%!S%;->RiIj%!F}wG6#0h`Z^eH&W@2<`KC2)A4bS z0JgKq&hKO0Pt9_S`F!XgAY-O#5B3;j>|y*C;GN6^f1a=Y7!RjBz5CO@>klN2p8jy> zc&h!M;*+xc>-gpWQ+!gNe;uFwpW+j^{1Jay{fZKL{q%-VRxYgVe(oF^s}$jX6=A(0 zv|7R3ES{_@O_lz2r6)e{k=#&rZ&&V1e*F3`W&SmNRvGCT(B(Ie0#8CnC6TF?4#}rO zVTNf4hPIB+f*B1nMiU7wVtTj?z}seFk8ULMcj#Jh5K$s$bKL?OJJeaqb^L7Dd!q>M z;IsZ?K??9kA|LlCWXHE#dvM4^CPcs?_YNoFez2-QGe+dz`Z4$<7WjJ*l}5gN`|<`q zCJFiF8Nei)IzEQf&F2)9D!i*@iFHbN*LN?+GqsSATnpWMGL%uN5$_7B3#l!~Z34mM z#g$^{d&52U&1JxxvNV!i;#xm;9*X;Z>=J?1kJdo?TmWYe;IGXCU8-e`~Ubkb7o_8K-2+I2L_bQ8WfAvo5KKy5lqMJ=2nfGIhZAB zCbv=t(9+Dptx&TttFMX+pmsGiDeY#9-5IqTh~6kQTv`sG%we7X`*UVS$hzO}{k{JB zI-G5u=RC{jd7jVnS>B&Pm&Rl^LSiuOcj+F*n+W7K4LtZH2Wi=qb!-aVU#Z+-%E3;= zA3Ko^;?UovN=K@j7gNBkq-3Y$sD*U5?5@3t0eg`V-n|GX^u>;e_96*h-l<4KRGuD)CE5Bb>g~v)PJfPb=z-$Kk!X8t?ohn!8L_~WBRdvn>an$lGwMJN&G}A zHSM9MC$lM*#4DAW>K&#J@02_xxqD0e1TOCS#qISn_&58RpOS1?)xu@ymJef`5MO28 z`1?K1!O3{O345RQ(tc+g6H^Or{N;JE>%d1E*Mg5+D~>V}M?Sp3uS~wP$1eE>zc>8B zTP7J*!oSf{v6tl76+XzS1Q)*E|57YQ$0%cHT?#n!Q3eY*^YDV6cR2G=;LLj#Yy-kk z4{+uY&&YQw!Ix_-Dxtbl-v7#7T=|M_TzTW4a^=&dySv7tW2uCbbVmiq|r?5sCA}Y#&wTG z1(v~rC4~0T?S4v(1;vIlqMCbqYji)<)_w)xm9fhawp$Dc^KY5gP(}CDclO3NCaqEXl)Ek(| z35Zz|ccOXv{?w=Ay!88EH~PJs_vb;I@4+w9lK4wyQgxZ>Y)ukBtt>JlOguS~%IYiC zcI?uwU+jZ1)CV?1YT#WH#ZSYYp$BGTPp=X6kKG>v>Yvu)9ykls|5xl8f-wR<0R`JT zWx|7<{+P46IQScMpD7f-ZDL4q_!;DbWMZ?o1 z8lLN1DN*otF9mM{1!D&biXDdILrKE3$v$Cc5ZZH__#r5>#z85TI3ku2MS?bU-RW@^ zR)q?FQf&t0Iw^*G*8A^E-p#}+-yr$Ry^!rFF*n}>xsLxE0iecDE!aI;_KH=Yu%nPF z&@4I(GM$Id=NyT`Y%~ls5TX2~a@_ecQ#fcK2sE%GpYrgn8Ksa4-?$hL3W(26K>wAt zr=rD6vV8R=iuAujk@xKK$aefQWx^uNDkqv3fg<~ODYEw;D6);DJJ^l3J@F`{I;g9a z>a6Eq1_g};1zoTA6YlTy1D>>>&=cqF9sZ!RfubMiiT0psVM;sEQk0Ta-!1>~(URsa z{y?UKtxLCm>O)^ta`5{+;O25zoi>l~Jn~9FdtQ4O@euq4=k}6flJ2MVpHrXsz|=F3 zafrw53z)bz#~K@JTWfovqGp*oCyd`_QaM$~Z;e|e<)_ONkMGRV<+atjqke@xq9UAC z`||JKck(xTx|M}4iQWUAE`oGW0V^CPaf+j6*^c}fiH}z6Gw^kFk;vKWBlW9>^GW#1 z6gR!IRL?%@a(x8aNOwd(tzX3?+~+upNFroK0sGBo;UCa+s*PK{I#YeZo@s&vZsZB@ z$5Nz#F^!f~*)HVl_t|xWekQb_gpry80RC)`ZjOF@h3&y_2bl+9_UfCv!8F1-&^&NVv3Nx@kdT0eoIz14dkhWneaHka$qrq`zT9L-XU#pqL_ zWr#XY1)pG_HtF;J2TL-H91763TJFGFE+hCnyhb4J>J; z<-U2#Q(N+HX@<*t5@4+m_bG?m6re! z-NEWdEN*fIw0(|eLzi;iXG8IfZG--rrSvkP7W;pylW-`2>XR7Ar&R}JFasZod=!{0 z?5;kj&rFQT-nBI64XuvMcmmJmLXTFdeu;soU8G~gkvCC-RB!EG#x>RD1n|Gqryfs5EmK=){i2yyU{;rtuJpQn z9io%2;%5wAUXAr^@N|dlc6nZD@}tp>t|Q_bqBU(s(!y#c$>=B%-!XiLbWGxWCuhse z?whN;UA}mO={{hWGkz%<%Ep=672?cwMxNUx7rtsSXC~Dmee#UN3DrZfM$sEN?0vV9 z1q?X(LOYwd&~fg_8>q3t+hQ-_n!cgD{oX>(#;l(KUtncJiA;IBWKn6BAI zMX8v?YtEPO1!F+=}8aN%t)GvwyG9owqc{sLia;;do|PStS0ZkM4&9F)ppM$)nW5MxY&NB-~-pM;0vhZ1*D4 zXBdCM_EPz76Yw-{*G<$8JqKMM{4VC6yM7PO%-r3kbJq>HlFc>IFD7e;ZKB;&gGRaL z05eG5p!K)GQfoM`5}t*9np_WCnbnt2^Np|``M5#WLtgVSY{dtm-IGMb<_dhQ_0{%U znJYwXRll0xn`49hZ+;W}4LF`}&|{u^JB)~q(E52f1ztVPq(tbO6*QZzvgD?$!-=Y+$i6266O zAP{M)-i9k|x3p9~lkL-r)cCM!TkWQr!^t0`%)&M(Q@D1I!-+xEfl zh>@-idnInTkAloiO}Xr`pM%WJ$Zw|ozD^1Lf~0N*kh(1jnUdqbj~NuSJGo$?b*L5l z;xhRzMf8#aqc4$8TmjyT-n#L`c&QkC z7W_$YJzRQAoEg0p@Fe-thtk_=xK8}t+aK^G`qDA!Z4Ry@FKv|OyT__;Vs0J)E{*Ql z`)4G&E5}wJg;st9uZJXTW^$KS-e33_JK`G`W5J`vW)B3leEbKkyb+_wrkafLKQjdA&Fq$7=_f(G6MY3ZEoQD_=sAcW%BFLUg3++UNe@Vm zvLRoLOC@=IWM5? zh6k2?FRofwn2B!(2Q|zgBHmF<(MVeAyJMNlyj2W=@PeFk{7O*(t(SjR@8f> zhO1e`!vp$MZIrabB`phCQrK)|SnCX4iC395s~Z({*-?1X5XC)P55Fo%ggj3id<|b- zPc$=Uzs~F@Slk0ZO9QfXpryjLxThn$b4veix!x^Z|4>50^U|dnbm{kUy*mc&YovA_ zi#7N#=XIQxk1%KwCslVz(Ia%@&LRh0pd^Frl|(s_FQif!hQ7Oc zA<-5m^+=xW0D2_K+am|8hHh=w)gwXJ2l!G9we^uGKg8Meke{%)vpDOY7~iYg)k0Rg678&%xV`P*O^ELa z!Jd^4?EJYE{VSc44XJZ ztj!HSi;ZGDM(Hi5frtK0IM$kHlY_67BhFSFke%DH!-(Q*VO^4hJ;`Mt9s9Dxt{~Ck z09C{$@C$@K!Q3GC!29Yph=Vi{3xUifL5$Zh@h6-Zvq?=(#SsB0LjpWFSDf&*OX{Js z6~$tVWvwZ!=4jWc8qE} z)1dV;^D2ypNQ|L=c7LG}{!O%_98{oe2=s@vAJ1d$uqiA3_PToYlpB7W#(8;pt@?-M zL*`Kr{jda7$X037;MENZWCuv|+p4(qd1^$li0ib@J*RI z_a^5zmH(zGjgvR@XAoggMIL7G=@Uv?gBtSv{KUB9^5X5w&8Xe$t;%~9#hXM!)h4lz z{UCN8p$_ar*v;95bF(JR-dJScv3vkD5Q#-c8wMb<`k(zvNiS7BB@OV$lhI2tL{~QdAYBCp1N74{_Sg+_))iP`+uLyyk(b{ zKT>}aZ8GYXurF+vdypA(_ITr3(Sn)$Bhe0eo8tHfzfD|#J@n&E42Rh8h@%_ErXzYI z>I^g=Eqh#y)I8qwIY+e%+rL+wXWT2!M;=e(SYX&I=4tkdHsFBf1&-ub7>y&wZWjmQh|9B!@%AW49|r04E6hChYGv4&tC3N*);Gc zzA~3;A8kbo>6oouxucEB&^|b{W%LEK3OJkjcC?C}+6ZC5W}4O7?2xDpu~Y5$=3<9xF!DeOsoK7VY<}yq4x5HKcdQwZ@jpd05-{qyA{?_(_8*r} z{T_6ltE^QQTDzUNwpN$X7SJC^ug=3jPH8*+nFW8UgRg?Q3>N(lp;J5HA)<^f_ej>@|OX^?UJH0keVISc}?E;oq{2=d*c@zBG%8 zb2mrr4R&Png&F3IP~bcl-RPrF6?+;AO+`0UgHBZ5a$Z4XMDDGlpoPG_nBmK={cV$| z#@yAr?%t)C!GOMk8NBQ~C|F&F`HP8zpQwaWg1g)*qEyUib(qo6XY(~->mxTPg|oEt zlc*=lG1N91nx?)OQIVTH>)k~0UstxZ#L!Nl*3*O8;&-CF^zZoY1=K(rWdQ;)thK93 zvS=O|>9}V!VnkLkdUv$_nm=>W$`z(Bu0kBwQZckjwm1!uCOBc0dxY&YxXRBlB1>Ar zkbC$ABj|$@%G@`X-itl!@>}8ifBh-aoDmh79hsvPzUz<)kBEr3(AmeF4n4gV&(3sc z;SK&!i0AGf9n_0f%wDyXf8LL!MbwY8uFxL+ZS2tlkVjbuY!orCl4=BllAl+G!TT~AGW31Fn($kO~GU`lV1 z16Ctb?oOEDz&yg_lGpBBnSgjCalhb69BV<1-*tok;x=Li;&d{hQ%18E)^(XXDt6(V zh3_xCx-fRpoJH?1y1FR#**VX?|LoOgV;9d^{Qlyri({Xg^W6K-U41Tg$($wcFS)t| zNQRIhMO#*gdK@u0VsQ+_F-W&UjAB-R-&!HYTW0W51kI#T58I0|zFt(J>t;F-&5clS%m55n%pY!YES;ijMWt zZ+~<`q719>I9<5Solr~M9eVgXX8_+=PwlJ0@i^MEJO2fK3Yy@MoWk-9Q9A)8( zKeqa={-^c2ja`>pp=bzHj)65iCp^&^pUdv#gbSVQ#wdwzghp4waNCKzeSD+u8~=ar z5kJn{>blZSwycz==lTK!S)MRzWI+NBaIvIUhbEG+~Ic`i^bJExd{wJGZT(WtQg=v_i%z%eS24XsAh`|<) zFPsSul_jh0HL&uGG?XJA)dRp9+F9(E8$CAGOt?UP4RI>v?W@xYOkaZQ{6acpILmRU z^`GP3C!xlCcIuDG8k}P_5dB(IzMC=w!ktCN&#_!DO4O5tSqrk7I z1hSX*L?7NiTTPn$`_=sS;0|RCtAbBdA_q=eSN;lgq|gKJBZx%QTdd;-VTtD+|k+HJL6{cTm|A`j)I!DH5 zhyezZsF@DhlQg!#3oA=&ZqG(bj!tX7hF^(j)t}LJ0(#^YYSEGQNzP#4c*tR|$mlaf zKeV9KiZ}-wE|1?aexL0l$$tcS`t37SL84IwE_3!Iqwr#p5f)fW!~;2m>hLylj+v0w zY?zkTtewF`{d!HtelK3EWTLiPb~Fe~v<>Z$x0OKX#^J5d_Xuq`P8L)JIbVge9ltW48(ZRs?UID zOl!XQFeCh>qZ+iH)?AGmUl~6=Q+fM0i~7WbOd3g7$1ALJEJoq`Lgj5?I`9H@UxAkg zC5>M^DXqCh`rD2Y{EE^Dk1j^}uc6I;Mk#-plGfb%a9Xq9Ea*07Ch|&rcOY`)MSqJ@ z^uwXH1T^>u2Q)+k-k!X8(xFMg;2nM<8h{ONT65n;F%SJXNpbtecv$$b7I;{`2L6Z< zmKP##eDP;|@!4f)?T~__^8@&=qhENo*#*c4PeR@gCb?spS`jcw1gA zwS4bF)i`}!9>yBkCWJtCav0advkN$rEQNIUBCan2590JL$Wt(0llCyW63D-&;2`~e z=}Robyu5+>@A;M#-0;BqW>Ic^z(D*5u8s^ zuP#Gx;QI3y(U*(R*Ds)#pF#irO)e@+nSn8gz!f6~gVxycD;9RMT=cUN-BCX>!XxdT z{=1lm{zIzjWdkh=$aR#~CoFr+Ow<}6!4;a&o7V5el6Q^5l>&_ZzoEbI<=gm@dtq90 zGrlaMH9Ma%H4-JZ=WRiW6=Hx@A^MdnZvQYfQ&bmU66NLJi?0^$o3tt0@AksyV=>>N zmGc%%B5G6IW*+{Z9t-+f@45eg#iSqs-J;$Cw(gDD;+D-ce<0{K;?jWK-i+ zlo?^7D{E@JhA)UpCe~{HUYw87^>_iwh?&3-WObddbun7?N(Do_TdVjWGCyJp32y`{QYZ#iCxCNK<2Ee^aBb z4}3*>$Mfm%ya|-*W)y>&&&IDLTK3jY8#>Zkzx$;6mD-2pm>R7bQ{&?irbb&InlVg` zPxLZ1K8+mndz%^;Aon@prpEc&n9=H?>o7OzEmVg@GtFa{$A2%{OHGX{`plY$((B$E z-!I$LxT-hRnTbU`%b+2IPM3OCqCg9%67>v(J*`a72oY@;FiuU4uZ9NXTpiEdR!TKv z1_h2i0{SaAy^lJ6YDd3lG-E=t8R$ybsD6_eT~r;7DH>l^j4K*PH2x;-#TbMAsc#If z!u@$1=J(-`0c>R>4zjbMv1eb=pCNs8h}DCRnQaDsm|HjKWuR-#`mh!LTm$o&dxkn% zU8!vp{dd-f=?|snq?r?~qU&|Ugxz;E&0Lc8m#d@jm*Z#D{{?3mf9^_2)|Xd?n~SrK zUwHse>VOs9VlAmla@FCT9yoJr4QQB@{8{U#>~0g zsJs#B=VpfG4$c{w9)@!gICut>E)R?mbnjo}R=$ zr2ovRGi$8j)>hZQTVImO{9ZkA4(cnB<_D#{g+kcnjx=xbUhly5RPXgRcW*rJo83D{ zA-w61K$&K=h2V)6etZhyHFq$sdjm&Mf&Gl_DGwiV_!rOIM!blCc4ss)BNN6LnID+C zcHG-B_hnQd&I4@O4bTe;I6sbi-3QPE;_Lv=@-y%zEcKCHdq}l6+Eu_|d{UKfq{k!0@=4a3mC0qY+QN4rLvZR2{N; z7UOK}F~armweQ{g0|WLWLBR7;gCD`z95#FvI*~H%JW?VTgJ%VSJa%r3!-Me}UAsQ~1uw?vz;oCi z>T`hu@f7lYLgFTLws`(C=&WexNb|$aEIG+{3qs62qzEU#JL-H2R-qQt?3?NA>zE;4 zGWngOJgW)CPqKZbT+ac?jS#yT{WtXrvr)2ZQ3y$$R}O-X4r5Pt2y}~U0oQdnYYwsT zh2_Jmi^WnMB0_PMt%Z#wkD1^H)IdJYSy2JJsLpW29X!1dKB+xx{bX%DNj8{=J62XU z8}=zWJ{)_vX;M5t;DvMs{r=a8aQ6Hn=63JA(l(C03QO94Q7T|sD}*09@D>VG+WMkvX z(BE85z=e0a>fADOvgBW-5O#MQMt=56in-854*?L}$!*m=uJ%}IiV>`7dckT6a40HP z2O5qb(ySxp1gk^z)MI&B>jRtX*CBGH;n+G;(g|29LYH&0$-d}Rll|GZOid0ge-k#1 z0@+vVK?_;vmHUoLn6hO39O_V@uWjucK~stbEmwpyT@#Jv@^N_gZaW8^mCT&vE$@U5 z>N5k%tNE~7d3oU=zL0qu>%l?OVqn!5mhXWzuvg~5b6-%-{$mJ_orzt6U$E?*_nF!u zp7(|LjjUWit!Vuqy7v8}{pjlQ0et9I$zFTjU$YrlaB(&#WI=s%E3obn2fothHGsXw zxabQnGVYlWpD+Cf9F) z-gXs^7rKo=*ud7GMJkL^ztP~pjG6Lt^ySa+JJ`HRV^glL!ri`l;)RKa_O(5osr`$^ z?V3&H0ZK@_8`K3DnU)@dnZ`DIam%}quP{yWfzwn za}U#44iA>i;in_!7*r2MZLLq|>IS@8D^#OUeW6gUyxP zeX6ql89@#G6>QxhQ*TEv$%juSd5h3*SL3&h7n7jzPs&che6_kQ$(-fAE&@`rT(V*T zzb8o#jF7?PNVeYcGPi3wRw$XrlI3;X{3zH7Oo!z#A%Z9;p8K6P`6TYRD8e{a8(75S zk!)FB?>)LC>-&1x>$LYU-xJjXc$W%cbel>T;0`bkh8#|V`!vJ~7y`5c1+GWH2cGt1 zJHt51MxYdPD6RQ4pVl2foE*#{>|j<+dg>}%&%iMf$LIrjh>*nC!pCy9KxmK;nBsh+ z%78UmSkMt?BQ%o(CZCPNdFlD=JJ0>Rzo!T|dB$pkOqdL7K3GuPxqk>2)Zc;Pf57}m z{AqW3r};hH%kR`cA8-6S%6GJjUy<3eE7B@nfh^yzZZk#B=6hnbZE@yh1=xC8^Rjx{ zIQ;F>sRMnG{}R|&K|Jg5J1RGvu8`Zpjs(}m*dDLYFD)~*I(50pyd^qF@P`(o$w~5B ziVK{F*fMW>*QHHBV0aJfk{s>&I`AL&l3ohsGGadV@ln1yaL)$-$CO*>D+A)hNT@#w zrvi5H0oELAz(S2xW~TQV$gje1_|no@1O9^nKfxL}2cy^Wa`FjY&#vP(xLq|s#{DOb zZ{6%>*%r5Jo!eFsmCJ2qlLMWy+}VqwXR3sMb};(70#CXHzz7Gjg*nXIp2=v>YLwfG z_6);%Pv67*0xg<-z6lihT6tg4NLtt+3SCV0y$!{NW`?_Re2dvzmz6 zSV>rER-aZ&xahD*$--tlA-Vh+@H{Qa+761_2&)>cWc!8nzs1ko z3w~As=+*aH{4uIYGoTROZt=~-G=3y3HR2Dj?v6y8#o-{E8wO*D`rc<}Lv2a_NS=oW1y+NbbhdRCJ4nZ|7<3pF+_GHWRJ`W<|Fda z^hBFQW%P~TRo=do3Sdh;6omMQJ?etbA!c4hq3xzEG&cjDJnkH=5;X9&+#?&9WC_}Ma@Q2;zg4$`0<6^LZaIxk)+fOD|)}ur}WlCJpZmo1j*G8el{+)Q9F;`UZ#n;SC z@GjCXBf`f$F6UVOJF%KUId$3!&L)@Md?%J_(lI*Xpat7tdJJP@J#ZW&*?)oC^YAwA zTY3B0l0?ZWgDrt&kii|z7VA*fmkevgmvOAZu^LAyj(iQ(7akF4r5Lo*K(x{ltlukj z1CU|`YWE6i_Yv$0sBT#f_3jv{?AyeZbyQ2LgKjkPs)Agi%76F3vRj!{Q+m!hPO;(DynBmCQPtp(-hJ0;uU9pEEnPc1 zpRZS&4`ZD8VVwBGPTm2!RO4DM%7K?dybFyP`^VVF!akD5N8NGkd2vs)MgKV4nhJ{9 z8pO}%X;1a2jd(mQ_HCW6@vZRqy{E}Z`!*k4(t3~~&0liCiFvo0@I_A^t&G{FZpJ?mSR4 zp7L8Oj({zK*1+f<%7lev-5Jqnhs9*xT5+Uxt(a{HDPAk)FrY6S4}nT_1?)X*`1Obf z@{=8MibbTQi7rUbtP;Wt#f}vN_ zjaG?0x{I>zPUftOjUS55( z`{BB%Q)`|B2~l5MFU9kbcuv<#ARpV-8Di!!vb4Bg&>3bPj(DZLah;3jBk(*7*R%0F zUW#KX2R^gHJPIQ#1kWFa4Km60NdHW0x9}OpQx(oQ+Hsu6p~YBmV7$fQ+J?h|`|~(T zacEBco=2hid@aVy4C6M@TK-;xO#N8vn)YL@uXY|r{AvY{cp+LQcifFf`wN*0pzRK> z^2Z2BGA?v7N%I`9@U)NY&yzOc5!j*HBg_FVV~B&}o36>`67~P}hKeKl*EwJvBI$biI*v$EW9>P6-Wufv&+@e77O1LMn3`8k>#dA4tA&L@H%3w1V^1df*Pam$Q=3rpNyBa1T_7a!xPE2y9u_SN#o#> z{2eULYW@*Xo&$d@UhBI;ez%<10pTp*EX#KTbym&as8<*~zg2qAD(|^#zZahq&PN~u z5-W?a2G|B>Z&kH@8^TYt=>?9PXq|{SmN_uF2BWgprT zX~MHhI4RA%bgX3)XXVTblX4l`Jbrcve{R-9{Bl)KDTQEPL@5HZOIS*wIf#gTV?uS4 ztXh02U6A36IW`X8QbQihWlx;9Rn`8jrv{RKKqoUYD?+H7?)j$vY- zn2oWY_x|jwvt2H2n+tpHjYJ!ubxK0(fdlcmUa}RHkIksaoO9MZ<}3bP*jtP&*a|x@ zM7UA7VSAWvCM&*T#Jdc6c`hOD7Jk~KmJx5Di!OwIrP&pV2u#{e_RY(-TIJmHmu-)! z9>w2kpyFPDL@ym^yIO8K{joG(SW9f#qvjnSnGDV!|I6n)vMn@PTMVdc>? zFWW9C(~-k;)o}c!9l~uVa?;tOOoC2^ymVh8FB}@Sq{UU+4r$FucVeWoFKR!sbDyb< zf4drUDfp9Qz9&|TEuAl!0wJfxu9)#yIQdHcNQdZ}-m$$t2q_12cD?t91v%{rG5@!! zur8>;_sWIWS_u>PJ+vWV;yQ3XhJ(gccT8Nu%%%4A@v(GG|ERqg{ockE4>2*mH7S@M zyDn@4q*=zb;v^iCaeRlP+IZ%A=xXxR7+OeJFy+942?83A4-E#$Cnn$6O4_Id2Bj!eF&rnyj~hdfk0|6VD6yVdiW(mAPqvzpr?49%A$)d->4Gc ze;rqQseEhL21efx_Q%zRo^^e}j(%+*`cS>g_I_P%c9C~ta{e|`5m1E9`+8DA@`rr=w z(%be=;5)U>>pL}hYVfXd34Li9B6E$p_2Pa{nU27F%7=4vLBd^HhZYEgOe`Jpz^cZL zk|*Y4IhMW5z=^x#3+7o9t;(JLk|*ZVsE5YtzbOE+aD!F3L4|ls<)-nM$_c>;_zUHx zoJ)jdBY9$4Rfum@ZZci^m-O~$yd5dMl{_)=HW6=!UTT-#Zo=Eazk3^rw=tK3ZWmZ7 z@0EDp4>X`lA*$>VpF!zLmW*%&@R7=fZ!ual3gJdeahClj8k5H8Pr(1GgmBbd zeyO|+JTf$Hp?cV+yH@UdUb{Hh+c(#n&TfwQqLO{!i%NIA$^p5hgRa>T?)chD zmlOFKn1oTcp4^vyBpO<-pPz|F^mMr}S)?y|>M|5~Yol&W+sDMjB7zV-+hICfgE1Z& zygNF>o+xi%f;|)w`^G z7c_+AVHs2dn=^)FG5@f8?DSWfxU-B-l7$p8I@qJaHZ`7MfF>x~hvgrt7{ar>IkKv9 zm5H7k8|&;*83kg31$*4b{!-CD&e-^leE{-1Z&wJxtxW9ns0G<0peM+-$QyDPmGHQm z<)_8%JTWb57h(t|@XCul>ZW15ehhs1S4@7e$jvrPO}t&r4k7$xq%+)h()g>ipGQey zuWO;wuDVlNyYP~otavv9m3)h7`?<-$${mm|6UyQDv9*@GuwV^W;L%Srz`#xdO8g7K zq-TK7YZ){K`?zSxG41j?wzK4;q4r67?8NBn)DGgiq-I| zd>C({y>C`k7agUNPfMJRUe9pQ{7F91*}%1$25d76u+1K*NtAq|b0n<>jY7&L1v17P z7vmt)jLVMC;c(CLW9wDA;q~-QcGv)<9@RW`f0^kUZ~BDar4K{;>lgox6e~?`_|s4; zk=-&${!6l+sh4|jZ6rx7dG1m~8OROD*LQWN>uQT1Fy#F~JtC-RAmpsZ#tq4SkYIAc zZ=Gt4`h%@W%RG2CBulLWU6HJa=&bk)=yGv?8V9YHsfV;Vee*9@$PuNk*>$>uP=h*9 zx_-4`IbowG0@FQ4U^3(Q-(1n6^++7wWAtyX5THK<;MZyRb?Wgz@X&H-NsNsnLMWy( zC+tNTjExV3(0a}ZA9NbQTj%96x>R89xZTT_=Yjv-gQMyz;N9bV5eJop{`JnuJJdJ- zXhk63Qla3(@Rem9uwekc78BQ_HY5l0IB?E;oau2Y(H3adTbo^-u*S_pL}!=l6Yv(S zQiU|Pq-QFO@zdJ#xk7nmOH;9I6HWBPg|N8Yeb}ZD-)x}cMk)nBK5c*%JKrr(o22|yJ1$| z0G_b>w`osAZIOD0^zDK8-t-D4F0j@g2vQ}c<0t%bP)CCne483t5|?Xor^?(S=}I`@ z0eJEni9Dzt&ms?M%R00rwIjtddJs9nXT>S!9gwGi+mXP{6nnMD~94pzfSra(K!Tv`$&JoI=>gM6i*dntp-ryudYa> z9_1V;FlHm42yQyetvh1m<8bv`r}2Q1A17GG|LVHlX*?1$+KAte>|o(1@~f+^Q+FT> zEjG)hw~ZEKb^RZj(H}^j_s*(Fw5g+}8T;m#;J3yZ*$0YlOpv*p3D)20DSi?~g8mi# z8z;qt{?+vltnL?_8kFyI#3X%7inqWCwEw4ernV{h2KGQW|BQ1v+Nd2z2Cj+z7^ZYs z3f}384*z2Or>FgN4$#5V&^Mr8e6&Dk;wK-^DJrKHWrW@KS?J|q?Xz-bWox0QBOQcM z*lG80_{87OJ^P54zkjbAe?QeRMdI&E#k=|Y{5$-8bc^VU6FvMr(@x;+=+Iw?IhmoUj!8Xy15BML#;H zv+BF|48HM^g8roO_8$7V`#ys9-xOcNw~z4kIraC4IWK$rJFV~_q7yP3n_VN?4&)r@ z*5~&ZcJ=v_XZMZylHY+n-Kj!Yr5gODJ>4UYUFPwaxjt}HpD&<2UBM?!YLAa~%7DVlwkhcKD4c81>$TpU-B&o*VmB6no!zFKuduVjK|8x> zX=kT1MP0T=&(zV*t~3feyISPcQsAMWHk@g9*W^TQ-rXJY(|v{faA;&DqU+}lPtO~L z?+TWgIbn?ZkZCtx(yi}1UAx^cf!1#pJd^X>7^3}4w)b&=p`ggdB^n$33o~uMqU7>I zlGw9_sPjutcZ(cjMC@*zz6f=`4gIdmb)a)*4)?|d#9VNotUC%W*d`$L`#6UREYs-h ziXYP#@nfcEKb%AHV+E{~N!1qTrm=;x?SAl^UI2+Uz=M2aM=3>X{r` z=aG(heuP;b9U$1jEgOa0f(LT?+4e$8suJ8SgLBy8zP1S4zic0%JsvCAVpCyfS6J|v z?ZJKxsM8At#E0b;;g0pD{uHnfakM_J{gcORZkVN<8n z;k)PUgskDTKdrtqs-(!a&pr&<4+!{{ymyU!$~;GK46_{?uAqQPxXGRtrBN`ol7xi0cu z$kHh)Q8x0JpnAv~mB^oOu~kZOiCU~ToEf9Pw58`bl&PS(391qJ3yPNAa@K&ZD0Wee zHDacArULa(ZjVAtmXW|DorL;NcQS7=u|4WMGsA7yr@%a{YHzqY471~;R>ZM@CvMt4 z1?+d4UEuI}l{A;$gVDOUgL#XjDnfk{=1a1j;eh`bkNJ{NS2^Koi?3ZukmnoiX+JT* z@{z;lk@DQUk9iALj8YE&td~b=`JkJuEPyY^+Xm+_Z_K0HuIG_9zkT0N@L_>{488r) z`95|wuXWJ3FW&jx6ZqZZ?MA$LrlZsK7}jK(n`xf*#W|uiw(dX7$3%C&xtPxXul6Wi z`T!2`ojm#g!}%_K0KI=!lE-qwRJv?@>R)B68wJ)S7;z0Uh;UivIUAe}TdLjUG#fU=BS{%n;tp^T{L`zJd%gGa&U_G?D~q|fq(6gHp^)pVQYqK$1zQXrD7X!<` zfGz8<;Dgnm=@&$~`2=i+XwS14F^+pecTIg@JqMopJH$soKhT~>Jj5i7bjV+M)pT#o z8SNfZl`Zwe?1=u*IhYUpt75weE0zMK4i2im7o~nSvrnF~!F`{sfmH=L*kNNUhV$E{ zQa_s+C3*yYpSCk{IN ztRa=T_8|Idc0{I4 zc9WB*p7=)lt|>l495X%7b0&K*_FE zCj8{4Z#mE|^qtg0UshzU{yo}9rtSYm`;eJKz4?7CaD0KlF|F8RQ3fP0 zk;NCm{^7V7SDai8YcE@Wp0gG6;p4X0J~9n(%Hnm7KGz#ioP@t zUesyumuKSCm*h7UVdsHO5vomxW$h_kNu#8w7(Nz1fD(U^C{ZTd=a!qVOOz-R4DM&n zvFZ}|i;P5H8l9dtnvZK=9aB+1KihbWUYQWp7G;a;7Y^*x7&oKWTM1L;8)s6?`H3HMbG4segW3;1##V}fJ<*Yzn*h6+pazY4D`JamFqxd3 zaH}oF#%=t$0v7TtAsu=wXw>Jlb#zBhJx?@e7v;v-rbR{~2H~Qa@CM%Hc75RH5Sio# z^dZuUQ#9*U*zj@GJLx>xI50rnh0iQ7AGwWg*C@BVzv2=~!{k*^>X#A4+LNv%>liHL z-53Fi9GS2cI(UU(Z6%u)s;907aqe-xn;yQX91stUO*NN4AW>a?I>LMcF)K1ii@n(- zbCS)uvO{K0@zQ2q=PKl3K${)IncD1+>&HZ|J!1}ZRe4zxZ5;k~>9^E}*0s>=#?=zy zEm*Q+G|z9A=6RW5?3Cshl$B_o@)r-CviYw9R!bpQ_+o{UhsU}q3RDCXA=mcya-alp zwj{|aj2YbM6pxerp$gvE?r6@+?qc+ilWm(iVHu<{hHiabFac z)BK}scn{tQtnjMa=MSTV4OuQ?jQ8y5dh)gO#J}4UhxCN)@c8Nb%0g$%7g>Y_Uo2kK z=$5`(zRI*sv=ko)QrtFiN~ux!w;gMtz7Mb75CP3elEH6RKp9q+#Le&oCRC0EkZM!J z(&(i!Sb;;zFYL8n{u{<0W(*?`5_Qj;;K2htklC_0^#(>*VV}8_6_y7-1IwdV?WN#W z2#M=FEKH1k16p5ef2{Sj&Rdz|Zd4h@-C(1P!g_n!jli@t!OpV4-&1ec2+R$#R#{$s zd1*k!Siv5wUq)wR@a$zrkLq((iPNg1_=}YR8JnSXN;0Y)emh~+m9yEz;4j(6CmF4g zfn`;dq27pLsr-` zS^|k9dU>p%3qDmZRFEYae4h3oCS#r7E?tJtj_g6$krmvrBRkm?1aN5emY%4`KkB!y zCX^KQhE4V1IqE%4_2K4HSe=Jsbo6WQZJrNXCM~Y}wD-hypX^?^4r$lmns#-V|J%c0 z*Fv%nnV@L*$Mc@Rs*?%sHWm0+73|SvLQ9(lbhZAO9>7|Z35{(`yPMI1r*K>Vz0(=< zlo@9W?u)?#lWmOE%L`X|*CwK4BX|!V{)Ps&ga1wZB$@D0n}pw12fp{Ut}zaJduYv5 zA4~Y?jPSiTI%%yGos@hkDMIN>8p7M$3*4%;;t(7|aiqXDjS-IB!QMP5VJGhM>i^!D z>WhN(66R(`c*nluDLVg;7n60zQ^58^6jMehmEx~Z98Z5C-5WVdJMD2FqmNu6kmjrXUt7#qk*f zEmBX!@p(|f#y>f-dww#=8m?c|LC2tjWt>cqwfS&7!J|u>JmOV6FmNoWjmIE=yIkMy`lO8meL~iT z{`JLJA)OxS?9K2Qx21F^a zLl0bqUM)Z4iI9rXA6bgNEmgdsz<%43IYNq&O8Xrge(W1@*mF#PmIX5fP?(gf+g-_M zQAKSJX>UYr^*=yI?FoBwLUz#pp>=29iID0^@rh~;&=BRquO03|Zv*vo16s8naWMC_ z$@0-JE{-!d%QBI`{8QWP)$iA zPBCq%-90F#(;T0uCVTrU3|jcEsHIsB%@*3z7TJ3ef~lP13I7Xd$T}K(I!1UGGJ0z* z$)P;Gl|tU>=&eb?p=H^;|9&O<%74RNtVXaw_x9Ep=JSy_Lk;s>Z#rJpQ_$FABWbzw}W_`E@aY}JIJKn@ol^8 zD?E9`N!oPp^_>yb>kob2HKM98ZqNc&YKf)w#lW{M$+}VRiCpB5(W`Iago;*A^ix0F z_l4}C;ziF`A{wB%RP%4oT>77A!8hIlY$v?q&Fqa%mr=mPM8PIb$w!w(Z;aVEaHD=> z?8ZTu%Sw$YJHuy9o3(k?g<0XVr_I(5ErO@Qfv3q4Mu-@(qX3<#uoIu29_~B@)+22lJ@so zP+xQ=t(~wQ9a09(!Ml)kgz^2L&G}wrJ{_%t7n#Nno(*vtHRj@xh;*FFDApksNq`rj z7MAzW^I{CA(3s3r0M7-I!lrkTbIhOSd;C+kyY*Z*%Zlif&hGfaOl( z7W;uC^4)3=YBHi_kvw9X*uy}$$?D8@R}ALnIklti%nZ5pWG^7u$u_(HmRk4V?h)a# znwgc*c7}`qU?<%10K)mNmY|4XW@|ziQ?&c}^r0c!^mlS00$iziU zLFW`}E+6xpeO`wv7JDAweAj(1M$hazJn8mNZTS5??WY2~@n73rmiGT9 z-lgrwuKD%%=?(u?`c>W1lRwXG*9y0959Yal@y>ns_`K~d=6Cu3d3@R1u1B!^NcQy$NK(w%$C-xEKXn{!@VrI}=CckOM%)m`zES0R4# zD&XJFxGR3LT=)=P<+oiNzDam9cgG|3eD79gSNjBv4#=s3cMk`2nA0-m_Qpjz2~n2? zB33F_V$h`)5xy?+tbxw{^pl{IIbC~P_`@7x8dh;eF)mRzq6EK`;g_d6>35#`aygu} z%~jMw&kuLTUG7RtWJLWflYX%fSdNIxjQ8Ehi|l=+x3i`6C7ot(JNkapm41Shp5FW3 z(b<Ge|jDV;n2WBO1jJ-z?4^qiDFwNw2c>)$9+{ptOm zrN0bmHsOTPUTc_ubP||At^bIX$Ao1^f9Nc|#1NpSAs|c2{(3=~KQa$P`n2#~gXX z6pVT|D!zU|MEBX;B_Yeulp|tcI3vwzM#daU+zw9cFxA% zCZ`^9X94)#DF%1q&o~dm`I>~|?RK4Z`*6HdIqe^CyuSmEH(^S5FF(m!DMB_dI1wXT zF2%^s9X;=%NW*E7v6w}frl@?1$Z28_qAaAP?oOO+XkHK}`^Qo)4G2CExobx|t^u+nTn(L%7B zX}8=`aOoqNLFz4U%e^x$g`!2djDpn+py#m8_xU(?+@J~{&8s1m94(giO|3kTbKh~+5XTmNc-4-32ij1uUxu2-lB%S zY>L6JFMF2AYAGg8N3m4>msnDA;`t%X)@+Z^$D_XTAr|Rbl*zQt-SW8CGOclD-lv`*7H?9V$rWxPC#)@zd( zAMN+q+Yj-pDfliSTI3eX`LOE2xX-@3XRuGIKh8dID)BzbA^!t8q4-}NAuTgCC&3LI zk;lf7%NWZYb4Ne3Yc4cat>9cP>}$_}ZkYnUn^*Auog*Ho2x3W4R-ZgNMbjA?{q%NL z2Xy8YftJ8$Lgv0cS8MsoX&m^jZdg%bp!Q*zI_M=vr-g~fUUgs5jpgC2+C3FkP3}hM z>0}es^{Q-=H<=C;*n-LQrk%{_d=ez(WJaVKi0)3E~R zyJLX5($~tl4a~ZHHpS(LTT^$HJ;h9-C;0m<*&Um!<2*?^#`RT1bBk zduV79n`mHx9Y%NueS9adPT`I2|8_#5o&9AoV!BK{3&y{1@u&aa;(zde75^vVKHMwU z-*v0_X@B1#EbOHH?PgKD$&Yvc-XYAF+{?w(oAQ{7{SsrnEbMMqW+`}!wORPL=d!T9 zodu@Ufs3ieUxn|1`>;5}U!GCf1$(^+C3o3ng&8s7iPP(u=3*H;dW|XAo{*JZsIjgz zMbs(`a)+u>UnmEb8D}pAr@x5Zh^4Su$nfa0nuSVFp6j{0nuXVTE(>qBEBOs@xx*6; zm)CtRHn^0wEBHpZBznT&Qsi@42$zy}&xrV1&rr`u{0kq5C|=T+W9Z|49QYLjv4@r?V~&TB!LZ44 zj6}@VY>Oj$EpZgSZsWEE@zA3KYEiiMH8DcVmW4K2AYTz-2v2&Z-gHgMDMI(mQCWG+ zR#|fimoDBL#De=D6iMT2q6Q`aCJ^_!n~=w0XV}g8JV^#HM$Xj)dL>EaFb1+ExgHIu zS>$pCF4}7@5vZfU4-%g~qr9$iMf2R5fX|*5okY|7Tq<`teuq}a+r zW3Y@>tWg3@BgXKdC^N9&-?5{X&A302738^oP5YuX7}eP<+}4xldLvEAHw!;`)YZJp z!>gdp3=jiND&8B9>`jWnBUB-L{BOedyB7YSFTCDLF;MvEeC|yh^!FuMqovx6b3FPO zlay@V_Lu_oDMNi~z%gmynf0e?*PdXj9DDq2SUdY#U#XUXKSbtERm9{+@Ow-x zj*vw$&wpV0#qr?txr;u)+DSNgTHgD6^M2*#Z=xTy3CFwr>jd^Ad4$Jj0*??|TCp2* zM9@!=T4#n!)K`~B6VZ4K{49{L4;*QK4K(Lvrodx|fD9P~4W1>Ub_4LeZ6#t1%)ru! zF$*($olAmgwf{j{?N3Xq{Yl5q&QHMeCUbBSt(PsA8>F{9;HT}AnP*CTCyZ-sH{lf& zTn0wLr5o#q^4pIO`Z6s2sj)KaYHR7_671vdw975>r|VFgHq@p~97uX!jCG;`&#^GKLFfAuQp{fg zU-K*A>dZ!tRN^VjD@+{m^m)^yxfycGRmyFF(3(m)@}|B6Xa%t0Czt^2y!Y;(-u>-*iT8rReunPHb@1!o|8jiY^M*a@ z>ib^US`+#F*Sq4Bp~vrgzOTy$KJB3DRi*%Y%wk!xs-AExo>nkSrvHC@W<6o5!D=#}CCuWa$fr)q8WyvgL@y|Lb#Kn_p z{(I+1t0(S)k8jPNXJul+o@KSFVUXBsmdVR&zuon&3HX4JV|+0J``I5J4&FNN~dOq(Oqk}mT^2V5yK zSZj}{<{W4B%Eq(~uH~qzlIMVD5^)?k@A^w-TYbLu4)DDZT2v-z04(G&f#t*kImO5A zw)z;O^WvGrTSwx&Kt3lrK5^qr;Yb2MU=s5UZj71uadue+2kzI?(AlB^UmLV|a2qD$ zTnX3P>??qt7IH>b8NIwg_1$qrb6~#{m!f=lHvhz%H-#_l#FOZKH)1uaamHIiktWSn zmDaQ1%KN8%E=FV!w7XEepE(1urn{ab`8mebr!&wja~V7H|?UgANS^&qxQw^&$gq$wS7x!VUm;RgWIcR+Tad(4^W(J4`v3tJ%7@yIskgV zy7rz-byKjqh*zwdSnEf$A~^-dRaZ2bS3;_M8!un2F6#>!YSM2-xJ`GWZ^$}R)A7cP zw1mAG{^nnHpM=aB^^BCFBfvKp&`9qaY@AywkL73a9fAyZf)Ta%^O@o>BYXR<%)WND zJ4Rja3)R19a)+|Tp^CQtbe#GTd>P2sWyefjS(zx|muk1s3~A;=(36_MF<;km69Q4y1zZ9hVFT6&PL=FdPnG`=3qK%U#Tiz=+c+E=O-Y{%d;5bOFSX}p2DoDa`k|NJsT-XJS&S2u-(#w8 zj(fw6!HuBvbZ@_x>d~^e+t%4migIoFR49agDQVB7*d zS-+%)8MU-6d#Ad5fVAS=&p+(s%Co=$M*JWjo!;nVl3mzwac_ag0l2Z7_<+Xw`Lkk( zr+WTe_Kwb7m6;lMjOP23T;+tc)2b^c&@M(M`5}}zHs32btyK<7V_2Wn4vY52gGn4 zoAe6WaVxYFGA%o7Ite{!S2uL@*r+E(+XKqgZY|q^`>)M4%C`dEQ8r3Eqp%S=bu}A! zXqG}A+Z>49sC)-=&4zWjKY!ebZyR*3Y*>x2ZIGqp7;mlc7#}9w>~G|?WrwP%CS$Bv z^{=7soD#zgxU-e`Sii)(7Cpw)pgNm1#4l|dx)VD*k#I4UI0Ga_*mjCXIveSCO5V$4JCPU#?>l%=Leb#ELh*BVO_LhEu?v3 z1D>;d>!OJBm&Q6U4;aEg)(hwNn}#^p#9{T3m;vJ$?QU7Jt{!t^e);6X&4)P!&QQ?{ zTC4BNS2WARY?3ZSyoDdbO!qmEg~1=qg7SuUhBbgIUJc2t^2rV0jwW77?^^x7s6=Z$ z3O=IVb64Fh<}O+T#l1jNS;%#=?!I|!i!q)?|CV)d5K@Kmj6+r+q+cDxa*aVQ;_W90bmB@xZYu^R)i;_162#_e9ID-hG32 zzqwv~?1t|(V&M(nuZqiV_+BQK-thgLczwSrrmy15f$#`q{@kD`I=)5d=ShcdvUunh zvOTGG{kuI8`#IXJ6?sUyr$t!X70{hkaEcd_R5=|I|mX<}k7H z&Gxw0FmeY%8m-<#(otPDec_U7P-WaRb-!vlYL+YCw`p_ADBrYA7~5I!M2nW9V1ft zfFVDKbwx0ragE_TbXU1Pv%T8Mj668tVDk>yo4l;KShJxyOI83`kA1b%O%b9@haJp_ zd4qtJ;9y2EYjk{FOQkJ!8<&x~HB3A`QJb;BeOgvh{Y64>{gD{?c3p-y|I2LP@0u(H8J@NO#PH=o*J-YA}Ddn#bLyVNNIp$ySN;}I#Ys#qp zFiaw~OX{}B4DfpUTBWr{vAI>43Hbn`e4IAi=JdanI@bVyJ$2iNI4*KYi=&ocF4JdeXX^61Gn2^T}(zW`uy`*O+pJ|HG-Q=$1lRy5BPe{ z5pN}Z^V;~o)h9j2CQY^1VMifDKiy#?IrJt0{XIS(Ie8o*j!NOTAbdW)wn<+b;g5-J z65jLZB+2n6VT;GSU&qgdkB>cu8Xf;_I-BLWUbxGESWkB8B+LVnVs8>&^4Q>y z+?s@ErC2L%)3bE&=YUjuJf6>ZQg^euVT}~_0}orumNyBHcc);)lg&$>x|&pAM>b)6 zmm?+lFKIeJjA!9!p zZ{Y8UKWF#)Eup+lPL(9?n*@WW5b>rVj#v+abu}K+_f5j{$Y-k8F8-+9z+bX938N6l z*SHnkg7-ks30clu4g5)_Zr4F)w7CfAiZMC3FU7ratCLDkHNhgxD~>R!>@yH2)qwz7 zkq_QSCojmqT4TvPkqIJzCvfR zF7!ozU!fcD1ANhL078%^xe@t5Qg(t~}-ohlR&amUhTUv#c9^6YA-NN5Kg{;kJvMKi2{Pk19 z(l{hxY2dt!GLtSg(si;K&kWFC>+swFvk!*WwhgEY3&wIIo&{JR0;U8`jmJ9jV`m)t zlPuOF%{=?xtQeG=Y5oVKw*AEl4tS=W3azDu0_2V7-JjKvZFo5qMsr? z{$lm@bnf-^%ZVu*lpI-qA7qW+HHN0oi%?3O_5g#K z1i|6ErA%dw{QDP;M&aY_ajh!=5AGFr2#~MWAc*8H= z^{03GM33xijztbW_QYC45z|MIGScF?%k!=^1kW9wAFaW7ZuP|G_rddh&)obVJU4pY z%@4%$@17s?1I*N0JwkcyETn0H`3sB{<}sK8n5SW$gINZ1WBS-T@YKF;;Geb%p6*tm zUw3l#CTkqHU*!#JfQcW39<{P_jj4m=T9Y1aVem-B>0ir)KFlki=a7vp_EL)8`AVAA2CY6!#(+aj#)@anMF9)M5m zh%v)uF)dERyPZtR3eC@$m6_NUVQELR)8h$Y{CnJjy->IeyijisZ50Oh5Kbcggxy{tP(5IdpIWpVWa35;V$S0-dDXcCkjXfM)$MWr{@0j zJ!*n*b^AuWWF{0?Cbrb{ByeEsTB3@)T9u2ki*km%^!x*ML)j@;IN zb?}G5kO7$oeq=)xWLByMlzk{#bb;Gcn-j3xo)xHz`$kN~SuwdvRR;M^LN#dpz8DBf zSi}E{GI;SLj73I%0lpsZ1XmIAnp&jeAHnxSokpzQ597^*GBB>>B530>&(As3nm5MW z#HlG2l0iwe$y}{K{WG>!6=Q#pq2upGnCVivPa>`v_?py-drHLZC`#q0;rkB6ja>rX z!lk(XNpWZ7N^!@eUyIo&bA2wwTv9wtirI(|L#3F%Ldm4*eA_KzUMIzTPw}Xnb(zZ3kyn^WU)TPtH5A&7m@$-tzHz~*wU;Dc>BU^7 zYV*=0zxI&?UWI$NR3Q63E4hy;mcxB8G*}JRCrEdAuLZ@Q2i2X4%whKNL*{4Q|fj4UkxAl)2B`WH9((Dm|hUh+Zxk;*{*aTfaH zHi_5XO8Zsx#}|+z()VF>ZNz?jF}{R$Z-1=7t-#;#{cAh*$7k{OHOfl&yVM^S`g+Z^ zn9(0!kYb)ty6fQknF3;6od7xOFF>(jpeV0$Y1Q+cnlf?T~U|GGX9f=v z&&M#yhZQ<&D74bomL)cC_$bNsPZw@&oYo+iEPrBokr>e`lC zbgmF4&*hqX3HrvEE4FFWQ>xsBPf%~+_(@kU?$~+$zl{|dCpX45_LgxS+p*!ojsA3! zzac@C;(NO&y{C8n+~vRWk5 z+=Kfzl4r1ZuJ5gA-+=Q;VR%x%`k?H-xKHSF(y?ouNqZC=9*hzASc)^MoJcE4lK(7B zKj3|SbbxXFrHgS*@1i-#2t)Ds|A0M$_!8>}Nb$u<@eM_MCZ?)g##|E8dirKv63n>0 z`&sz0YZgX7oo~&ewVHN~wCjnq{w!o12*6h)&UVAm2QNFw{%2tkQXYk-z$ zo%qd+9CvNMO7Z*MSNPm@t=H?kclwKb@oU>|jNhOCe4qP06}que+y8?sdQG9xu5YncoW^A-UURH@f@tukq#IrM$8J{rPXl*-+xw z>%6gk{O;JVN$&RcTf3k1xz}~v=zhI?KK*|^*ZBc&EdM@9KcEF?cfUT$w3e9Bw8tv} zWwK;S(ag0~k=A1D%8!Xjz_ByB589wxy?ALhPKO3BdY;qvoVipyr+p5-fH4!h1kX>z z#W_pNk%b=Ne-Q2@!fmn9dGc>lk6QnhwF`GB$869_JM)rzxdeCgmtN~VL4ZEry zvWHJ(_04|;eh%26Sy7hfvIUZUcknTZ+y*9Ay)9~cI&M4CLBCGNo;U&sI#+=?AJ<4J z90ih7ykiz}v_Y4hnQ$`EdQ5fS3BSwzj=Rj^+ccnPlB5*_8vjPOnaPH3l^I=0R(33< z_h84eb|$gjAp-B!AMRUw7WA}w<2+Fh{TwJ`ws;pm8Bbnh*D)2DcuNz<@fmpDDH6_3 zjNLR<_94mjrNM5Dm}cH$O|*K1k>HSuwgbgpcc!O7?wte)>hVTm~2DGG(;Zit9d@4-`O9Se#9kk z8TXnb3kjM5<`~k4tJ835(yc^#tHsFn}~myRP`BFEgzc^#a|?ORb47DdPe%z zmhdjpD^Q9u`Sk|)*Mr{tmpwwc=f>|+PsxI?>O%{F77BjN9$~qM=p8M>#4i8+!yoet zwPSq!@A!`{VjsLYn1ejoAMt;-3vb*I->oFnIOOkgr$0V_KK06Jb@SQ-( zX%P&ag~-dT!#8zPcyhlf{I&J?Go0^fz0pG9f}g&0<9bllA-N+D{`G+P1rx!=Y=pY3`Cvo{Smhh_Me>mOOaUHpSu#fX^2{`{o+4n!D@^MmzfsfDctI)dN zDzo;#hPQuf&OZu~$`cnRy%Fc12X_D(X$L_3mOa899X-M~-F{r}9$^&fDD-15FJSMw zAl^#`@hdh&ts?`Sw$Jgop_?Dc=QuZt4Dk5C#!y#op97w znus6Pcr=FezJTX-W-^dlw)BmYtpySb&OnX-tJ)d(o7(C3UuuU!cx#8=ytU)(@zxIA zQTGU&+k1pRb*sRgN;1Jc!iM(P->DlT#s}5S&GIRtFsxPwnGAoG5w&>$-5(_~uD5Ih z`4Ve(WCai+SJ`-LaK_{&C3gxak+0eab903a=ySo)wM1Bv%l^^}jr1Xhmm=#D9_elo z9`Bj}D)%Alxa#_u`k>-f^T zP{Pb4B+{7Nv*1)csZ5kmdSj0_TD7=^X>%O2+G4$FYv+c}Wy@QIs7~Vh1NOS>PPA!6 zEuk)Y?}+?m+0xk}e9&1A{$Q_OjUM45Ymdz6$6~x$uZC-H z<|4T@&-ywv|1yp8=FITJqPo#(88%n5B=(t%MXZ%Li`eFjaMEaRTEsS+LIX?E7s(c} zV;XjqnbMyeQp@PmpFAXeSD5-7EwLVa(#wfaVkz<2mcviUldc1YU~ZJR3#Jvl*B{z@ zvK_R>ke0_Jxu0|CKc|DX5&UnrE`aL@=!okGcJFZOg3_7-SbaxO7tn;*5G6D0Y2))o zbu8C7B=Yezc3hyHNq)x71PQ0S-^-fR@)w&lidIK3cn&{x8$-TzP|V<;%*3tR%S>(A z?M+*ne{`8>elU!|0dcsXbswVKZ30 z+#uWP5k`aZ@3rM{KQvW3qjd|w1E?+N%c*UloVp56e|`x^e*TKEYIef>BEp{<=<| z&(Q6nT3QD^eKtObzaGQIj(SnclvwAmOOBUVBU$C~m#qU_wtN3(mqC*y+sNX#9C{hc zfL9pCFUOl6RP_fMSa*bMj-oGHGEHv{X76J%YKtR}inbJGMn)}3>Kw$oe#sQ}b8%V9 zrp1km;}%ED)0>$2mc?Pfo?`T$i?60^TJ+N*-6FD=&7Zf3gFX0C#N{d9BCNVuAxpi5Q5k_{s zjg)nI!p`&7bPMQs=NB_0M%Xtlo^R!vO<^-cb;xXMg*DZ($nvozddY-^ewV0~;Ne;k zP#I<0gj%36S7F-p)Ow7kO;4R&%uYD_6zJ#Ia>(jHr(Yr6rj?+5J6@Dp7I^w}w_HN` zqy3^=7;+5{z_nMNzbQY=RS+`RZU@)bJu$coRdL|;+&H)HlJb!^`M_+(4Tu@kMOJm0 zXws%shg#pYl!(Sgb^)8h&eMSpT;`U`es<83+6JDiX7s;A7P$^xeJDMh@X506MCO@ zj#cdnQBMjO5yS**0{R9;2j{s`H9!$+1g=RPa7`E><7i(4;@4|pm_fE>0rAP&UK9Hi zye0-u(+&I_TC_yZa;Hj-oH9gg2Nt`EJjr!DKLpYzVsd2 z_S{{uuHb9mX>FFdwDX zUhF;|A>_-C!7niy^5)(VwHrDP$(`~O<;xH0|C@&V=z8U+_xY$>&~?$7pbqB%|M^JV zTiX7v0i2CV&o-2{Zv>CpW2Ye<5^ICRHS`sB?qOsWkr?ya8&wukBQW?<#(Gff#S#Vdrz>!?-Yv)>jT-J-BYHY41F%M* z9ya4y4&yDU_t(3gPwe!c@x9+;uYF_vO$+9;sso>YEq{JBiybJ*t(YBaP3n_F%_iW& z4?}uicX;B~ny%L0kd6KIQITI5)W+gw88~-)AhOr!NE5SUjNOe{gc< zIE-RigQtQi{V@-vSsHnLUh&s4>Msu(byMD`lCL6;*`Y;{S=8HL^Mg2R7xc_>in4$Y zgSMk~0^}7-tRzuE@&zOf@gJLd4B;k|CfT6MKI_VPAXDmB01y5*>-az* zl$U}xTgT4q-8MHzaj)>eKS=plv-$4!xX_Bokn>?yE6?HmKSnmL(E9O}*8kcWo^ z&I(!OccQ499ka2M)qf{;F-#)c$&6VD-Z_+I#dnv+vxZ)JjKhvmdAyPceU4ID6y;xB z8n2g1pzjbK=^_p7R14%!-b;SM(>8ghtenxmjJEJ1FsirtrRqL*uE0rUnrVJ>I=ce( zXv-XaETUP)WHobAJsyU9K`wL{d&WfaW1NBDoIw7#7$Ao#S0EjI-)6Ney;;Mp<4aMu zer&bcw&^=bz7A$wt`qwj{RU?Wuijifx!7rUGwQj`X&P^9Fl-i5HD;0f-5AFt!OVu9marO81Ka%_L_dk|r@`=l;%y(`U_Y$8@Sb7jY+^$4> z{pwkR_EMrBlB~=f`6>Lj;OI#m=n)ooFSaIDA4Si24O~6TOq!!lSzfS!JN}vtjY66o zuQ(Q3?tzQUto7yRut@o-!n*eLP4f+x@7jAz+^hq3hc(t*X??M(#MFl$>5x(TcN3?L zq^laeRK`a3YG>6k@afWy6+7m)!7Jt>UgFWz0@v+VN%r?ptEp;`=PHx(Sl@`8v>%&vw_m|s!FB$(_fYd?OG6|=NlH8wosoLZYax<1wVFEwZ7 zwjKliyI#V5*EcLP>Fb*YDw~hW-ZUxfz*mjMUNo9c9E*Tc!IW{E$3VJF#U`S*#jYjR zq>Lq&M^#FDlY004kDC-Ol_Du^_e@qtQp1e&tAMt0?>w%9K7csidz2WCIoyCb#*Akp z>Qzgo_WIgIWuWsC<$t<;hN?fWv>#UoJ}})RbE#xy#=yJn*!R|ha>}_>%s&U-mhrP% zJ0iVay%U^y<7&Y9K7rSW;mIm7e8h8+p8#cG0?*lLXT1rf8O<}L6L>Ys=MmO)mE(5j zZD<)nY}&B&;0&C54EhG<);17AZh>9C9J|+KkZ%2_3-x>xKa6k zyMEI;LF;#*HOx{lIm~(^emi5eRYRw8A=}7U&DEvgsM%y{cVriqLN8wh)+DuGqcKH} zKQ^JFRz}(Y81G#Y-7`rJz8}Nh!8tEV+Mq_*2WEX`J*66TA|}TxM=KbLE49e*ef<0>UC)f=I;9c$U^ZpXAub=PN&-1H`LB9cyT~)6$c1VsL`t#&` zQVq>Vpl1R1)qcQFp>YcXIy~TaSArG+{>E!`0;1)SKkxAbvtWH=$b)+rI@0 zXRFoAA?Oww+x^rcDn|jr)BeZ?`O@nte&3$X_sdqhEjUjzL7*EL->W_b{AZx}WbVYM ze%};yRBHv@NW0tuDnea7PKK<+TYUs?I!I^}HQ^_RTi9|fHQF6{X$!O-V+PDDsd~-) zt6=SZRnmROfTu~K_;Hl4x|W}9FHc>b_bpbAV$;gB1TdwQ-DddCda=dIZ}K;?Eb8G|hYA(eL* zc*Y{Cox=R?xyXT~yFp~M$V1n8tBNOIv52{~hp8S8$p^~a&*1k@b3mHA06Rc@-`N!l zxd*54V-Hg^5hY8&dpq8T!v6@Ayt#W6e#dq@q0f5Qlo2_}g%K7ed1>1NJ5%!GU5s(y zRHtyL$6AekhT3XG84_UY#9jg|AZKdi&@a;0oLum$AoJC@#q>~fCL8P=#qW0>$Eq8W zg>MbDq<6zdmP(;P(#c)%zb56>fvP^fvu-b82*B zw*vu)9p)A?ME|%U`LtB*sjdo7VN?>Y&aV>P#W(%neEExTrN@Q+sun%_QE5~MXQ{2F z)l6)UFbggZLvBEA{6(1lsYlq{4cr6nvRw<^J89s~z65!TXC>L3F5#yxDj&^nbgr)Q z?WZaJev!W{i&&C=6F=o&>~;xCU;Hi5gP7Z|CJQ~d|&;Wp(UzENP+za zzSzqU`=u_b{hzPZeiQVa`~@qZ0)0(SJXgKq*P`zbU4!ahfp{u!h+i$m&%*w1zWC=O z{+d6CzZK`^TgOlQN^eQ{2VFvAC$)V6)(trf^)Z^S({ki|8D@uINEOnqKH3iHRqoEb z22b)ⅆ+TT5{on!>nBK0j8RPqW-8tAm!)~rCH&Bn zoO3GQDJ{pg*Izf>e3Q1joxD` zEG}WGFQpg3qp|^GLt6i}G9&XN(@6EB#ro%!2blMfODOC)x7Z~-)U&5!SsSDOi^fa8 zmu~sVGydp&4Za|C2<|AAn2|lg_nvW98}9V}xogGlF$*feCDS-_kGt`~Wc!f)O7{je z`T5$TuE})C6pO5B)!&L$IBUVzjNR}RVAt1VHpz9~n?CaUZ`Ax1dxlFUKey{fm(RN8 z{0V0ef6U1aaiBcD{N$YB^orQVG zBMj=wbLliIeC_Ayo`pM$wocS|pbPYiss~B_8 zkL`hwzVq%f8`{<8dwjdh6YUD~-I%A;@be#F8L6S4uWm4>ezh29P#x&2sJn|W+hGb| zBH%7od)GYd-=VWuV~?`REM=ljJIx|b;Oq*tK}S1e@8a&87XgZ?+UOFR_UjOrYY*cN zR80gzE$Hf3_ja~ik#+VDW!(mDT+)$R)_n)punY3Acc$>nh@Tzfqh%Rvqa;}?vJ*}E~YYZsR@vbEfs@->PzzX&7R<(0cl<+Yb* zO7}2EXvvwp0eP{nB>Aj9yqqtsj<@Lez%03IXNMg8>l(ZjcUY=TG0??l;R^JHddO2l z*R_?AG-0J4h_|#khoFu~o-Di})E<<}=!Rw>rxVn1vy1^b##V+6G_vDMnx-*=V>{Xu zVGx7dJlVG%N41k4MAAV_@{iL7a_V@*NAvOQ-Jm(Ii1`56C6BPs-$~kLm?8a?xUuGN za}AD0b&$Mqb;O)BpqB&%0c{Y}3iZ`few*SZrL3e$1zo03X3`0O=nSVNilQ9o0di2e zx&-X&YIn9WHlm+1NSn@O{yXp9^;k>AK#NRg&WwaqZTKl~7Q$afSJf*?8I8H9A@1&b1 zHFTZRtOB}Pe9)Ps=N|>7GQ=)RigYC9(|n_gW6C_jlOoNE1tqLBI~G7oG{*CaSl$R( z4`zi&nCo)~Rl$JUSZR(3lH?l|+aWiY&8~qYTL;CVmHGtfOn2=q72h(HRLOSWOwB2! z{DdGsTpVF8r6NCuGi%{v$!<2ufE;DoxuiSn@}xgG9(~@H#Mcv#25E_BK%*i4|Hw0B zKfZo{HG^9nRpX=2vtw=0{E`CAFMXi-B`Krw`QBkv2y^)~X|-c@^qWd$$^_;kl_?Vh zQ4iTB^$OD?{Oq|vv@63|+!#?@R6F4J@H9BPg%2>Ed$*@1M+@y>az27T1`dHz@w{P7 z4bd;p8Etrv00$VbPjQYTK1>Rix)T zGt|W=waTPtMxHp>T4y-es?;BBRhikTz)3u*#*xMvSO!@uwRoa(+w#+ z*cuY|*fa4@1=h%1oUF<64R*Q7QorsLo&XJj$%rxT?`QrxkmdA`i?UE{6--Xu z&FI*h_)TcN_*Gc%c^dtc!q9p?X38s7$-v79;a{!Uj5CcW3~9GibFL?Qi08N5bma^v z*$ml7fWq7YscOX*QK%5hYu*-z7z})}b!D{(eqSf_BS0SCTwa3v)$tLl^ z@EhClty5L*6dn>4@aaVVm-9+I9}uCNkFL-zo2}C1z^Lo6sF2oUUBc> zDJp(PzW4n8i|}2C3-UH?3ue~*Dxr;y;}^~gz^(3X^waQ$wgrs7bspB+_Fshi+LNr> z>}@8weRX~%P{Jxq;m5qE9MsGKm#`In%^7cMvv|Y+CLZIjpX2UE+tOMIMknyWPyzBDItgqk=@k(4Q>H{3&-HUi< zLcZ(fxP9%L2g#%>9cls6?_nz;Nsik9MX@a>25SV}y0U~etRCukehO~=Rgj9#VKJvI z2A_6jhmv1}ws==$Mrs;Pq3?KvuR3hiKt{vc=N+@E58%#@cH&<+3bIVl;#vUT#qD*# zcUWL%peT9>cph2MH9jm_09n~C;pz7LoEV&=8P}+iv=)?Oed8?3r5?F_sh#KxXCXc5 z)f%OOCKWZSD0h)-|4&}B%-hUAU4{@7b0Eda@-sl zT?dM<2!mk1Tx2b$RG%KdA`ECZ5gSxsb4wK5X)^5Ppd2vWiG-c2Zw#3!YkIy51zriL%i_B z_r88BqcbBM&9}5Z5??&=#}mze!%HmK|2ef0hR!8yImpsLVqSqyPir4dAN7)BJqT)3?!^Clgui_aZCR&dp+{EA+zuDn^fKJmtIBLy+HIQU%2jb&AV0AU@)Iz!Z4qNu zUYDORP<-)@zmoifAqVmkB)u_@JTzc?N-$83fC1V`F&NPZ%l-vkMYPGNUvBIZ&` z$1gu|MM#jO6DY=S?eomET2Y)Q>Q(D3=1$O;mFphF^L>weouc9ayuIt;*4>Zi+a8ji znAG-<9RWK~f&47km3d~FtF5E0oxsbLCF|{X=iFnSf!UcIdlGibJSq7z;LGO8%~ZN0 zl;@7Ubb-d-q&DM@sX67QrEL>;uov1O5AQEYh5b*+QMpf^0{F}q#^d|t3-QpNnS}NA zxeIIs?qv!RawbiQv<^qy(>+Pn)2R7ewAd%MA-!6RFN%{|EH*a|{{G})Vq@_f?};tc z;d#3!(P~FHxzu7Yz7`u(6pa?+d@ZJsT5N!?#bS%79`hih5zmLA&8l%CkK4ikqaW>- ziFSjLWm3DfITZZ#Mv}>(e(9IXpnmC>&G5@**tU4(G7h4DQeVAUKEp4YLH#rWJme&w zLH(ZkxxdbaJD!&M#eP#f`bC54-fR70w&QX13sv*vYyD!puU~x7;qMpDU8KvpO&D~o z4ZgF__4SJVrf(rt^Jmo7J3U!E^@YkF`8uwGdc&JNigk1FT;0Qrn$$Mx!vIO9=3dyW z=($&tsX_bq$dUpt{|8D{DcOL|DE<}Ubpw&xS@Q$#4v9n+p~13klEnt#~SkoqQP zn{Yz1n=G|Vf@1a0|`NGP>$*ib_Q@7s!kfF^bwF!FTJ#BP|9`6VB9 zG@x0oF@uUaZCbIX$D3};FZ%q0I^~=|31%F23X}*b^?=>hCPuHI5SsIKh4{n zA&A2W{4j1El?pos3zdqiWGX1tunb^LSFX$WlN0S%Idf2aBG)OOX{~>k-wNu|G%3GX zptlWb@RocNIHV@E4cJMg|Fc?v5m$4-m#MEOzUq<-aekaHUDS}4ROKA_trhPnvQC?PE9NNWdK)p7a!2fY2x`g zjJ%9;mxa>SwY+7ccTgG71Jn3}&Fa8eyt-MY@%k|GOQd=*wqAr}`k`idAXhnypOX;@ zN&xwYMGyY7Y3zbmSX$U1v7dJ zx9-;crA2Bf&sEl!`*11uCwWn_U)C^7YS+jnojSF)o`23srS@FS-x=?>4@j-u%3GXS z=#R7m50G+}jk2IuqPO0AL57|u!yF;6EYDne^{E|9; zmqwrtvxZ@F=4X85NlmjHbX{-H>gyYY{p|Pg={$OaZw#U@_{JcGIbY9Ip311mJn9>R zX~-Ai_T>xp>&w@_o!*k6wNSDne2CLqqJB*}l;}%dxIm+G$A^Ebp`2m#(%hhv(3Yif zv?}9Zb6{ZQc^a!UC)|3hUX)V#y|H@IM@ymk+OQ`mCnz`48Z^ZLE)a*fox%Lqwy{&8 zS9CI1Hh~?^4rTRFZ!?%3 z$VRh~>;SeO8^(@eL)j3v4;#p8SQV>aIhF;WSvS+kv@-&8g=uBn%&*MPOfz$d83o_H z|DDYF-`HJuKa0zP9~d;V6sE!VJSjcDlYUS5?5>BQw_~t%F#ln`kzBv}jh}xpUoih< z4l$o$IUL0vVD>XL%>OWZaeSy|b}}C_+n6oP2h2aAGPswFW!Y=)_VT{)hmFpt3)VTQnHVH%PC zamp*qc9``rPs2=uF~Y>ds9`Q5Z>2Cc7$b}UCI&_eBcd#qVd`NH!&Jd6gLxVz1I7Ro z4x@%~p}rbn4#Vst16mXfY%Yh1V}UXmhLtw-Q}0~Sd%zPtE(Bb7GFJd_i26wPm7(@8 zS_5Vraz6?f=fE@33eLUsZf1z$im_iDIF-%|dqtwI5ibHgd+jX|G3_5i_;;#Gz-fp* zALJk8EVml;jkls!$ME2pessi0rp0CZ;bMQ+NamND{?B04d;f2Cxz+cXn?ig2WWe?m z!&MTs2u3pllwxHWS2@_CtkhSh0fU@LUygN~))Oy66}iuki7b1#kQw=t-7pf839k2S zj9$GapzM9zq!6V?lXN%eMl<=ydUf)V+JNR)0u;?J$_O!C*=&+UmL0BDHfJm0J77>@ zeeFiKL4kLfCQyb%;#EMtdbf-C%VwdkukT`TDrxUjRm!m20)5P-?;`9f#&yod z=$7#>@$d2vja+EY9QmZ3(bMUBvc1Nw4LQPpjeE6telzGMTD7X|oUjN8wd&ejw3{Y` z>N@PqvpHPz5sozFp2PWCiQP>Ie@=MFL!6U@jCoGb_mIyTcSs1+`khTxNjvX<;Aw1q z8wX>o#vN9?na`mA5nPgxB7a2zCc!h&<)xVH67)G|+-*=~Q zYUoMcP#jrl_>;Pf!95hx)S!LDUpuYeh)}@Pq<@ zCob22)~mJ7NiCe}3Q*8g%Ov6)#?e_Tgbyl< zYx-%SYE@vFcKs|K_>C@Scd^tj|4~e7#A2pc$H(OdrN~CFW`Yc$*sVAxyxkeSIcCdi zxVtte-@=!TpSG@=XEW8i-IZx*Ks(D1(e5B_Kk|lc%p4- zL9ywO`*zOOa@l5!8XUi=ncH^$?whuC6fr25QVxO1SIEcDs-FQ)cTpBcxS zB>TsF=>|xtOoz|29+gxhY%%J5*fHxk{##2Q%n3U^Ri?TY-7>~?rbJaf4l}{Xb|wzA zrgWN(&kIGk7kI?pM;d7xZPmPa${8`d4wx^{P|i=CoD5KCI*%Wa^6eA;?2;CYA9GXCO`p2HU!* z%NIh->`+^jqJwD(UEOgZ@-6S2xU}t=5BHz<-n%kPoyX((A&tfe!?-5FU}M`zX42VC zk2L!Mu}=RVr@Rt98M4Ag*Y~#1TEjSK+r;~6Tj(lpp7!RP6F%{r5i_8>!sse5*>x?i z+Y z+3nN9x6adiDUwZa5WQ=P&y(!X(zbtW&-&BQk&~{zyPe%MLmcLsz{u3?0(*nuZee$` zpZ~YMe=q%yu2d{#T4RcC4`oOGAli$Jt@;wG)5$<7W&--V*pL7>8<5xZu#&TrzL0ak zyJbhJ+z{t+P_I!`s++%5Z*tFED9x1;Kj%5&l`f!Hr$_g>H#=HUW+Hz~OOtKm6bdp5f ztze~jV^*hk?99ZI#?CZ6wHQ0Oc+wa$cjlf`Rzz)MS2Ja^_{c5c+0ok=*T8PAOQlrs zliJ2_SB^BgCKs_|p;37aYC+loDa%sMdB;Q}#>Ix`Jf2@ME?l+8aks(%vIa zB{lNOg*As;!{qnZtd{1tbA+D?y$**zq*$WLA8@`d+0(x6oX`Nb7RRf-Y+RD>4?lD| zC$&Ah9qYVmfGb2Mt)1-4*fSmEXx2O@6m(JFn-n^!ZT|MbqsE;c;L>u?cv7@L&y1?C z4JZpgLw$Y8)PS-9=;?V)kEl31^lu_$LMJl{k|N5M(*czfM;SC}$l1@X?WF_G3{CHl2~cRAbdY`^XN&hLcy$$CHVaBN==$Za2hX5x$NQvtrS z@O1ek*$zmO%6J$zQ`%-4`#ozp{LOk;>l%Ji4WS-hL7G$W|}(JGvv=G7d$* zG8cS+6V`7;V=rEfviSnjmF{DIwGx<)Ir*(rqUgl09lzh>ckcizywmrop7}HET6!Yd zCbLel`M_F*KQW~(dZ+YqdQNP=Wioc?7RjKi9(HA%!1;Mr8J^~zxHWzFyE1AWR^Aqz z|Kdm=KLSs&)Dr*k=CgD6JTpA}Us>Y)Le|_%2j&vZ7#@DXJ5RO{O*sg;kmeAz%>M{2 z1Afyc>0RQ_aDkVCezBh@ZHGC*=-DJ?0$toJn*RIlxqi~EXRVwC!>UgXw zvXw^37ndSiF8~6u{6DwLP}VuY29T&*AbRD~b{`wGMZm35K1cfgyXxb!^0p##=Ju>!src75>XBFyej{(7s5&SfE-Tu!3{vp|GpOaJBr1R-emfsY-swy8am zkZtyQ$Nj*;_YpPp|K=h_n80_?O#X}O!(o~mRQHRnZosys|4;kxclg_6J425o=O=59 zfajUg5fw!8Dlm+ifyeQkz|*-0;`1>(`g328$q&^?Q3s+QGPJ4e$$W=6m$O_E`?IF>f0L>hI&uFy9_&>GZ&S+4p6NBY)n$#Nwg zc#P7gk)Lx}Tck$Ta$7gI!22aY<6{l!CV$clQ#haSE5_DI!oOX@c*YKR>pX)s9YOC0d0V%%2{v9hbEk{x zn$FMRVKuilxYvi#G%!vYw8IL*g()fd_T&`V$@+$)(NTw#$$EPmFYQvNtg`#7+61{j z3{oYdslxBz;J{U8##_AqU<4D`rCzvC8`1elw2%Gc+_;VyyZ>O|I(CMq6yJ>vx58t@ z-`a0=N_}zq#;m}Tw@<%o`s-2qw3p656n(25n3j2}hr!0yC5PmP3{xkOHRaDA)t>7_ zE8TvM&f)#}(dhaH;*btIh>tFC#!5$Fp)t-ReMuIdKUXzKofo8cC;QnIfcYhdI%QWo zhgx1>?oN0fYjH|{V?z-yne)8kZrG=w(V?#x5jlzTr`cUs9 z-Q8~l^05WjIpAC~e8P0EDc&jdNsMB@`{sd|{w;Iv*wYlXE8-kpnvX$LGk=W73k{VuFJ$ga$P(-dJ!iOO}dK5E`CYPCK>&p@7Fg?N<1 zy;t8fHARLb|GCq5sVpxJK9X1|6IG;F=s$D3k1?TyeS@8R;nS74+)g^1Kwty(?l8_3 zczAu@OMMET8Cd1q7ydKT-KJk?Ih^dY#(rIXw?5$0Rc^(@@WB%Nb<}I73|(FB2KzTA zhyE>nba*MeCk1}Gs$iE%qI*mGu)V{iMafSE zx9XGg0SyynGU?m2PwW5lOagFZ1ZK^Q;ddMn;d!PHJB(^y}eR_xo`6r2d3lPYy^pUh{kRyDx(zXKl}bI-ztL!NAs4g_9N zH{`F0wA-a-1h?FXOWCD5AqG&9bE zL8&R6kH#7;>wFd7hv3aJysd$0RwsdjeoN#0MWZwwSi(=x&uU<6V`7n zgu$UHYq+v*2Cq7F8@T5y2pMRfhm|LWjl1zmD3=uq}nj;r8m}TkQ*Z ztoKyYX<-YY>taH}aN?sc7a-aKA^ww%3mPs#uf&3nWc?p%NCv+i-I#*E7cHBknfNQ~ zwlBGRYIwru9g7>33ns5)Qw#Ct-vPKoNc%&+?$XvN7tq)Kv-fF>G|9S!(0Q zZK1N?X-|$~7s!)W@UqUe=nA%vWe3jtv@*{1IJ@O#t+X#pDr@$&YA##By)lYK(1$R# zJ^Xvn*nG)n@*!Q^Y=b?6m)(xDkD{mWwd-KW3`wpq9DPYB_cJcNS)b#KyMSV7Hflo}Sj?>Hg1mNA5?AKUgGf zr(to=IP)5%_s%UmafQBA<|mu+JXV{<9{khQ zh9l_PhC=AU<{hZU-?;~<1>+pKI*isp#<_dw4}F~PL9ONTMT@MJ#eUd_uZs||X;^~- z=L;(Vp}ekZRNjkRs9x-6HvUAz@#qK#vvd8r4dFDV%yxBBCNFCTwaHQ^58AJ+JA#un z7ozrSA~6p9KI&%UHXM@J&Y2__36EN&`1_aza-H){i_WQ(h1qcS2QRaQ>g6_QgmCwa zsQ3>Ynk4Acqhff#=meEXm}@VY>brBzq!l=KkDUQ~Syn#4_D$0I+fQgUSN5PTznm(b zUqJqMq%&As46o85HF$%7>pBPfY-{(NNE!5OG+yq56{D;8OWX@?aI!iQEn+OQQPcCvw z5n+=UaR{vf5X}(=v$0QB37k1xb!+1a#CoIM+FoaVcwfAWEtxO*JzjDwC_pufxN|7t&RjRDZ8%XY5k{0))%FtqL@{fzE6GK6RcQPF9eor)Z9AXZ$hBb!$4xAQ>)#W4FDfZa7VAAKf!Hz2{+oD^TzqpoPU5qb`bMN3MZY6eiE7gj=FvX{i+DUh#m3>h^ zI-N398+bA{E+($K`^NQ*0PMoQXOg71kqw0nwWJw)kgXDX053~tt?VxBaP5-?cJX@1 z$-uUmZTv5IiS2@0w_}Ql&OcKfiEvAC8e>j7m;B?7|IY32)Z~VqmM2a+FXi9qQ?O4m zz9!)QfMSz;AfSIv&Wr)}yJ$Unr>YgFur_NKbWkpe7$*61!N<%Uc1qpqH2V(7jrcknc8D*`OZ$JggBu&;B=7GjUL-G}oLuqm6WNDPK2N8j^O zQxprEeOYvl3@6k#Yyaacqk_diCj7O{y9`{rhfc1<`(><_MDWi$5|go)&65jh`#-X? zyQDVoAkpvjaRT?vfe#MQP&=G*uqq$!V^9#WRA7L)Q}m?i`bGPBhm7iuEPPBzDK9hm z4#@iwbEXeSzm3ro&C=!epZOW+Da}!V@A3XwT)pTi&{yv((J!_yz~|PKChQNJads~J z1u%WTvAhZ&mPag%bJM^Z-(K4id-|<{uN3m$Q_}OMUC>9o)mUQs!bJ2?rCVcqnRo`t zU-fGKY5f}TX2t;pcIJuukCj*r|2gt08c@cccbNP!!Yr}`=JKM(SN!Iwvzh-WkYikY zZ2G0vw>_|nNwQqtYPm>n_7WlgHBli+S322wta?{orkVJ#x?INj0LrJ*DU`W#S0j`3 zjytUD9k;Tp?DHewwk*z+#&h7e-cuCxycoCy{=|qQfTi21&>Z-WJoUai(2@_UDuw1_ zcc5mgl8t*tvrD_WLj@>^_bX*oT6j!h2JEP_;(fl6KM(%{yK?32bH88`Ethi}&zW`s zj@gZC;B!nWSrHYIc)2s0|J($=>%YLM`oS-iVv6oPx2##sqYUm{l=>6HLc{OjzwP(% zVEKEu%wjHt50}uuRO(=;f3z3CO~Pg7y9iv)4I!V8=}!I3#3 z$?Nr}+_J7yXgTJxe`9q1Tc$|U4xC68ft_gdwBAI|)ECccf7Rtvn0l3o z`)xP+MDVy6uUn6C`Zn6~e$-Dn(wboj=Ci9XE?X@L0hJh+hk?h47bPB)}FePvyGWKQujL z`cnT({WJQH_5Vq*$k7tgc-;oTLVUA#RFJelz5KrW$KCm+!`;LmMmVJ*fqja=E&Xrr zpV=kDtS>dc2pe^YI;+2ae@1sSAE{emI!7a~n7pB_K%TvjDX?1TUeLW3by?co<~L7( zbFbqqZEk*6f6#$lru71h8yw(KEv>l^w<}T3E65!l<<$M{51CXwGU-G5Qf8Mn>$?y3 zGq6Yf!+w=^F%Qkd{?YkS*?kbaeR#)L0h>_LZQTX^mK6it%JEA2>0hpELM9M|1;fa8Jziw^W3gt%qqX`J}k%_p9gFba!aA| zR{Rt)M_b^-!k0Klf%976Be9(0A=7q2P9&U2_CQ|Tc(eC+%d4@!FOXk(XeDfOw;zfy zY29&3CN`YM{$YLf+cB}S7@U+e^i`<@t3~9`srqfK;0?&WtC^C(w=fuw;De!0uA!TO9(u2QiHXj-Re)y2DkqljgglKMXy&GaY+qO~GXr}-mF(?L9QF&8z-tjKGW%K& zv%R4?Os_QiciQEkUIjg&Ilbo>9Qu{%Ij#J2S?^;K_D4mA=?20Z&bJNl&H%5N;JDhb z#ujUq^#!uxTI84TKhRT-HA~w{cpu^PT6c^l6Fk=V_MrWMs-@F-Mf$cu>2mTZQVD;x zaB+(^Y33EX=i;ntCT1=3i(HZOryem5vnpC&&oxL5F`cqLKfh(XP6w*IfaTQ-)bK4h z41WSIYrk#xgKerJb5U+!=O26J@)^SZ#nCvMhu=aR+TZOzr@-l_=y5PHe*%2-fA*Jo z-aeBCM#+vGXJks1(z&HTb`r}y=}KocJ+U%8Y4BuWB=;}q` z@U|z#?lQSVZa*vMK5fI#rjY8%8f@obzvEGX%GglnaER<=l)oY^LXNwiENv(5HiabZ ztX%f#S_RuqIl|5ir>!ApJg-F#!mci1Tq)6?#L=I-&c^ECshMnbys*=q=y9<57W1PA z_iF?B9k8ry!8X3Ruqb}fE#J$*Z z2Rr=w!u%>h=voGTBL|UfH z7g+l)NWF7R`wssK-Xo>4DS>f;!Gk`!qHtMnXm;CSZ4aw`E4QF$QZ~4n{WQ_W@m2XIok;vh~dw~#g~pmQy_p?!U_fT%-0U3gv?di)s9 zLBHD0ww?kD39c>M_GQ`%orM%&*mfA+{<j+qaz0Tl)$!ApZbzlCE4mO73eLbYP{6Gx(_{c^N`|!s~rTX6iHZ|6T0NbJv zb3xDgi0!5V-A|%-YlHbP=%VsWke8X=T5;CUzWNV`NQy@vvwoe7>KA02k)EFmJJnwJ z?C5P`)6Bk*rKP=FgTtKDB(e5)`#3+sgwdv__-8ardSo)PX9gFW|BKwX0l$IE9Ur@8 zdq_rS>~BYRxR@UosRbixhWV^p$zco!*Yl^`fqT#vvf$pwgHdN#cR)`9NbkgG zYCg=suAOjVROCColeLmwmDY9`{+4r&=F*5O=!sqOD(~pnpF5quQK-1wO9TUj( zsMgDRdVv z!QPd2;$FGNs(9cx3LQJ2L-GSOFZLDIGRcxohKm4BSgVb?B{UiE;~XmneVdltdGfu! zbg6jOCkx-g)r;U$B33&f?E?l8bl#&m(%6QNVzh(|wk>FZhl7Q&*)((t5`=$3y8h)p z8GGCLr?jW~0(|G4TXL%|r;BOpjd+PSxUj;G`LjM+a5)ype2}7pgv*U_3dG9vqp37+(r9Rv|>>9e} z2c3>i8Lo%c3^b2@1#1sOtc{Hg<2iMP;UNQ)a?$=WR=yN%QeCJXvHRvSBeC(uu}qs3 zx(olD_cW~aLBk&$-X1{v=QKgzXNF9G{nt46DhL>leU<#7(`fbO$>VqkzxykHW^&{Q zk?=+fdonH}zF`o#_mj(QnuFfHWi*NdMuk@Y*9~o!*!2oWun}<;4$rBN3~$n?9ESVUIP88 z5?&6%WJ+)YP10Rjj^=KlY(e;;ur55M{Pg%P?Z6pv&W3c7jy2G}3LJ^7cdL{eU@!kv zvsL>SyLX1u*YggB>lR>E9!|eg;fz?p9PT8 z0tBGz2|t9*lazG0UNYg?v6prtbfeRf7% zQg}ybl5YoiOYA3Kqi=)%2=ErFs3fUAM;~%FE~!cXp5EuA)R1FLr-QnvjQoNy=UT0iyTkuVU14tNEYkjzPX5@ninW~eUc>{M7R%QB=% zJVx|DN9;6FCm*Vl0q@Q{^ugh19qk4puYfd~Q9A#wryUe;_;QT%Q#Z?9gTDplq%^7e z==8JHV>bCDyR0Fo;Wce+yF`~0EqEBtM++Cfrj2N~VzmU^7;+0lZc|P1(9-YMV)uKW z3|x{-CPSY|lftp{#0g&lpSLUduN?X)ia);npTJr~(An+bty=%njp0nvDtlWbta6uG zwJiR^AIRGwNsB%3ENZ_+SV>^f-bHR@xpJWne~a9KD=LXmX)I& zr8;+5`Bxpk!jlSGZ;3M`SegF2K50mVPO6JJ8_d`7Q>KCDJtWJN&dhbowzxa?1Nu0D zUa533Ite7;l{jU=Ux%9K@6e(h3kWJYH=X|WIcoU-9W$n;CC+;zw!`i|A4^hA8>E^T zbrZZ>1mYwYUpo_z^Woibg{_OoSIDg@;$-K+BLn#kVVtRM*qy_V8m*`@&dJ9poM=8` zjEq@HzPLL3(MfT=b>16_rLZp$Y}Zx#LD!byoM^^A?`=AFVbGqer?lh;!8_)$-*F@_ z`ldcAYxvutpoZ5Ey`~MegD2|R1#d58Wkg;WF{cWcna=iG8i)gBQWRJNQKJv(yFbs& z{7ZWu^zih>n}0R(dkNfNaKrLLN1>?}bMzg_F(H!10Av;9`*phuslRi0)6%{A4JN4# z-sn@#g+WhV&@ZWFcF1Eniy*mtg3N79lUU;-}bI& zzjF|F5G}!0Me}u+{5q{i-3EDY{m^kh72X`}EBg$T3ZA`L=erj5flPimk2MSHt=0%@ zfHmG4XH`8PX1&GgzdN~s)}&sw#3xJIU+#)emfA^^O6}}#>y4~E6xSBNlTGt)jnK)H zwteNt#P5h}sX4Eh6VMi^3uqBjfB870isa4F6%9|jtJHy3Iax7fH@lmGmoVyYQEu=-*qCAiVdplkm4g^3Vn0QM9J=s=aiovhP44JPXQg zO%Cy!#5n}78(2$eJj7FrhdMPO0od({dZ&l;*@6bye|MF`-zvAY`fB?AXKOHQlH`7g z(0NJw6cSibJsI2rDLVEU{u8%u5890*u3dUg-WKb$NaQVd_xMRP&Z;JXmv`1}qt%+4 zCR)FFciVid>~u)9J$W?+J!csO@@#~>r%tCdz)%anX_vPtcbvwVL|YzxC^EZ@YeSb4G#+q&&5cmTRG)TEdbV#BCK90}q?zbcs!<0@ht@3Vw3 z&N)q!Y(Z2*K+c3@b`tzlyJwzB?##oE(pRa53am3847ynj(@{gKP(#+)c{k(()n1A7 zGFED~^nbg5Obu;RLmT#lEQ2J$rk%k&66?3^E5;YAk$^{TI}3gM&$q_GKR27MY}>9A zYZa?E=;N%+m19DjHS+Aws84oBGAWsz%9J>)pIm%V1q%_SVhz@SxMo?Ga-lNC(+BCw zGwHRgE8w$X zhH?|23vQt~ik^WNgxE&REYx6Xqr6S6(&GG)r`^@+r`-<}%3CC==x(xPzO;>M!NoRR zlJCrRa;$s@y+3@3pk-nReimyWOm8;pg+Fjd9!vn{wSl#epW z(t=wGdj7IG5_`e1bCSGnN!vazaHa%y!2g!HHxH-9cx%;(7L@NSM+?S3n(b5qh6sMa zrQqF(CIuh31GeM|@a)B&x;*94U;(-m0alF%*6G3<>v@|dyjeVN;UkcSIu#+owlm3RucNaLLU81;hOws?uk zX+fPs;rlh=3u(-vl#Y|N*bU{kV^Xuy8nk0_^S(>LJLRqZR<+9CMD4vGzad`ZlrXYX z^jUN8j%FGisQm+l64e?3K5r}1c-zwd$NloQ$xbD(sDBfUK@ED)rzv=6C}dPw+9Bw8 zD1C5C=D4)2zFFTUzD|9>^tramZEMgMSGERYA1mW`7-?i_tSY!A7HJ~JjGwG;yY|a% z9f*6P$=h$Opcla{XGtgJm-X$R_e)grOTnm*1RlMgcDHg8=m>WK67-*Xl6A6m`aZuM zIJJF;(mDtB`bVKhdIrBe_*w9)#;+N_;5L72s0n!nC~-Pb)j{|v_9F^@_oIZxl&z;O z_iVd|r`;6m;3XFpA&)Ati(hx}J&p}VR6a*mDql9i;v{w#&L$eE3C`7NEevW=TK{xW z#Qvo~6ZV#zJl5wQVsZLz4f@vdkfKkm2ykaV{ZX%6 zN^olq?z(79A^U=`)?0Scc>02TJEDH88slu()srsJgbBF*4&coxw$L`K7IfA2Iab}* zy&h)^2adh%1{@o?O))3bcJEXsWs58CbrHKBg%!p(7$qM3x<}Tfm_s;IbLB4<;OSBP z+DvrH*;y#ySJwFrv>03ZFYgm^>m`SWk_vQYi|hT@yp(2D%fCu#Sm)b98K?IB^OPoI zt76Wh7ej4vyIELRUu$INDtDsQpp%lP#9L$ZanG*TGgpEU3oSHP3QByNO`amqs%KT8 zM9h^ygu$P&&;wt%4qp$2{f)-?7mKnDTK>6gnoTE9iA6q?rUdar8x?aR+Ss{P-L!q) zoYw1A`Yo>KU-OO%_(MiptwUSUh}eFSNnYBHnIzBqjK+alg;B6@bQBD`cAoI2%oI{) zc~Y($8E0zYd%EZQu*)@)LM4Pu9|;*eaRb$BHUhfEBhdK^DT4M9Y?*xK#ZTOG_9&kp zcHMd61b=HQ&ZQ5{>G7*A=!uwwbG#%^0!KHk3jha3^x~WLwczbwCW@Jd^Oa9b+09_& zFk3_0CIbikRPfv}1v?k8fxK7ZAK2;7$JzE_HCLql+rElpi1shW7@llnQeHW^Hi}7} z!fUR)V-ZJZShMEJDfrm%aw&=~@;gU(!aGV2Hr{NMioA-jzY;C~!d~wvmC`9wM9W?A z2Z-0Z;9mS}eCJxq-=DZ3->z5SW0pcZ9L8K_I*pvTXNi;X@<6P!`Wp3U?OtzeM1d<2X3ZAY*ZaLaS|p^rb1$`a-0n=itNR36FzyuDIQt7Ur0uFYjo#44 zDcI){`=x8gRkC`f_U=V{4@Pz(KD9cvdDENfWTh>u|BeIff=FIzeKS(63Tgp|oH^{O ze=~R|VP^ORNhMk`?3#}+ENG4RPGQ5Y*>8Gb^ATWEptT4(CHioR&0p8d6L#UjwcF)6 zpN5j}6`wPayV18(Aejbs#XdK=d>_>JLJuvQ(a2XM4}kOt{0Dk z+wdbF6Jge~SYy7$3LZ}yjGq4e3DDX$lEOlC&9Yz2)r1D z+5D`(J&6@em9&pplA zgZwJnp@6+$+rWktWwXS-C`IFZ4ZE?=fjfV$QE8K6zpr2G3g|_NGoY~suCFl$UQ~R~ zs$!tDtsEtQYHhI(#BBRWXxymc5b9u!t_1S zNPB&QT0&sar*VE@@v}~{3H+;dQp>Pw?J;jleFw?*k^ZaDv(QW#c71+g7w~W^ej_-! zRTDGh${2#hDDweU4~TbptWg77i7++J=RE1nsa?qFQ{*&dH|6zRcY)J4;2sC@aMxWm z*NBG;YL=q)BS+}kZ-q2(9uRvnbQhb>!uyVLr&l(fq+g}aay|Q+1kyQRJ0|6X_Ep^2 zzCBmZ^2%nhJ%b>$Zs}ijKnXvSYH*krEmLf*x_plYgmpeFl)A-J=hrT>>4}gZy(4Y4 z{=j)f+Od(?Mj^I|>IGZGXAYV!74g2w2`7D3K~~~PJ8=q!pLJV4)em(A4qtpq36HX` zjv(z2iCjPGOu}ZGxa9%!*ljrch#PhEMyme%39w%xmKGJn;h$x{IvVw z!l&Io;U4S^>e~)akL#9f9hA=c{I+*{_Xh{}{!H>BJR8*V>NLM((3O;L&ig|TQ#{ag z9bTEfZpNNWnet;&vbvMx z^4q{mos)-sI=M|8$8Fs)wx4v5&J;V^bW4Befe*k3PO-M?)*)BSzmc4_cB|9Xu}{0V zK-ztTP!MhXq&!EYXsR=XO z+YsFRyYDVz@9A*Hb=UvC4J{4$dy2WNThSH>t7_6MUw8fZcW51HHFVggHcJ#6%F4<} z59__KK35<$hrnLu1%;MnxTgIC-q7{=<0-F-zm)=Hes zLOc0GKFa?$w79VMJOMkK!f6BBuN3q|pgMD^gf0v>bjZa@s)C;NHWrx{rEq z(aCyabuF-qkZHfjh4oExK~J0x)(qP7@Q}8~Emdzn&hAo8Ys(EhjdkSA{NpcyDoMKF z@pwVx@wQxf-|+43Gjf9r9_p8%7iXPgRI9Nft3W$L%b%QVYCf!poE*6G@Vl__(S}QV zV*vs7>S+(

  • ->RM|bJsaF2Ik4qms9yzTgH-J|6W@?Ws7ECN>2CUwNRmYz*oac`7 zabW*hz7yvGlZX5G-Pwn<<*dr8*{)xrA95WWVpOV9Ivt2f-pT(6&?W0%&X45%dZbdT z_BQOqlWZWoN`5iEaQzlL@R)>Tdt4if-kOdvh;th)ZHvS8?7Rp;@2R~UQXAJ6s)sKt zg#Cx*nZ*>#X@5`9H2>DZpmnArT#PM6KCw`nQ(k>UP5;z_VK)S>khfa?Z*qzX8crNn7lI)R~7}R9!9$ z&ckIF{<6>g0PuQjeSVi~{z zdG%u#z6y2zm-HR!LCJl-+4;I9A6y9q^#~&$K>P~Fe`3#Ij&sWj;7f~|fBEw8-0XhM zBxc#*Uqr!LYRb z*plSk>HDNg2x-C2b;$k;Ch$q61+Nexz1msuU)P?#UX&w_Q3z=YXqlQySPDFpW4?4UU)w2_3=HDOCe|4e+9~}IOw~J ztQxvpVS`iyl0;~wzYG3S7at#eiyuCCC=&agLuoDLsr#oYcFMZ8!=gHO8)(4O$NJ`n zz$cDRB;Rib4#3$1q1EK2aLCm=h#l{?q-xBqbno0+(xhme;+ZSh51Yg|w^?G_*d)yr zKWMdzr~27WU?v=Bx)4P^YEE5#d7JNUxy`o?J34f1jBiVuZpigpjAP>=!oPl}F-0O-;;fWoIg%0Xv2v*ODP$ehv6VdVUGsckU6M zo*#UV_p^N-rE&gxD+evskn6E)0oZ}gB+u_;fmt65v|-W&YWf4zHL6X(~)bnv~JyWW_)cv zSY`FKZq@lBrcdWR9(d!2ns{muv}3F`%U@W= z%eut&m^Ro1&jfVd$l+^hHAYUr2e3{)p)GLwO8}{~_ng8eZ`;PRNu)Dp)2DztX8iBr z*;t&34V&>HSNFASz6oV!KqA68I|cpBR*W6&f%1->&08^cQsyVo$oUTYP$H|d1t{AG z7@T!=6sm^OkMkrc!(`3a}p(P(6?FLpnN*^MAJ<;$ewnyNzcToC)`{N^DZl zdw**->ObsCzm^KCue*`rzppLfx1vQTwfow;CMm2V)Pcl9zug#t7@xSKPx)GhT<>1< zwMigLP`vU13HZ9`Q%T2TApucKGtPs;`q!z;cWw1)AN^Xm74zWATU-5Xz%2eRXdlN} zJgc!*eRAtPf}VhKTRpHmw@Dbe-d{U8FJf`1_mE3Fa2ITRUVzQAzm;&E z=1RoYX|PHQ!Wt$MbK7h~ssC9vR;F6ZU{4$KxE5zX_OTMzX~L(NuUCU-DVn*`tnYsO zI+K#szZPB=Knr$~4;3a+b7lJ0H}kQ3M-l`ppLeBy^~H+922I}=pcujI(R?3gh|d{# z^X_teFi8IN3!GBL(}xHLU2q?5>5=<K?V)p>|Iiac0IN#W+0bUA(*K-N-s2pg+j&sa2H*f?0%f7-*)Ve0nFlzBotJvDp zxBe_2Ttn~2@Qfu<{X7r;ZccT%9`wx3RyXq6@X245?0jI#F4x6tu#t3~6L#H;nDcSO zP^#2(iw7QcD|Y+xk3t5=-od$DuKy5VI=+>lX1I-cU388H@G9c7U9LBT&?8$rQ65eg z!D?ehqXJs%!aYP8q`iTDh_`PK_6>^Z%aDGBdnMNBx?DJq8?|lTx(qutA{z^Vp_o_$ zTH#xG2rACs%5V2z^{=mnpx?V(PhKOuZy5viOt*T@#lx7j`IMtbnU*8hKiWglI*Nlj z-p4xY5_wfo+2ET1JT{xoB+IRuEB=l0U);|3bW_*`@V=0qUH84Fah9R&nb_?7Jd||k zs@Mbbuc4(aJa{L@x!t0&DR!TO7c6mxd;_2STl#10AG|2ylb;VX@hdM8|Fosw@VeCY z9%wv0{~XUMC$lIkD9eX|kJ+hPeQo4LhQ-b~@c(|6OOpi(@oFP^d?4&NaB;|Gci)9t zHS#mNS=q9{1G@q7$>SfW@4xcvhP01>w~LJ{8JbGYyoY9k&!yT=L1@p8F}#55UeV| zDSQ+=uA7~%dM@ZW`E$f)UA{DxA8;^9w}7&Y;`i^aiuipAYD(>{xw5SBE${)1)2}fQ zyJ8RXgu{Lky_^rdBx{cb@4(uxU%Z6}4amH2sv7&!CcI*Wi!dhfjyyaUe^30|V6;TvMoaU0H)D64wUnyA!^k+E{JJc)07t ztv0_G$n%=-?qOHqiLw2kbxD}{{YUl(LBqerb$-7%3ZntpVd2zMv_6AY-*5po=w3ea zg6pN>5a8qsuKykK0ZwKYgj#6?zsAqIG(Yw*pHaP>{|Pj1*HM;Ig>})q3)t5xLONxj z`uHot9!#2NVH)Cr#koiGtt)HnxS{&M0{hJzeh+r6(^za5e^u$e!FR0`Y8kZ`O*v(kRpD~5G!J4EpU z>^>1W(ULAn!%uOJ@;TT7#W-S}NWhFxNE|<8PI)PgAXH#xD7{E$5(*ZMMK}Jrf=b{c&!Mnrx%7Co1^4+`Myl z@gJFIIb9bORE{Wcl%ju1wNt(+8mSj^@(uX(+;eOMH2xTrWXjIJg zhZW}6U2!;}o0%EQAN)KJb4~C8FX%F6^J&<9N<6NnBVE|x#_MVY-y^m9#ptUquaWgR zZ0mo1R-XOoMA*`GR_%`f8DLQYrjWF0aY^s!p8!N~v;&BomP8RErI{d7GMD8dM3O|) zRO}Qto9z4w{XUabPqjVnz-fQ1DhIm?8DX!TZn3_|q(~)CC`Eu~(A6|_)G;3(!LGTm zQy4w_x}Dm85A0x6PFAAT?LaI4hlQYEE?WVC5thVB{3KI^eue2CJx)*3CnEKrt9Yo{ zF-fdf0nYC{{vld+Hvb`Z%k0o^bYz=g&&rUGl2l!zcFpIsgMDJn*S;fgngYKVKf#AZLyfAK8eox z5Tp7UU*?g<+uiaz? zz7dveL#{t#f1*s?)t$jdAcuL7DCp}M;p=DJl-Gx_&g@06hpEHuN{qsA>0U?P;!6q9TOrYc$4NWuPQZ8Pvq8aGS`{7 zk)+&0&Ksk;rmC*0q^dk4!&q5gVXQIKnX6Ey*0{l3S-fDhh%rf)8S6|{rCM`kDZml) zDXv6085s{5Dyu3tS5(#4{v;zKcQn7nhRV`%V@=XbZex|Xl=Fc1&n3#pNJ>m7sj954 z8-aPHvBYTJXe_1r6xG$#m(0K2UG0tt-w{-@)B6edb7}|1eo? zHGnWuy6K5`-0Q)~V{h4=Eco&puU|gHDjs-_RTy@#iqzRR#%X_+Rs6DrLK>Eh-N$*WFv3zeA{~wK?YD0~oA|qpwv98Ei_sMha;r=C=~V)KDYj>A?l#(ypp8R08Olx}rKmT`dN|Y*ST*adx#~qp^JU zgZ1kT4YO;j%IY>5YK*gM4b@c}Jb%l~H5KB=nv&ULrtjG2StZrg%m#EL&Fh53S>{?4 zQd4IvO-N*za`SpI+3e9Lb7e_+eW}rt$Lu;UY~oXyv1XPDIWo-Z>^pO^muGORYt5A# zIMlDsT*6TyT%L}LFZHLfJh2^0H&+@e%q0RW#pcpk+2!R`B?6ur&F?OQxsGxytE%BV z@kg=+mh{A*HER}1@WP-I^Bsv7U&?u+8cVtLn+0%2YdHEep{@k|>{<~=7t`HaS+w7( zhE?VYV^w`!LL#?tA%`zIW0|48d~CoC^%ce@jV8>ao3)1+p4(t7sjo8@RF|N9ZyvyH z=IRP#W!)@eZON>ahD|wD6%|0f-xFTA?0dt3-OXYn&su8UV5+P2#vJLN(Xz(G)f$b} z-k1~nvCLdEv+B9ZDOrU3(BgBm>DL2&C zg6L8qo+*O#V$zHZf-g_!y^ql1dUIvnyy7}cAkXEE|9^y^srfyGeSG^*z||s&{+EgF z_ZKq`D}N8|{rjoM;^=!v+WY^g{X8UN+`OC6blx7l35PU}l>NJ8ZafU*+V$^IyBm{@ zZO|KdnhJ1+HO1ANDvH-*Rx?aZbwx>Cd2zjW(uxno;LYl4s>+K?>T7C%d5X);6=nf% ziRo+VD3LH-y#wEKH`hE+TwPJU!l)J8_upV8p`WHNM-}1StiP{xR|JO;mW*a*+uyxaZ%IvAE1np zZl4ACf3W<7VL&+oc3~#f&Z;mpVDOCgF=%&j1*8tt6r{Y^P`c4jSz_ew;Qnp`WHLSh zgI9Dyd7Xi~VUZD_7;LS}Jfd zM$i?Gi%&_a<>JdhlBjN+IDVP3a>EAV(JGDg;El>52LZkf7-N;y^>rg}pi9ELXDT5+ zW*@iFP|nb}Jh(nukfR16t!d7ZFY#;*pV2PI?)-t8WS_7me zfr}!z%0Y)Y0=S-7GBB?ACYA&6s>(RxbvXwNfYIUk2GNldXCVObwrLqE;R&CZ=Vs7WV@-)sXdtof zYjUWxda4WuF2exl*Z|j?E5~u-dzrZoa}}=~6Q45I%%P5itm=%jgrQmI!AhQQ50;F@ z5VggI5-?mfNKXSsOg1qJIt?hUd^F$hiGXpQ$}j|Jal=~ky`Hg)$Vg6tKt(l|=`8~t zuo38GOi&V+D$M@`4-~?96 zGxc=5`QDk_dSiLjCeB=oCaf@GM6Iviz?mV}&@lSpaDt(a{R#Ka%I+}rIWfov_A?# z=xWS$o4FDYk`2aE;##?slsj;%z&+?j8iO;1ZbT1x;)sZ; zsM=gvU!B1f-nEPn34|IkPl#Io25uKP0jij}wv3oyV}h}!rm7~9a9*`AbZ4mrTS5$@ z;%butdJ~A9z*aE}ITOSQ@f$aNIyV}j)(B3bbbJgNZX*|_^F|{LdqXv3?gq>%n%pD7 zVtayYlz0euafwGi=7}P-x4_RpBr;b6R*`Y3Gl9OAGK{fu15ok^A0y2F+B)KV#$WAE zu!=)C@Ae%I`kW|M5rg}RY4|o3Is*K>vFDg1c;1uEFcz%QF3$0O_uk`#@GgxQ{zt8s zC-H^{3gO;2%47T$wgLI;?_0!ZaNfbgEsG|mmr*!#tBzs*h_V;!CWh}3!Z|H4EMR;t zXJYvKLiwBqrJ`(#GyXa%!k6X$KdQ`|h3^r^a{Sr$_ z_`Mfajl7SZ`#~Xm0QLJq$5{MwC&KTIzT+uY@x?ai;LbqL_B4JG=&$pvqD!d%9*r1p zPxi$02S?ja%M|9^(7qO-etR^ZeEji3v7fyqig%|ce=+=z+VP&q+eFBZ9d3KzaX0HFGgyhDRqf|*J0!bAn1MdcHm^bJ^o zTD5lUHv*y9!^lt^;e8V-RB9Bki14l4fU?CJ(mRMR${`L>%-8!)C3(ZabP2_bdq)C# zHy78M>LC`=g$0#qn8%j!y#GVN8g|^-ZeJmjGe%pv1AR;pQE;I*Y z7shwB2QPee!~8iVgg*z|gRy^;`9t~9wE}wc2QIOSY~WS#i}?4HFuxzf{9cUTb2s2G zZ(K^h<2<8i{EQiw9_QUDJh%l>Z^S=tA^$yqpK!py4fyYGBjKkIzW9dlYGMBCss0GR z5r3P6aD)n%C2JynSto=G_0s}}-+;dgg>Zt8aDVj;_(O;APd;9Hhlqa(hMQkspk3bq zy*dQEH#Ww3svr6Vb=NV4@7;jko)Y@cqCxfWV{Yi*hlTK?fQ#DpCiNG>3I0<^e{)Bg_{cbGZ8<)lp zJ%^(`Z~JZgJn=iFx%t%&dJ^*}#@xHCqWwLLUEJAs@c!xe_Qm{yL@Vk)z-y4+9T*mR zYD2mC!3n$zl{8l0fXR=P@7S|p|-m4*6Lb9)zU0YX? zo;G`vp{~SKTD1XK4zIM%kTGv=M)9W74T3ZSRXOlH>B`mre!f4-NJv8IO$@JN4!Ynk~wul8oh~+sq~ZxiKJ-d_ednEf+F|t*InrK4YS3YP}lUdA7vB^ zNSPMBYDsRKtNw>=c(Z&UynrfHJt)$yc<3DJXT{fxe`m$3=QVJ%iP?{@{b6M`FaeO+ zZdYdrWSrzip|6(FSQn)XB2|!G$`)|BWo3{ru~G}k1e^c~Gz++FU5-a7g7REHj5}}LP1Xcs|o^nM;BtW1_tipuwejF{EfcAa`%T`)n0+}DJ zUqMxlTgyT({kB{YUN-`FvArSt-) z0;#-y0kVfo4j3{>!6PWes8}ixqPn*Rqn||0$p}zX;J+d3d3BE>IurXpb?)t>1Aqk6 z5jl`LvJ~Q!Fq;!e!b=$Ui9>A6oN?dA^^_O0Vwn6qhf)ZYX17VpF+?9|jurya{;m2djg?Ly#tJ z{v24(K-4tASO?lhbpwy=%k?xzJFVg92$njxsOhnB1&&IZWVtHl+_ zg(sDOLp||CJ(ox#Gq`1j20@QKk;csAw7JW8ECN@SLbC~pud0IR5oP4fr_h6QGYIL< zB&5556J#e(L{GfDO6bvG#WYTJk0_*`W_02@mLS&&@kxnVSycy^Dl0{0D_JNQDxhg1 zYlnZO50Mqx1PW^KPU)%Yj9QbiygUawtU4q3Kahol>9iMKiggq)WdDp7+Prl2T; zTVxBt&j1!vhur8++7w!F#Eh${BY_;>D$ouhzh@TuS6uC-*y82ozrF9ZrRa_t zAv@udz^_y1)fp!{Lk78Jb^QEfB7k+cnbGJ-!Y13a8JF4(~NLmF~}%h!u^W@ zM)4Z%2HX$Q`&CA96!%HEpTfP*jeDF_@eb0zkM~0?tFYtFA|Ef@-aG1(E2wvtSJj!z zJk}ths3AmI{20S&8b)<=6XlZ<(<)P`I4<)j0m4g6cNy+C)>jva@3|Z6v^7=W-LR0w z(+ZgoPsm)1KTh5$G3BjT3F1zO0dfN6Hz9mkUBejpsKkW)i_9A;4dveQC&UMQ(4>2Z zt+!n7U4Xx+9BbbKxJ*K2eR(-13i_|)ey$33KHyU~d1DLpGggoxk_SLB)o6He?fRPg zi=hbeTqP#JTd@8}6WyVbq8Ol+vu*Oi!Z8DilO#T$J!~h9| z6YSN@fOcec68HnO2MZ>3#7Wk{MxmC9AYEg9rFSmO^3)4bR**kK3uM$T`No<`V|fNA zUP2I)J}Y(B9E9ayeFVy0G!qZqBUvv(mXVb?RL8+_d>zLXEN9h}MPWZuZJ$ zvyBz&jisf=(v&&H^HbJCFeotrxQ*soTHH)ayM6xcw*!A>*FXj*+XK<$i9lkIdgJXW z%TO*b;GKpVGf)p`l@Nw+b;gF2W$A#c5YHT0Aqs$r;n||BQOeyfREPtv7W%!=Bhino zL(!^b>1lkbx`6|IAVO!}NK08u&6P$*TW>BGIdV!7UrYZ8Hr}|NOPxcNp2u)09FzK^ zjP!Zn1JQztY9bm0k3d6l5Ax3; zJx4*&?3JsyA|k9*6Po*XaCHB%4(TB1VTEb)=oAp+7%{ae4pI?0p|L==d60|g3zqRf zu;@EvdHkSN%M#NC`IlF1AcTiYpeP{7-YNgD@DC(g3CxRN+oTx`Z2QgAA>2rG*l zIIv03dDm{Pr3t{qm)DEH5D=bAz#rn`Spq#LO+Xmmk6(a6YwDI28A}rCD{FzEAxx9( z?akP8|3~^TqSeJEHC45>o+U=mR!HXJ&_f;-g8q%bubH?MD3mY}zmi3b(W|l``4!DD zqD=7Ee2fKgZ2rJ;#hA0;zC_$e@dWs)Dvcs^-pST}Ad%h>QGds!EKtuy7!1PTAqxGH5_YKLq&~RMTeu5&5AU~o z-hbq|;|zEqoX7{oCv;8sG|!!~V{R0uJbDze6anraDgRs^ntZA8`VIw8bs2$iwu$P!=6GBJBX|;0RNLrX z78@HEZ9E~ap$y~I8`Udo7FKP7Y>P}cfVZHzL5AteL>+38u^dTYE5T>yEnU4bmjj0* zPLz0a2H8t^Y?vUul5lBQ4^9+(5O_ABX~)U5e?Kp>#2BBMX!kQAvx#tx2Zl_mg^YxP zC&s;L9lZKU0sb;Wji^b5oC)q^yiA@$*xgh6k#xyjZw!{%C&^EltEq!2$u$d zOVEi^7a==Q_8-&7B#YMG(Dz>5f+(v3-5E-e8#sg-1I)3;+6n@snPa|sEC%s)qzqX9 z|I+t(;LNo62&FuX2DW` zc0-8QYC*UqU4bVKm$Nia;P4R#N}EmSa8mqJ+l!qI{uP74^Aq?!Z}`SKEc4WZt-o_2 z#mK{7A)N3Ql_ttRnDIuK@s*a`!DUug)#7iZal;)HQBZqifCCqGOQrQ>2e!FHSYVqF zCV+yT6^ubVVdD6OvI-nNrK}=Z2S_*r3M^qY4dpPT97pyT&={%sEyN*!8#(#32dBTJD7RGA6 zmbo0N6G{;*4FLvt&iFEMTMo0l04(*Yg2KXFo$$PJ_44I;%NH>`6er-n1r?H*yO??$ zEKt^PnJWvIk-$jLh8n?IoNi-eQ@oNki(wE`G}0YgA7X$h8grA3t^JPzzC19B(`Ls$P7VYhf+ts zll)HK|6hCG0%uoM-g^=dBSs`NK1w6qqX|wzl6hnj!eI!RXEHFa%uGTg;Ni@iGnqrq zV{#so8AK$CRVr3fvD6CHD3)5V-c-20@N!eI>II6a;NyzbRIFO@YObK=3YY%Bwbr*D z`|N$@%wz_K-_P^Q%G$H{darMNYpw6uzq`pBBn7lLo*pS2h=Fb*?yWKpF)h{mk%CyD zDFQ=kKhh4<)iFj&LU>1<%(0OCEv;?s9i7{@cU^L6cTaC$|Bivdp`E)f+x@1?5?L; zh}Qd6^^KD%A6@-prF@$DG*&E*N9*bmxSx+C*NclN$r7spdR^vm9+?Q7-p@H)K{Q9c=x#5gOOHRCE(UN5= zU3m(omjD=>vmiMT6^NdZP10y(7s1w{x4%u-9*(Z3N`j(Z~GBk0S1 zgdJ~m`{Smj;1{C+S$0irej8%Yn7#D~#9G*3aghMRGiD_3%rLM`_OYM4DAeKyAFqZBj;q~!%(=9>TYJB9Dc-PWceXiYAZ^(gp-0c&vq!XuWGZtb)XXeSPWIYCe6 zNvKNcS88OI!7HNplfUEDo=X`3%;B?T@OC33gwqwxS|c#^v!E}USeW>bRAlMB5H0~3Z#_4GTHi!E*e!mFh&5seys2{QUJ zE%;W*D|d7Ue*2BllW0`JDFz?J5p_~g$TD!2lqR}NFJM#7)4AffukkC}L%`XB_$@@1 zf>jHJwUzWDT^>1I2QKj>EI~{~HxQV~! zo}lr2ie}&4G2nF#dUWIOYVYniJ+KWXZ&&ZGmTnjb>2Dcm>G677dXOWyK5nOoR^8y_Bw*Bu1j!6JV~wKQ^KcbD}m+zOyy2GFWKAP&$@ENcLLD)5Pp^ z^>py!sPk73Hcqy1=vg59ozM77Gz!wIvgSgolF!Nne+v+wsFmst`LviMFjx7G$Xj9X zCZ_kU`Mn>6w$(X0(ey+HOT&DXt>!MvGoMLja``D`KM2AaiI`*`MdWj{*IUZ8l2Qan zQq`23iRTMo4%Nl;`8XFFA>p`~cQNz>2%{-R8I;`ZM&0^QIak*tiLw{gvT8CA<(cXyw=q#7xV8)=*hyZ{B{V@R$M>C$i~sw!K20Vy@AhPzVpImbmZr{JEdO zWCBS!9||Yx`=!Y#y@0rWAHB|?+`t+DNLWW$vM!tkrbTVOGc*m%@y*apy#V$b^U(7{ z;KAbdRCN_~66rCSiG5BS_eG<%$-iohPX%;Z=jZ!l*%i~RhLcTq&2)2_E3>eeY48Rg zgN3EYfIRkDsXjFQa zu8@{CU(mAT2ufP8jn_%jfQrMOxq)_HWvsPAfx2>Qm!&P3cPK6b95}3uXjI02SYhb< z9rkx~{oHg|b*14-?8O1=%k)$}*?7tRwjcV<`(fXK_ysk6E=*9mqgcPoDwKclOz;~3 zPe-pQ&rA6aBmW76u=QWfl=IwHNLxL;UC=lu<6lLaG{{zwR!A-Y(GHOH8V$7-I?RPv zwZ`TE=C}_*6twbpx(5nJ6C}uA3*hAh_)`aghxi@ETz(Z@t_EGq?|9A;&a(R-wmvyc z*!N>tTQvdelgHE9It|}zuI*oAf8{%~;B?J^Y-{J^?XqlF4N5fKo@(c*ZbvQ4VehB@ zom9@up&Nle!E>*HjffL(z!?X8+ePaPY#)TZgJZz2hwy#}!ZM_zoGGQcxro8-SxoZ`J%wzb94rvb$k^2*?ydVj$CWjJiYh-!tnPI z;O|8N_#1RNFEtv+eLwiF?z?aEl+|Tvg@a`QAv4&Am25D5=N6Yaa43X*0VINlJ}O`>eghBb|p6#jgr9!p|#>S;@wlAOR74+!9W3D4Oq%t zj-*Xj3Icg0uOlz=$l}sRNR5SVGoT6*H>Y3N#f7dfLv2}m^}U(r(J3#e0h$fP%S)ll zDtHV2H7Qq_dzrTTzd+=7%qUdF;&YSpy-km=q8~crtdNNq(e}19=@>GX8_&1Ja6>v(AUqW#v!R~2qD|jx#4)4Qw#{Dnfw$07*fL;VEFT;c*!J8x{g8+ zPSg}iU&~^B5^1e40-@Xzlo^#CStcIK1L{n; zz5qz78{;P%K>CTWA^^10gQr5IR=j}i1o~4s2!I454f|j^2eLXoNpgP$g><9Y;}L{BoI8FtJ1rXROeNC^B9A~PSWCi2qY9fv z0zldDLZu1A2t<71>DuDtdYym$Fu9Vcg|z|aZ4!`(cRE=N7MLn@g2up%zImc~V0hds zpq`|K<1vy{z?_n{7vr_M0I5C6JWdcjWfa4yFV&0n80wLoX$@Hyv15NSlkU$_5hv5^0cZX0iZv z_kth&aShvrJ#~YRrpK#!Dd`~EZ19`}dFpkS#=wLU?6wjPUx52lh2>h+hBZxocL(E> zgj(k>9Q||-Xgf524txm8GEnBASZ>l)hi{wZ4bbeM)&-(1o*kOpQA(C@;M$QB=Q|#s zu-%ndSJUso1xQPe6e+ENP+%-VyR;MY=1Uz?zhmv7PX?T?o&Lh`%s83WBf_y8Iz3j$*(*}U?|MHS)F z>il7G&AC}ld&=iy+Ie97OL)j?R+})mGl&Bo7-nRL2N2Eevsy8I`^@fSp33$qD4r`g z5?j=$U_oNm2lFFxqNI2)-teD+`?7CeEfQ-`uw+N_BYc+|o`Mb(X416AJcZ_$4H6r1 zttb_1Tg#+P;i9?j^ug&C*c~ZlqEL{Vn|b>1&5RTW}xr3t6H#k>QNV zsD)N^Eyl|Jn~tE0Q%dc0djWIv5zL9z$emd<}hs!&rPc?e7hj24ai-Tv4fd@Kq_CvuH2NK#n<(L6hYsceLu-BM~rIXp! z5Y5Srj!MN(`5=`2BkCEa6@QKy&Od|>_mX`I!`RoSJOT=LG^>LN92!#4?joW>C~UZ( zhM~8oP!ZrjzY{J32|3ji$xVnnDPi0+eF8*%=$Fx{4X5f5*e_d?x^m|3%hO{`aV%9Z z<4_ble^nEd4WqE%ChUNgvTQa1I+Vg_B`JkUIY&Dk*yN;H2o??;p+V=I6S--m0R~@^ zh@FPyHt`*LwOsG%mF>Cs^xS}FVY>{sDS`3iN*j%*g{`5A`f_mgk=8!%VyQ-jX|NmM zUo=tujAKOG;3=oV6Oi!}SObH1PUwcQ|- zSnd_RvkkYcPc3QAI-ss?kj98cD}0>wSwjt!vtOIag>O-u0Hk{-evdl^uuef@JvqMo z|1t2Kk0TuWJa{PgA>JeDs0(iV&3tgLaHdkk9v{3c5c-^wF5SjHMQN!>{XG&@R3aDV z{_7_9OU!cmVEzrSLUN`Xn1Yv!saI+|)kBAJ)47n_hho_TJEuBnQEOW>j>n`|N z0@0ilBAYkENT#Vym~qMi&#tbw-?!oHA}-tYC8$tv9l95$Kd3|(ra!N4AV~*HBvp4B z$$`x+ISAcstF^75KtPC)I@2t65QpL8tbb8u_yEJ@3^1nDf5zUSU_p>eF(*7yEvQ@G z62zAay@fh-7u3Vxrs7u!TRfus(I`B1K>;L}zsT;Oe_v?;KR6D4uwNd=hSvpxKZHpG zAxWjK1JzMY$lsMe*Ol<$Mpnt?=>!<5Y>2FxSE+h&BW|7QB2yj0xH#_^GA2SEyx$5i zwC;QEH5BF~g~=6cJT$(#c0shTn*ebLac=4-?Gj;%&39Jv8Nrh!IkxFxOO{Q_&oD3A zhCa%pKF(LTpK62PIPn62W>{Xdk(>f3^Y$~&yJ^<_fRXIYv#z&cQ)5)1cVXI$reWO{ zQX{0DidnT0o}1}BHEx(aoHkt0pka;1OdtZeEaee6nHP&Wrk1&XecSXR-dKtH4@rGR zUyzd6bSxm7t|z5Tfc9}6Zs|%;yMve_hWZ(pMlv8f$q}6DQpF}NpQ;)J zJnYRwxh|45&}Rrm#bmM=*My9nMANd)L?IU(QH8KdtTsR{f=VXug!(iyoGOlkF6UG& z+!rcUVQP3^XD3{z(5Yq9g+v4KF6(L^+K#_Bn^Tc4_#&8jgSMu9@Byjb?!Qf5wC{52 za9U*-!+UPizRk7442gNC#u;PeoTmZ-KmZARP?HK2L!@f4DrODMi*?>}|MGowxZ{R@ zWsg&(X3SobhgNHFec|hG=C#@I2E8H;ZrrCZdb}FS8Ks{+JlDJ6e16s7Tm`%kjAUTm za5mV&2am8syf2v`w#xtvzc&xz(v*VDxqPuz*mA5L0)f9D{B<(ztZzR0g>|Q}R~{8d z%1TVJh(`Gq+11`9N(WI9W`1dh#Hrf75AZZ{Auclx@D&8dH0R119xfxWGL}GlU{Bsc zd|a+`w-@jVHiiZYEM>jM)gUvx*`Fj@mkSN{2a9kEG2DL5A8}lu~V$sQY+^MzFra_vB(WC=zPJzB~KRi-p}~lMczK_?xJ&x z_C)hW7g7#6n@_IEL4Y-nRM0jLrkt|Ec4A9GC(A zCP^0UN7E;EsHUfFp^UK>gP<#1O_twP>FCH`DWi&61si<3hw~zm3+JZJ}re zjG%R=coFaiW%zvB@-&}Fko-9Jbj9*;IqPGA&{seP5rd(13L%+44UM)ui=R%*9k%ho z0_m?UqCag(wP6#Gr@_tNA7+0kIN5Xx3vRCJRCxmO*vbo%(mXq> zOaVn(6R#E^4@~f+@kw`{`6a>VqP($;G!VByFE9Qs=xig{`sLfxE$Zm_C+D>Z-8#r zVU&F^qp}ETR`gpMDP7PO$3iUwU34L(RCIB_)Al?9VgOda*P5jA@zMoO$xelQHYM4uGJ9 z)`gHa-V5u0EM_K_9?!?`03SoFKQ&I?yohQdXe1zPfI|ic62$@t92tme6Kl<(VJy7% zw!)WNSZf)(!M?;TzvJ$fAndWnW%8!t>oS z!?F?S+mKitGOfDoqCNKc*u5|3r80{?cHt|F`-YTiUW<|h+1p26)%~XwL`s1Q+GrbG zLXtloeTs4$vKMMB{-nB9>JF!=0M7kMwKqx}wSu1jX!XYoM<9*Jx890G)sb?jl2$}A zg8=LN6vnegaD>(eiR}fJT}Gn;0V_;4kq9?6sWq?Nh=7dB~wiZvgI!vo&q=wn1YjI zGN{>i08djSA#v|~=<2f>F{-`19?=w?QE?bVBw5AXzIh)sD&W6bRs&=53@w zYFGP)GORG_q@UpymR7acA`J0-I_ORz7D$6;HZ%$jF0qb)S9B^Zk|*|ic3RV>Uo&_M ze95l=6gIY5O%^86S;Ry9FwVLN%ie(f;8C3EACvomsE-T8JsNaiEFn`2@t&c1HJ%1_ zk@n5vP!6(dr0zJhIvNExw+MG*q_3eL@xfvl4#4@(B<8*WEEeJ%q6EtXuaL5Und0Fz zq}fNCmCyx9-Vo5r$J2ttArKlMu?PVu50b3S@VT6w%H&G$&vKFXmbW_K5}C91g2PGe zE#qO%YwKaNm7G*`l28Nuu9XVv3I0}+8A+(dY;Fg~gg^B5bou%HF8g0#U7kUC6q9o7 z(gJ1Fi$shMk#IjF1Vlo#0kK;_olhnQ-WKZ+G|z!>!4xpx2Ra7(x_1tB_4Nwi94?K<_O!*pvWBl9A!G5z zE9&8E+3Y_0DVE4X+&_<2(ekU4f7Q{(NBY0?4jQ;%_(-0M(Qb{vg_v-STe-hJ!Eatm zvSe8%e8s_g5J}X4dDZN}q-1h;3L7IQgSr%ek+Th_6YpV!aMRKFa<>8g;WUml|rz^ z;2XxDb96sd;(@Z_fF6MdHmK~M2`Np*>ZzvoU;}7#+Kv`)W-m`JJz>~WYoYm5Mfk(2qP*!YJ$_ck?Wb>!yS=*f zhy_01K9Wc^2A6oMZHfp}TFOlR1SP6C`Thn?Kkq>~Z2I}^DCc&=T{6CJwte)c1wP;5 zZctoPPXcGvuJK<8f=N;g5oRrhSL4iQh=Huix$U{NPMSPnX0osY&$?yyBy%Cv?08kA zo!|Xx!^!W_RrG-l1f*@D2e`u6%x`}iH*U0)Ln<^wH@WOk@}lZ5G&+pn#;m&gqS~Kq zwzZ3)c-AU#dR68ZyD`9-BRPB29E2;-8joBx&{IX)>hBM80K0Tw!Yxqao!_avh{Y{| zMgy!)OIvo~PK3fU%;SDj{ru2<^<$>Q}red%X<{Of!%dOyT!*kRSAECZJ2y6U(oHQz6c!@q>G{+YQEr?2NXiz z26lVUbIOCFe?S*P?}aaQypccESsIvtUzoh#v3p_P8DR&4EstpHcMzLHu~;RK!EbL& zsE<14#t52$!mM|O72&k*$QL0mh(Pj$4PP+wgjMWW^K%}VBl$A!DwIle}4D(IUY0We;B+rz>ZD}#>#JBg2yWu18O?oe8|iDFK7e(J>ha> zJJ9yS2+Km|dm*$xejvR4G&l6~&kv7(HHXTfLF`8@7I;|DUe< z@%WO1s}Egrc;Atmj(+OcKOg_aiH5VzTwG`zc>O&CH@E&K|IOhGKJeOiyyY{OfBZ|| zxb^<$fAz0F9A4Sh$Y32 z`|sJu-uzVWA8ct{zvG3)?)%Z&$6oQ}e`~sb<&AKf7RNC7TpBx*&b+(Y{RNm!($K5Qn==BX z;ZiZlR>ycbQEjBE&GnaOR5?g?Q%iBw>8EPsIpSF*f0B_yp|xmXSY3}#m`?{K8j?_E z#+C0@X@6VzYn3N2cHtTsO~Ir!tLZ{+L$}nlnZ->KEF!I%S1GMHjd8jXY&Yd&!;n-> z#4;rIu?3<@2b@=tR6+S_RSMf_xVrXBkXi5psktDl(o+A)l;jhe3r74rw}wl`qo=G< z+D6(*$Qmbi0YrR$bK2Dnt6nfrMebo$J93aPgVbq|-&Khf{r?|>yz~AC%J*NZo^rTT zs$Z2B4tLaAGEUW2`Me%GG3RM<26hB&WHzbEYzRw8OS~ue3Fn&lOrR_!Kw@PV$!7Wu zwEB|^JIp@*ElWwN+nn;kaapQ3#$WV8*OG}?i)BW}(e#54;(YTE#&|vSfDl^#3;{Yl z+>DAoVJm#}L!0u%su7OCHy}wHGwg+NnCIuZE4AItudT~Pqe+;;OTf?-%=tPd=xCxV zIK&MtPg=8^ad~05gB?CYWDVELSez{A#YS@D>=ytyi)`1DS7}%WmG*bpUahxzC&`|X zppmyviJ8B#_EsO_Wq z49v{}%GqHoy;MJYyjxR6$g-hKwj?dZyeiI^vp&%(F_3K-M+Q7r{TCu^&dt#GhZegn zPpP5R<>y%K6*p0a*NP2R{XT^Cwgc;JQJq|GC$Q$?)_M!m7E@L|gk@lG&LCH+5ldJW zpCyg3WI{Y$X^r#jnDaweu35E*%=J2pGOYCSHd*t{_}-y$%@E}qLk+6S)s;UKzNJ{O zLPcKh{%7;Ex&1;|mF&Y`T`Cme!CK=zJhPuCT`GLR3kg>k%7pbD$h_1F;ag$ZseZDl zUjE1?Nd063{#*^~8SPyi1Hv)~0j5f+5;jo5!-a;}aG^0)fN!iVUOc}Se+h^3_-4a2 zN&PBl^R>MImsjZ|+Dis8+F*4B7HiUja4CS3i+f^`<#Bd%)IIsLB}_QM6nF{;3mNNS zRLhZ%r!b-`w*WDw)_fa7IYQpH2rcj#81AQe#*c^CgH8DjjS2}-C7?>7T0M_4wTn*_ ze|WGoOx}}**|pYQvgVT-0mC+LPX0{eV}lJSUWn%EUfZ@K046+-W%lzwIsgHYBjjr`pjb7>1t{SD8vA(C|_Y}UT=zEI3FQS+DEk>YciuuRy=x_dJ zzvHxA2Bm|%`BY(NmgMK@Py9VJmT1qllRRrFN#>Kf#C!hF(dSY2R=|k~(vFj=S=4Cj zNB(wV5@=!>5}^lbNDA-sGSQBsktIjI8CkOb(a4e=2!e)99;k#lc%q1p6UjURoj;f# z%I1n#WvQZvWETSR$Xy@E-*6=sFBLH>MW7`4H;xJpOW9i)jcLKxlOolNqtNBVBcUl3ifsNhBvyBceoQtsdW6@_W5Bwr^O{ z&k1E3ydwc@Z#0+3{OK|V0YJ&37f;j4Xo}22WYH#qc<8~vgqCP4_))-e)W(sqR2l~H zvk_>{v*QDgZRF>P(g`gv%8RsMpd`t^6!k~=>Y%YmW^<*zV;=m3j0oGNIEQ9Z@C%X4 zdWmE>_Qw|ugYE9KEo%==WNlg2Pf4xkE=wtTKoSf@sT3}ngr#aPR?)E;!w z8u9Hpj7_Q_Ac5Ko#szhp+QDN1AVTZ+qF(5VbQPTJm>)qhr(GD#x-7JYP)D+OAeY}q z`vq5@Z$-P|EG5D-aqF%n4I?Xs5W8E5xej6k5vG(9qZj+$-TqUa^oh zA}>Z>DmPM0dpP_h(Fb}@V=3-Ts9wTxA+k&POdCn%M@m43fR??vY%dy;cqRpa zpw?*6A;`xHlYNy5Oy3XW$%?5^J_O403Ib1Id?mh;w#I3-;ve)9*YS5fc2twqAZvZY z8zNB?oi&e10T!h8Bn7J0?Y58sG6Aou{`iVVyGDz!sF(znrotJK3VcHAoW@>p zYR!AD&7k2vkRQb*5(NV~;TwfujCMR~0rn5#{HigB_U%Lp`k8XW0kT74liwYB3>yzw zM9{Si4xudqs|WT@&<}B*l1m8uZn?O`qQmS^pR92uXiw0ZrT6#V`0ZQ%;a_fT-f+j? zKK6!RcU^GN9Z$7C_sUN+-ge7Pk1YAZo>gtRyArjhjV+_J^lL)+=ifg6{DY^Mvy%9r z{>$RjMLR6*mGs+)P$_*xalJhcH?P`S{g-}t_nGeRRQ*%$Tf}csz)vP<6~Dy+Kl<)b zBVWY7Gnbw92g_ge>esyX4^=15{-YJIJLiwXdokzZac zU#;9K<2T88>-%K)dSdtSwyihaGx@qtzpB0e7ytInC(gdK{m&okIqi{OUf=%fOOAhK@w$83 z@4oFjQ*D#~(T;k=?XWTT+tnLq}Ppl=<4mndK``Pw)94_@vNoG+|t_C-qA_r`1PB)Y>JhG zTCtpaczrl3Z2Q1JQji8e8jOM^@F!8Ua|;xjk~WyeB69% zUJ~&rQ2G3O{&}2z@UDLKDY}gAmq)z25mqd9({dhvy!AYj*Z*z4{-&{@-x(VozO(79 z%)dYRuP=$bT)z7K|GyuB-;cmNkHA&H8O#54@siISoIX77{7l0h0DhtHkP43?rtr85 zPavl7qzX?Vrtqu^r@kWMQCONu?sIU?-g|#X)A*Rr*LOWs#-6{+rrf`J{3B(lA zDije@xK@Q55L38Gg_{vmxK)MQ5mUHJg}V_`xL1V-5L0+ag+~!ncwB`i5L0+kg{Kfx zcvgi|&p`hXma4EEF@+T>tVB#}WMZ^@YRpAE2 z6mC-CX2cY3RpEBT6z)>tZp0MsRp9}|6dqFHQN$D;SK$f76rNP!DZ~_>RpC^Sq$n&^ zVL4(7D^ysCn8I2Wnh;ZHR-qj+g>Dsw5L389g#=;>X%&iyDO{_<4Tve+q{7XJDcq{U z?T9JdrNZ5aDcq~V1BfX+q{5?!DLk&i6No81slro;DLkvfsb`}92uoF1j+nv<6;>jq zuvUd8#1xuUXh%$;TZJLS6s}MqftW&Cg(6}K*Q#&>VhT5@a5G{Gx2kYEVhVSua5rKK Q_o_ftUP29w5$O590DD`K&Hw-a diff --git a/test/util.hpp b/test/util.hpp deleted file mode 100644 index cdc9305..0000000 --- a/test/util.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2018 Zubax Robotics - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of - * this software and associated documentation files (the "Software"), to deal in - * the Software without restriction, including without limitation the rights to - * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - * the Software, and to permit persons to whom the Software is furnished to do so, - * subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - * Author: Pavel Kirienko - */ - -#pragma once - -#include -#include -#include -#include - - -namespace util -{ - -template -inline std::string makeHexDump(InputIterator begin, const InputIterator end) -{ - static constexpr std::uint8_t BytesPerRow = 16; - std::uint32_t offset = 0; - std::ostringstream output; - bool first = true; - - output << std::hex << std::setfill('0'); - - do - { - if (first) - { - first = false; - } - else - { - output << std::endl; - } - - output << std::setw(8) << offset << " "; - offset += BytesPerRow; - - { - auto it = begin; - for (std::uint8_t i = 0; i < BytesPerRow; ++i) - { - if (i == 8) - { - output << ' '; - } - - if (it != end) - { - output << std::setw(2) << std::uint32_t(*it) << ' '; - ++it; - } - else - { - output << " "; - } - } - } - - output << " "; - for (std::uint8_t i = 0; i < BytesPerRow; ++i) - { - if (begin != end) - { - output << ((std::uint32_t(*begin) >= 32U && std::uint32_t(*begin) <= 126U) ? char(*begin) : '.'); - ++begin; - } - else - { - output << ' '; - } - } - } - while (begin != end); - - return output.str(); -} - - -template -inline std::string makeHexDump(const Container& cont) -{ - return makeHexDump(std::begin(cont), std::end(cont)); -} - -} // namespace util diff --git a/tests/.clang-tidy b/tests/.clang-tidy new file mode 100644 index 0000000..880d946 --- /dev/null +++ b/tests/.clang-tidy @@ -0,0 +1,38 @@ +--- +Checks: >- + boost-*, + bugprone-*, + cert-*, + clang-analyzer-*, + cppcoreguidelines-*, + google-*, + hicpp-*, + llvm-*, + misc-*, + modernize-*, + performance-*, + portability-*, + readability-*, + -google-readability-todo, + -readability-avoid-const-params-in-decls, + -readability-magic-numbers, + -readability-function-cognitive-complexity, + -llvm-header-guard, + -google-runtime-references, + -misc-non-private-member-variables-in-classes, + -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-reinterpret-cast, + -modernize-pass-by-value, + -hicpp-no-array-decay, + -cert-msc30-c, + -cert-msc50-cpp, + -*-function-size, + -*-easily-swappable-parameters, +WarningsAsErrors: '*' +HeaderFilterRegex: '.*' +AnalyzeTemporaryDtors: false +FormatStyle: file +... diff --git a/test/catch.hpp b/tests/3rd_party/catch.hpp similarity index 100% rename from test/catch.hpp rename to tests/3rd_party/catch.hpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 0000000..4e79cd2 --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,42 @@ +# This software is distributed under the terms of the MIT License. +# Copyright (c) 2020 Zubax Robotics. +# Author: Pavel Kirienko + +cmake_minimum_required(VERSION 3.19) +enable_testing() + +project(kocherga CXX) +set(CXX_EXTENSIONS OFF) +set(CMAKE_CXX_STANDARD 17) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING + "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." + FORCE) +endif() + +set(LIBRARY_DIR "${CMAKE_SOURCE_DIR}/../kocherga") + +file(GLOB_RECURSE FORMAT_FILES + ${LIBRARY_DIR}/*.hpp + ${CMAKE_SOURCE_DIR}/unit/*.[ch]pp) + +# Use -DNO_STATIC_ANALYSIS=1 to suppress static analysis if the tools are not available. +if (NOT NO_STATIC_ANALYSIS) + find_program(clang_tidy NAMES clang-tidy-12 clang-tidy) + if (NOT clang_tidy) + message(FATAL_ERROR "Could not locate clang-tidy") + endif () + message(STATUS "Using clang-tidy: ${clang_tidy}") + set(CMAKE_CXX_CLANG_TIDY ${clang_tidy}) + + find_program(clang_format NAMES clang-format-12 clang-format) + if (NOT clang_format) + message(FATAL_ERROR "Could not locate clang-format") + endif () + message(STATUS "Using clang-format: ${clang_format}; files: ${FORMAT_FILES}") + add_custom_target(format COMMAND ${clang_format} -i -fallback-style=none -style=file --verbose ${FORMAT_FILES}) +endif () + +add_subdirectory(unit) +add_subdirectory(integration) diff --git a/tests/integration/.gitignore b/tests/integration/.gitignore new file mode 100644 index 0000000..07f45e4 --- /dev/null +++ b/tests/integration/.gitignore @@ -0,0 +1,4 @@ +# Test outputs +*.json +*.rom +*.db diff --git a/tests/integration/CMakeLists.txt b/tests/integration/CMakeLists.txt new file mode 100644 index 0000000..dfdeb61 --- /dev/null +++ b/tests/integration/CMakeLists.txt @@ -0,0 +1,40 @@ +# This software is distributed under the terms of the MIT License. +# Copyright (c) 2021 Zubax Robotics. +# Author: Pavel Kirienko + +cmake_minimum_required(VERSION 3.19) +enable_testing() + +project(kocherga_integration_test CXX) + +set(library_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../kocherga") + +# C++ options +set(CXX_EXTENSIONS OFF) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wconversion -Wsign-promo") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wtype-limits -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") +# Attribute warning is useless in GCC: https://stackoverflow.com/questions/50646334 +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-attributes") + +# Enable coverage if makes sense. +if ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_BUILD_TYPE STREQUAL "Debug")) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 --coverage") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") +endif () + +include_directories(${library_dir}) +include_directories(../util) + +file(GLOB bootloader_sources ${CMAKE_CURRENT_SOURCE_DIR}/bootloader/*.cpp) +add_executable(bootloader ${bootloader_sources}) + +# To run just this test specifically, go to the binary directory and run: ctest -R run_validator +add_test(run_validator + "${CMAKE_CURRENT_SOURCE_DIR}/validator/validate.sh" + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) +set_tests_properties(run_validator PROPERTIES TIMEOUT 30) diff --git a/tests/integration/bootloader/main.cpp b/tests/integration/bootloader/main.cpp new file mode 100644 index 0000000..8b1d126 --- /dev/null +++ b/tests/integration/bootloader/main.cpp @@ -0,0 +1,320 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2021 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga_serial.hpp" +#include "kocherga_can.hpp" +#include "util.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +/// Tunnels serial port connection via TCP socket as-is without any wrapping. +/// Per Kocherga's API contracts, the API is fully non-blocking. +class TCPSerialPort : public kocherga::serial::ISerialPort +{ +public: + explicit TCPSerialPort(const int sock_fd) : fd_(sock_fd) {} + + ~TCPSerialPort() override { (void) ::close(fd_); } + + TCPSerialPort(const TCPSerialPort&) = delete; + TCPSerialPort(TCPSerialPort&&) = delete; + auto operator=(const TCPSerialPort&) -> TCPSerialPort& = delete; + auto operator=(TCPSerialPort&&) -> TCPSerialPort& = delete; + + static auto connect(const char* const remote_host, const std::uint16_t remote_port) + -> std::shared_ptr + { + const ::hostent* const he = gethostbyname(remote_host); + if (he == nullptr) + { + throw std::runtime_error(std::string("Could not resolve host: ") + remote_host); + } + ::sockaddr_in sa{}; + sa.sin_family = AF_INET; + sa.sin_port = ::htons(remote_port); + sa.sin_addr = *static_cast(static_cast(he->h_addr)); + + const auto fd = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (fd < 0) + { + throw std::runtime_error("Could not open socket"); + } + if (::connect(fd, reinterpret_cast(&sa), sizeof(sockaddr)) < 0) + { + (void) ::close(fd); + throw std::runtime_error("Could not connect to remote endpoint at: " + std::string(remote_host) + ":" + + std::to_string(static_cast(remote_port))); + } + return std::make_shared(fd); + } + + [[nodiscard]] auto receive() -> std::optional override + { + std::uint8_t out{}; + if (::recv(fd_, &out, 1, MSG_DONTWAIT) > 0) + { + return out; + } + return {}; + } + + [[nodiscard]] auto send(const std::uint8_t b) -> bool override { return ::send(fd_, &b, 1, MSG_DONTWAIT) > 0; } + +private: + const int fd_; +}; + +class SocketCANDriver : public kocherga::can::ICANDriver +{ +public: + explicit SocketCANDriver(const std::string& iface_name) : fd_(setup(iface_name)) {} + +private: + [[nodiscard]] auto configure(const Bitrate& bitrate, + const bool silent, + const kocherga::can::CANAcceptanceFilterConfig& filter) -> std::optional override + { + // Simplified implementation. The bit rate is configured externally. + std::clog << "SocketCANDriver::configure(" // + << "bitrate={" << bitrate.arbitration << "," << bitrate.data << "}, " // + << "silent=" << silent << ", " // + << "filter={" << filter.extended_can_id << "," << filter.mask << "})" // + << std::endl; + return Mode::FD; + } + + [[nodiscard]] auto push(const bool force_classic_can, + const std::uint32_t extended_can_id, + const std::uint8_t payload_size, + const void* const payload) -> bool override + { + ::canfd_frame frame{}; + frame.can_id = extended_can_id | CAN_EFF_FLAG; + frame.len = payload_size; + frame.flags = force_classic_can ? 0 : CANFD_BRS; + (void) std::memcpy(frame.data, payload, payload_size); + return ::send(fd_, &frame, force_classic_can ? CAN_MTU : CANFD_MTU, MSG_DONTWAIT) > 0; + } + + [[nodiscard]] auto pop(PayloadBuffer& payload_buffer) + -> std::optional> override + { + ::canfd_frame frame{}; + const auto rx_out = ::recv(fd_, &frame, sizeof(::canfd_frame), MSG_DONTWAIT); + if (rx_out > 0) + { + if (((frame.can_id & CAN_EFF_FLAG) != 0) && // + ((frame.can_id & CAN_ERR_FLAG) == 0) && // + ((frame.can_id & CAN_RTR_FLAG) == 0)) + { + (void) std::memcpy(payload_buffer.data(), + frame.data, + std::min(frame.len, payload_buffer.max_size())); + return std::pair{frame.can_id & CAN_EFF_MASK, frame.len}; + } + } + return {}; + } + + [[nodiscard]] static auto setup(const std::string& iface_name) -> int + { + const int fd = ::socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW); + if (fd < 0) + { + throw std::runtime_error("Could not open CAN socket"); + } + ::ifreq ifr{}; + (void) std::memcpy(ifr.ifr_name, iface_name.data(), iface_name.length() + 1); // NOLINT union + if (0 != ::ioctl(fd, SIOCGIFINDEX, &ifr)) // NOLINT vararg + { + (void) ::close(fd); + throw std::runtime_error("No such CAN interface: " + iface_name); + } + ::sockaddr_can adr{}; + adr.can_family = AF_CAN; + adr.can_ifindex = ifr.ifr_ifindex; // NOLINT union + if (0 != ::bind(fd, reinterpret_cast<::sockaddr*>(&adr), sizeof(adr))) + { + (void) ::close(fd); + throw std::runtime_error("Could not bind CAN socket"); + } + const int en = 1; + if (0 != ::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &en, sizeof(en))) + { + (void) ::close(fd); + throw std::runtime_error("Could not enable FD mode on the CAN socket -- is CAN FD supported?"); + } + return fd; + } + + const int fd_; +}; + +auto initSerialPort() -> std::shared_ptr +{ + const auto iface_env = util::getEnvironmentVariableMaybe("UAVCAN__SERIAL__IFACE"); + if (!iface_env) + { + return nullptr; + } + static const std::string Prefix = "socket://"; + if (iface_env->find(Prefix) != 0) + { + throw std::invalid_argument("Expected serial port prefix: " + Prefix); + } + const auto endpoint = iface_env->substr(Prefix.size()); + const auto colon_pos = endpoint.find(':'); + if ((colon_pos == std::string::npos) || ((colon_pos + 1) >= endpoint.size())) + { + throw std::invalid_argument("Invalid serial port name format: " + endpoint); + } + const auto host = endpoint.substr(0, colon_pos); + const auto port_str = endpoint.substr(colon_pos + 1); + const auto port_raw = std::stoull(port_str, nullptr, 0); + const auto port = static_cast(port_raw); + if (port != port_raw) + { + throw std::invalid_argument("Port number invalid: " + port_str); + } + return TCPSerialPort::connect(host.c_str(), port); +} + +auto initCANDriver() -> std::shared_ptr +{ + const auto iface_env = util::getEnvironmentVariableMaybe("UAVCAN__CAN__IFACE"); + if (!iface_env) + { + return nullptr; + } + static const std::string Prefix = "socketcan:"; + if (iface_env->find(Prefix) != 0) + { + throw std::invalid_argument("Unsupported iface name prefix: " + Prefix); + } + const auto socketcan_iface_name = iface_env->substr(Prefix.size()); + if (socketcan_iface_name.empty()) + { + throw std::runtime_error("SocketCAN iface name cannot be empty"); + } + return std::make_shared(socketcan_iface_name); +} + +auto getSystemInfo() -> kocherga::SystemInfo +{ + kocherga::SystemInfo system_info{}; + system_info.node_name = "com.zubax.kocherga.test.integration"; + { + auto hw_ver = util::getEnvironmentVariable("UAVCAN__NODE__HARDWARE_VERSION"); + const auto maj = std::stoull(hw_ver.substr(0, hw_ver.find(' '))); + const auto min = std::stoull(hw_ver.substr(hw_ver.find(' ') + 1)); + if (maj > std::numeric_limits::max() || min > std::numeric_limits::max()) + { + throw std::invalid_argument("Hardware version numbers out of range"); + } + system_info.hardware_version = {static_cast(maj), static_cast(min)}; + } + { + const auto uid = util::getEnvironmentVariable("UAVCAN__NODE__UNIQUE_ID"); + if (uid.length() > system_info.unique_id.size()) + { + throw std::runtime_error("Invalid value length of register uavcan.node.unique_id"); + } + std::copy(uid.begin(), uid.end(), system_info.unique_id.begin()); + } + { + static const auto coa = util::getEnvironmentVariableMaybe("UAVCAN__NODE__CERTIFICATE_OF_AUTHENTICITY"); + if (coa) + { + system_info.certificate_of_authenticity_len = static_cast(coa->size()); + system_info.certificate_of_authenticity = reinterpret_cast(coa->data()); + } + } + return system_info; +} + +} // namespace + +auto main(const int argc, char* const argv[]) -> int +{ + (void) argc; + try + { + const bool linger = util::getEnvironmentVariable("BOOTLOADER__LINGER") != "0"; + const auto rom_file = util::getEnvironmentVariable("BOOTLOADER__ROM_FILE"); + const auto rom_size = std::stoul(util::getEnvironmentVariable("BOOTLOADER__ROM_SIZE")); + const auto max_app_size = std::stoul(util::getEnvironmentVariable("BOOTLOADER__MAX_APP_SIZE")); + const auto boot_delay = + std::chrono::seconds(std::stoul(util::getEnvironmentVariableMaybe("BOOTLOADER__BOOT_DELAY").value_or("0"))); + std::clog << "Bootloader configuration:" // + << " linger=" << linger // + << " rom_file=" << rom_file // + << " rom_size=" << rom_size // + << " max_app_size=" << max_app_size // + << " boot_delay=" << boot_delay.count() // + << std::endl; + + util::FileROMBackend rom(rom_file, rom_size); + + const auto system_info = getSystemInfo(); + kocherga::Bootloader boot(rom, system_info, max_app_size, linger, boot_delay); + + // Configure the serial port node. + auto serial_port = initSerialPort(); + if (serial_port) + { + std::clog << "Using UAVCAN/serial" << std::endl; + (void) boot.addNode(new kocherga::serial::SerialNode(*serial_port, system_info.unique_id)); // NOLINT owner + } + + // Configure the CAN node. + auto can_driver = initCANDriver(); + if (can_driver) + { + std::clog << "Using UAVCAN/CAN" << std::endl; + (void) boot.addNode(new kocherga::can::CANNode(*can_driver, system_info.unique_id)); // NOLINT owner + } + + const auto started_at = std::chrono::steady_clock::now(); + std::clog << "Bootloader started" << std::endl; + while (true) + { + const auto uptime = std::chrono::steady_clock::now() - started_at; + if (const auto fin = boot.poll(std::chrono::duration_cast(uptime))) + { + std::clog << "Final state reached: " << static_cast(*fin) << std::endl; + if (*fin == kocherga::Final::BootApp) + { + std::clog << "Booting the application" << std::endl; + break; + } + if (*fin == kocherga::Final::Restart) + { + std::clog << "Restarting the bootloader; using executable " << argv[0] << std::endl; + return -::execve(argv[0], argv, ::environ); + } + assert(false); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + catch (std::exception& ex) + { + std::cerr << "Unhandled exception: " << ex.what() << std::endl; + return 1; + } + return 0; +} diff --git a/tests/integration/validator/can.orc.yaml b/tests/integration/validator/can.orc.yaml new file mode 100755 index 0000000..3b6ded2 --- /dev/null +++ b/tests/integration/validator/can.orc.yaml @@ -0,0 +1,32 @@ +#!/usr/bin/env -S yakut -v orchestrate + +uavcan: + can: + iface: socketcan:vcan0 + diagnostic: + severity: 3 + +$=: + - ?=: rm -f *.rom *.json + - ?=: sudo modprobe can ; sudo modprobe can_raw ; sudo modprobe vcan + - + - ?=: sudo ip link add dev vcan0 type vcan && sudo ip link set vcan0 mtu 72 && sudo ip link set up vcan0 + - + - $=: yakut --verbose file-server --plug-and-play alloc.db --update-software $SOFTWARE_PACKAGE_DIR + uavcan.node.id: 32 + - $=: yakut --format=json sub --with-metadata uavcan.node.Heartbeat.1.0 > heartbeats.json + - $=: yakut --format=json sub --with-metadata uavcan.diagnostic.Record.1.1 > diagnostics.json + - $=: + - ./bootloader + - # Wait for the bootloader to exit. It will only do that after it has successfully loaded and validated the app. + - exit 223 # A non-zero exit code will bring down the entire composition, which signifies the end of the test. + uavcan: + node: + hardware_version: [10, 30] + unique_id: aaaaaaaaaaaaaaaa + certificate_of_authenticity: this is a certificate + bootloader: + rom_file: b.rom + rom_size: 1024 + max_app_size: 512 + linger: false diff --git a/tests/integration/validator/com.zubax.kocherga.test.integration-10-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin b/tests/integration/validator/com.zubax.kocherga.test.integration-10-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin new file mode 120000 index 0000000..649b0af --- /dev/null +++ b/tests/integration/validator/com.zubax.kocherga.test.integration-10-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin @@ -0,0 +1 @@ +../../unit/images/good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin \ No newline at end of file diff --git a/tests/integration/validator/manual_v0.orc.yaml b/tests/integration/validator/manual_v0.orc.yaml new file mode 100755 index 0000000..3c884fd --- /dev/null +++ b/tests/integration/validator/manual_v0.orc.yaml @@ -0,0 +1,26 @@ +#!/usr/bin/env -S yakut -v orchestrate +# This composition is mostly intended for manual testing due to lack of adequate automation tools for UAVCAN v0. + +uavcan: + can: + iface: socketcan:vcan0 + node: + hardware_version: [20, 21] + unique_id: cccccccccccccccc + certificate_of_authenticity: this is a certificate +bootloader: + rom_file: c.rom + rom_size: 10485760 + max_app_size: 1048576 + linger: true + boot_delay: 30 + + +$=: + - ?=: rm -f *.rom *.json + - ?=: sudo modprobe can ; sudo modprobe can_raw ; sudo modprobe vcan + - + - ?=: sudo ip link add dev vcan0 type vcan && sudo ip link set vcan0 mtu 72 && sudo ip link set up vcan0 + - + - uavcan_gui_tool # Start the tool for manual testing. + - ./bootloader diff --git a/tests/integration/validator/serial.orc.yaml b/tests/integration/validator/serial.orc.yaml new file mode 100755 index 0000000..fc69798 --- /dev/null +++ b/tests/integration/validator/serial.orc.yaml @@ -0,0 +1,39 @@ +#!/usr/bin/env -S yakut -v orchestrate + +uavcan: + serial: + iface: socket://localhost:50905 + duplicate_service_transfers: true + diagnostic: + severity: 3 + +$=: +- ?=: rm -f *.rom *.json # Clean up the outputs from the previous run. +- # Do not proceed until the cleanup is done. +- ?=: ncat --broker --listen -p 50905 -v # Ignore error in case it is already launched externally. +- $=: + - sleep 1 + - # Wait for the broker to start and get ready. + + - $=: yakut --verbose file-server --plug-and-play alloc.db --update-software $SOFTWARE_PACKAGE_DIR + uavcan.node.id: 32 + + - $=: yakut --format=json sub --with-metadata uavcan.node.Heartbeat.1.0 > heartbeats.json + - $=: yakut --format=json sub --with-metadata uavcan.diagnostic.Record.1.1 > diagnostics.json + + - $=: + - sleep 1 + - # Let the subscribers initialize to ensure we don't lose any diagnostic messages. + - ./bootloader + - # Wait for the bootloader to exit. It will only do that after it has successfully loaded and validated the app. + - exit 222 # A non-zero exit code will bring down the entire composition, which signifies the end of the test. + uavcan: + node: + hardware_version: [10, 30] + unique_id: aaaaaaaaaaaaaaaa + certificate_of_authenticity: this is a certificate + bootloader: + rom_file: a.rom + rom_size: 1024 + max_app_size: 512 + linger: false diff --git a/tests/integration/validator/validate.sh b/tests/integration/validator/validate.sh new file mode 100755 index 0000000..39b0681 --- /dev/null +++ b/tests/integration/validator/validate.sh @@ -0,0 +1,27 @@ +#!/usr/bin/env bash + +set -o nounset + +function die() +{ + echo >&2 "FAILURE: $*" + exit 1 +} + +export SOFTWARE_PACKAGE_DIR=$(dirname "$0") + +python -m pip --disable-pip-version-check install yakut~=0.6 || die "Failed to install Yakut" + +echo "TESTING: UAVCAN/serial" +yakut -v orchestrate "$SOFTWARE_PACKAGE_DIR/serial.orc.yaml" +result=$? +[[ $result == 222 ]] || die "Unexpected exit code: $result" +echo "Exit code $result is valid, test passed" + +echo "TESTING: UAVCAN/CAN" +yakut -v orchestrate "$SOFTWARE_PACKAGE_DIR/can.orc.yaml" +result=$? +[[ $result == 223 ]] || die "Unexpected exit code: $result" +echo "Exit code $result is valid, test passed" + +echo "PLEASE TEST v0 MANUALLY! RUN THIS: $SOFTWARE_PACKAGE_DIR/manual_v0.orc.yaml" diff --git a/tests/tools/.gitignore b/tests/tools/.gitignore new file mode 100644 index 0000000..76a2a56 --- /dev/null +++ b/tests/tools/.gitignore @@ -0,0 +1,2 @@ +# Test output files. +*.app*.bin diff --git a/tests/tools/demo-be.bin b/tests/tools/demo-be.bin new file mode 100644 index 0000000000000000000000000000000000000000..462cff8036cd47c514fb107859b831635b5e6c10 GIT binary patch literal 111 zcmeZB&B@7ED9&2 "FAILURE: $*" + exit 1 +} + +cd "${0%/*}" || die "Couldn't cd into this script's directory" + +SCRIPT=../../tools/kocherga_image.py + +$SCRIPT self-test || die "Self-test unsuccessful" +echo + +$SCRIPT demo-le.bin -vv --assign-vcs-revision-id=0xbadc_0ffe_e0dd_f00d --assign-flag-dirty=1 --assign-flag-release=0 +[ -f demo-le-1.16.badc0ffee0ddf00d.01a43c554cb3de13.app.dirty.bin ] || die "Output file has not been created" +echo + +$SCRIPT demo-le.bin -vv --assign-vcs-revision-id=0xbadc_0ffe_e0dd_f00d --assign-version=2.5 +[ -f demo-le-2.5.badc0ffee0ddf00d.ca47fd04aefc15b6.app.release.dirty.bin ] || die "Output file has not been created" +echo + +$SCRIPT nonexistent -vv && die "Expected failure" +echo + +$SCRIPT invalid.bin -vv && die "Expected failure" +echo + +rm ./*.app*.bin diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt new file mode 100644 index 0000000..aa4968a --- /dev/null +++ b/tests/unit/CMakeLists.txt @@ -0,0 +1,47 @@ +# This software is distributed under the terms of the MIT License. +# Copyright (c) 2020 Zubax Robotics. +# Author: Pavel Kirienko + +cmake_minimum_required(VERSION 3.19) +enable_testing() + +project(kocherga_unit_test CXX) + +set(library_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../kocherga") + +# C++ options +set(CXX_EXTENSIONS OFF) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wconversion -Wsign-promo") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wtype-limits -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") +# Attribute warning is useless in GCC: https://stackoverflow.com/questions/50646334 +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-attributes") + +include_directories(${library_dir}) +include_directories(../util) +include_directories(SYSTEM ../3rd_party) + +# Test targets +function(gen_test name files compile_definitions compile_flags link_flags) + add_executable(${name} ${files}) + target_compile_definitions(${name} PUBLIC ${compile_definitions}) + target_link_libraries(${name} pthread) + set_target_properties(${name} PROPERTIES COMPILE_FLAGS "${compile_flags}" LINK_FLAGS "${link_flags}") + + add_test("run_${name}" "${name}" --rng-seed time) + set_tests_properties("run_${name}" PROPERTIES ENVIRONMENT SOURCE_DIR=${CMAKE_CURRENT_SOURCE_DIR}) +endfunction() + +file(GLOB test_sources + ${CMAKE_CURRENT_SOURCE_DIR}/test_*.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/serial/test_*.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/can/test_*.cpp) +gen_test("test_x64" "${test_sources}" "" "-m64" "-m64") +gen_test("test_x32" "${test_sources}" "" "-m32" "-m32") +if ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_BUILD_TYPE STREQUAL "Debug")) + gen_test("test_cov" "${test_sources}" "" "-g -O0 --coverage" "--coverage") +endif () diff --git a/tests/unit/can/test_misc.cpp b/tests/unit/can/test_misc.cpp new file mode 100644 index 0000000..61c47d4 --- /dev/null +++ b/tests/unit/can/test_misc.cpp @@ -0,0 +1,759 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2021 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga_can.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "util.hpp" // NOLINT include order +#include "catch.hpp" +#include +#include +#include + +TEST_CASE("can::CRC") +{ + kocherga::can::detail::CRC16CCITT crc; + crc.update(3, reinterpret_cast("123")); + REQUIRE(0x5BCEU == crc.get()); + REQUIRE(crc.getBytes().at(0) == 0x5BU); + REQUIRE(crc.getBytes().at(1) == 0xCEU); + REQUIRE(!crc.isResidueCorrect()); + crc.update(0x5BU); + crc.update(0xCEU); + REQUIRE(crc.isResidueCorrect()); + REQUIRE(0 == crc.get()); +} + +TEST_CASE("can::BlockAllocator") +{ + kocherga::can::detail::BlockAllocator<8, 2> ba; + + static std::int64_t counter = 0; + + struct Foo final + { + const std::int64_t field; + + explicit Foo(const std::int64_t f) : field(f) { counter++; } + ~Foo() { counter--; } + + [[maybe_unused]] Foo(const Foo&) = delete; + [[maybe_unused]] Foo(Foo&&) = delete; + auto operator=(const Foo&) -> Foo& = delete; + auto operator=(Foo&&) -> Foo& = delete; + }; + + REQUIRE(counter == 0); + auto* a = ba.construct(1234567890); + REQUIRE(a); + REQUIRE(a->field == 1234567890); + REQUIRE(counter == 1); + auto* b = ba.construct(9876543210); + REQUIRE(b); + REQUIRE(b->field == 9876543210); + REQUIRE(counter == 2); + auto* c = ba.construct(55555); + REQUIRE(c == nullptr); + + ba.destroy(a); + REQUIRE(counter == 1); + c = ba.construct(1111111); + REQUIRE(counter == 2); + REQUIRE(c); + REQUIRE(c->field == 1111111); + + ba.destroy(b); + REQUIRE(counter == 1); + ba.destroy(c); + REQUIRE(counter == 0); +} + +TEST_CASE("can::parseFrame") +{ + using kocherga::can::detail::parseFrame; + using kocherga::can::detail::FrameModel; + using kocherga::can::detail::MessageFrameModel; + using kocherga::can::detail::ServiceFrameModel; + + static const auto parse = [](const std::uint32_t extended_can_id, const std::vector& payload) + -> std::optional> { + static std::vector payload_storage; + payload_storage = payload; + return parseFrame(extended_can_id, payload_storage.size(), payload_storage.data()); + }; + + static const auto cmp_payload = [](const FrameModel& fm, const std::vector& reference) { + return (fm.payload_size == reference.size()) && + (0 == std::memcmp(fm.payload, reference.data(), fm.payload_size)); + }; + // Message + { + const auto m = std::get(*parse(0U, {0, 1, 2, 3, 4, 5, 6, 7})); + REQUIRE(m.priority == 0); + REQUIRE(m.subject_id == 0); + REQUIRE(*m.source_node_id == 0); + REQUIRE(m.transfer_id == 7); + REQUIRE(!m.start_of_transfer); + REQUIRE(!m.end_of_transfer); + REQUIRE(!m.toggle); + REQUIRE(cmp_payload(m, {0, 1, 2, 3, 4, 5, 6})); + // Similar but invalid. + REQUIRE(!parse(0U, {})); // No tail byte. + REQUIRE(!parse(0U, {0})); // Multi-frame transfer frames require payload. + REQUIRE(!parse(0U, {0, 1, 2, 3, 4, 5, 6})); // Non-last frame of MFT cannot have fewer than 7 bytes of payload. + } + // Message + { + const auto m = std::get( + *parse(0b001'00'0'11'0110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + REQUIRE(m.priority == 1); + REQUIRE(m.subject_id == 0b0110011001100U); + REQUIRE(*m.source_node_id == 0b0100111U); + REQUIRE(m.transfer_id == 23); + REQUIRE(m.start_of_transfer); + REQUIRE(!m.end_of_transfer); + REQUIRE(m.toggle); + REQUIRE(cmp_payload(m, {0, 1, 2, 3, 4, 5, 6})); + // Similar but invalid: + // No tail byte + REQUIRE(!parse(0b001'00'0'11'0110011001100'0'0100111U, {})); + // Bad toggle (UAVCAN v0) + REQUIRE(!parse(0b001'00'0'11'0110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b100'00000U | 23U})); + // Bad reserved r07 + REQUIRE(!parse(0b001'00'0'11'0110011001100'1'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // Bad reserved r23 + REQUIRE(!parse(0b001'00'1'11'0110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // Bad reserved r07 r23 + REQUIRE(!parse(0b001'00'1'11'0110011001100'1'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // Anon transfer is not single frame + REQUIRE(!parse(0b001'01'0'11'0110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + } + // Anonymous message + { + { + const auto m = + std::get(*parse(0b010'01'0'00'0110011001101'0'0100111U, {0b111'00000U | 0U})); + REQUIRE(m.priority == 2); + REQUIRE(m.subject_id == 0b0110011001101U); + REQUIRE(!m.source_node_id); + REQUIRE(m.transfer_id == 0); + REQUIRE(m.start_of_transfer); + REQUIRE(m.end_of_transfer); + REQUIRE(m.toggle); + REQUIRE(cmp_payload(m, {})); + } + // SAME BUT RESERVED 21 22 SET (and ignored) + const auto m2 = + std::get(*parse(0b010'01'0'11'0110011001101'0'0100111U, {0b111'00000U | 0U})); + REQUIRE(m2.subject_id == 0b0110011001101U); // Yup, parsed correctly. + // Similar but invalid + REQUIRE(!parse(0b010'01'0'11'0110011001100'0'0100111U, {})); // No tail byte + REQUIRE(!parse(0b010'01'0'11'0110011001100'0'0100111U, {0b110'00000U | 0U})); // Bad toggle + REQUIRE(!parse(0b010'01'0'11'0110011001100'1'0100111U, {0b111'00000U | 0U})); // Bad reserved 07 + REQUIRE(!parse(0b010'01'1'11'0110011001100'0'0100111U, {0b111'00000U | 0U})); // Bad reserved 23 + REQUIRE(!parse(0b010'01'1'11'0110011001100'1'0100111U, {0b111'00000U | 0U})); // Bad reserved 07 23 + } + // Request + { + const auto model = + std::get(*parse(0b011'11'0000110011'0011010'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); + REQUIRE(model.priority == 3); + REQUIRE(model.service_id == 0b0000110011U); + REQUIRE(model.request_not_response); + REQUIRE(model.source_node_id == 0b0100111U); + REQUIRE(model.destination_node_id == 0b0011010U); + REQUIRE(model.transfer_id == 31U); + REQUIRE(!model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(model.toggle); + REQUIRE(cmp_payload(model, {0, 1, 2, 3})); + // Similar but invalid + REQUIRE(!parse(0b011'11'0000110011'0011010'0100111U, {})); // No tail byte + REQUIRE(!parse(0b011'11'0000110011'0011010'0100111U, {0, 1, 2, 3, 0b110'00000U | 31U})); // Bad toggle + REQUIRE(!parse(0b011'11'1000110011'0011010'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); // Bad reserved + REQUIRE(!parse(0b011'11'0000110011'0100111'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); // Src == destination + } + // Response + { + const auto model = + std::get(*parse(0b100'10'0000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); + REQUIRE(model.priority == 4); + REQUIRE(!model.request_not_response); + REQUIRE(model.service_id == 0b0000110011U); + REQUIRE(model.source_node_id == 0b0011010U); + REQUIRE(model.destination_node_id == 0b0100111U); + REQUIRE(model.transfer_id == 1U); + REQUIRE(!model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(!model.toggle); + REQUIRE(cmp_payload(model, {255})); + // Similar but invalid + REQUIRE(!parse(0b100'10'0000110011'0100111'0011010U, {})); // No tail byte + REQUIRE(!parse(0b100'10'0000110011'0100111'0011010U, {255, 0b100'00000U | 1U})); // Bad toggle + REQUIRE(!parse(0b100'10'1000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); // Bad reserved + REQUIRE(!parse(0b100'10'0000110011'0011010'0011010U, {255, 0b010'00000U | 1U})); // Src == destination + } +} + +TEST_CASE("can::parseFrameV0") +{ + using kocherga::can::detail::parseFrameV0; + using kocherga::can::detail::FrameModel; + using kocherga::can::detail::MessageFrameModel; + using kocherga::can::detail::ServiceFrameModel; + + static const auto parse = [](const std::uint32_t extended_can_id, const std::vector& payload) + -> std::optional> { + static std::vector payload_storage; + payload_storage = payload; + return parseFrameV0(extended_can_id, payload_storage.size(), payload_storage.data()); + }; + + static const auto cmp_payload = [](const FrameModel& fm, const std::vector& reference) { + return (fm.payload_size == reference.size()) && + (0 == std::memcmp(fm.payload, reference.data(), fm.payload_size)); + }; + // Message + { + const auto m = std::get(*parse(1U, {0, 1, 2, 3, 4, 5, 6, 7})); + REQUIRE(m.priority == 0); + REQUIRE(m.subject_id == 0); + REQUIRE(*m.source_node_id == 1); + REQUIRE(m.transfer_id == 7); + REQUIRE(!m.start_of_transfer); + REQUIRE(!m.end_of_transfer); + REQUIRE(!m.toggle); + REQUIRE(cmp_payload(m, {0, 1, 2, 3, 4, 5, 6})); + // Similar but invalid. + REQUIRE(!parse(0U, {})); // No tail byte. + } + // Message + { + const auto m = std::get( + *parse(0b00100'0110110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b100'00000U | 23U})); + REQUIRE(m.priority == 4); + REQUIRE(m.subject_id == 0b0110110011001100U); + REQUIRE(*m.source_node_id == 0b0100111U); + REQUIRE(m.transfer_id == 23); + REQUIRE(m.start_of_transfer); + REQUIRE(!m.end_of_transfer); + REQUIRE(!m.toggle); + REQUIRE(cmp_payload(m, {0, 1, 2, 3, 4, 5, 6})); + // Similar but invalid: + // No tail byte + REQUIRE(!parse(0b00100'0110110011001100'0'0100111U, {})); + // Bad toggle (UAVCAN v1) + REQUIRE(!parse(0b00100'0110110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // Anon transfer is not single frame + REQUIRE(!parse(0b00101'0110110011001100'0'0000000U, {0, 1, 2, 3, 4, 5, 6, 0b100'00000U | 23U})); + } + // Anonymous message + { + { + const auto m = + std::get(*parse(0b01001'0000110011001101'0'0000000U, {0b110'00000U | 0U})); + REQUIRE(m.priority == 0b01001); + REQUIRE(m.subject_id == 0b01U); + REQUIRE(!m.source_node_id); + REQUIRE(m.transfer_id == 0); + REQUIRE(m.start_of_transfer); + REQUIRE(m.end_of_transfer); + REQUIRE(!m.toggle); + REQUIRE(cmp_payload(m, {})); + } + // Similar but invalid + REQUIRE(!parse(0b01001'0110110011001100'0'0000000U, {})); // No tail byte + REQUIRE(!parse(0b01001'0110110011001100'0'0000000U, {0b111'00000U | 0U})); // Bad toggle + } + // Request + { + const auto model = std::get( + *parse(0b01111'00001100'1'1001101'1'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); + REQUIRE(model.priority == 0b01111); + REQUIRE(model.service_id == 0b00001100U); + REQUIRE(model.request_not_response); + REQUIRE(model.source_node_id == 0b0100111U); + REQUIRE(model.destination_node_id == 0b1001101U); + REQUIRE(model.transfer_id == 31U); + REQUIRE(!model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(model.toggle); + REQUIRE(cmp_payload(model, {0, 1, 2, 3})); + // Similar but invalid + REQUIRE(!parse(0b01111'00001100'1'1001101'1'0100111U, {})); // No tail byte + REQUIRE(!parse(0b01111'00001100'1'1001101'1'0100111U, {0, 1, 2, 3, 0b111'00000U | 31U})); // Bad toggle + REQUIRE(!parse(0b01111'00001100'1'0100111'1'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); // Src == destination + } + // Response + { + const auto model = + std::get(*parse(0b01111'00001100'0'1001101'1'0100111U, {255, 0b010'00000U | 1U})); + REQUIRE(model.priority == 0b01111U); + REQUIRE(!model.request_not_response); + REQUIRE(model.service_id == 0b00001100U); + REQUIRE(model.source_node_id == 0b0100111U); + REQUIRE(model.destination_node_id == 0b1001101U); + REQUIRE(model.transfer_id == 1U); + REQUIRE(!model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(!model.toggle); + REQUIRE(cmp_payload(model, {255})); + // Similar but invalid + REQUIRE(!parse(0b01111'00001100'0'1001101'1'0100111U, {})); // No tail byte + REQUIRE(!parse(0b01111'00001100'0'1001101'1'0100111U, {255, 0b101'00000U | 1U})); // Bad toggle + REQUIRE(!parse(0b01111'00001100'0'1001101'1'1001101U, {255, 0b010'00000U | 1U})); // Src == destination + } +} + +TEST_CASE("can::SimplifiedTransferReassembler") +{ + using kocherga::can::detail::SimplifiedTransferReassembler; + using kocherga::can::detail::SimplifiedMessageTransferReassembler; + using kocherga::can::detail::SimplifiedServiceTransferReassembler; + using kocherga::can::detail::MessageFrameModel; + using kocherga::can::detail::ServiceFrameModel; + using Buf = std::vector; + + const auto mk_msg = [](const std::optional source_node_id, + const std::uint8_t transfer_id, + const std::array& sot_eot_tog, + const std::vector& payload) -> MessageFrameModel { + static std::vector payload_storage; + payload_storage = payload; + MessageFrameModel out{}; + out.transfer_id = transfer_id; + out.source_node_id = source_node_id; + out.start_of_transfer = sot_eot_tog.at(0); + out.end_of_transfer = sot_eot_tog.at(1); + out.toggle = sot_eot_tog.at(2); + out.payload_size = payload_storage.size(); + out.payload = payload_storage.data(); + return out; + }; + + const auto mk_srv = [](std::uint8_t source_node_id, + std::uint8_t destination_node_id, + const bool request_not_response, + const std::uint8_t transfer_id, + const std::array& sot_eot_tog, + const std::vector& payload) -> ServiceFrameModel { + static std::vector payload_storage; + payload_storage = payload; + ServiceFrameModel out{}; + out.transfer_id = transfer_id; + out.source_node_id = source_node_id; + out.destination_node_id = destination_node_id; + out.request_not_response = request_not_response; + out.start_of_transfer = sot_eot_tog.at(0); + out.end_of_transfer = sot_eot_tog.at(1); + out.toggle = sot_eot_tog.at(2); + out.payload_size = payload_storage.size(); + out.payload = payload_storage.data(); + return out; + }; + + const auto check_result = [](const std::optional> result, + const Buf& reference) -> bool { + if (result) + { + const auto [sz, ptr] = *result; + return (sz == reference.size()) && (0 == std::memcmp(ptr, reference.data(), reference.size())); + } + return false; + }; + // Compute CRC using PyUAVCAN: + // >>> crc = pyuavcan.transport.commons.crc.CRC16CCITT + // >>> crc.new([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]).value_as_bytes + // Messages. + { + SimplifiedMessageTransferReassembler<10> rm; + // Anon + REQUIRE(check_result(rm.update(mk_msg({}, 0, {true, true, true}, {1, 2, 3})), {1, 2, 3})); + REQUIRE(check_result(rm.update(mk_msg({}, 0, {true, true, true}, {4})), {4})); + // SFT + REQUIRE(check_result(rm.update(mk_msg(123, 5, {true, true, true}, {4})), {4})); + // MFT with implicit truncation + REQUIRE(!rm.update(mk_msg(123, 6, {true, false, true}, {0, 1, 2, 3, 4, 5, 6}))); + REQUIRE(!rm.update(mk_msg(123, 6, {false, false, false}, {7, 8, 9, 10, 11, 12, 13}))); + REQUIRE(!rm.update(mk_msg(123, 6, {false, false, false}, {7, 8, 9, 10, 11, 12, 13}))); // Duplicate ignored + REQUIRE(!rm.update(mk_msg(124, 6, {false, true, true}, {14, 15, 59, 55}))); // Another src ignored + REQUIRE(!rm.update(mk_msg(123, 4, {false, true, true}, {14, 15, 59, 55}))); // Another tid ignored + REQUIRE(check_result(rm.update(mk_msg(123, 6, {false, true, true}, {14, 15, 59, 55})), + {0, 1, 2, 3, 4, 5, 6, 7, 8, 9})); + // MFT without implicit truncation + REQUIRE(!rm.update(mk_msg(123, 9, {true, false, true}, {0, 1, 2, 3, 4, 5, 6}))); + REQUIRE(!rm.update(mk_msg(124, 7, {true, false, true}, {0, 1, 2, 3, 4, 5, 6}))); // New SOT, discard old + REQUIRE(check_result(rm.update(mk_msg(124, 7, {false, true, false}, {7, 8, 9, 194, 65})), + {0, 1, 2, 3, 4, 5, 6, 7, 8, 9})); + // CRC error + REQUIRE(!rm.update(mk_msg(124, 8, {true, false, true}, {0, 1, 2, 3, 4, 5, 6}))); + REQUIRE(!rm.update(mk_msg(124, 8, {false, true, false}, {7, 8, 9, 0, 0}))); + } + + // Services. + { + SimplifiedServiceTransferReassembler<8> rs(9); + // Valid accepted + REQUIRE(check_result(rs.update(mk_srv(123, 9, true, 5, {true, true, true}, {0, 1, 2, 3, 4, 5, 6, 7, 8})), // + {0, 1, 2, 3, 4, 5, 6, 7})); + // Duplicate TID from same source, rejected + REQUIRE(!rs.update(mk_srv(123, 9, true, 5, {true, true, true}, {0, 1, 2, 3, 4, 5, 6, 7, 8}))); + // Duplicate TID from a different source, accepted + REQUIRE(check_result(rs.update(mk_srv(124, 9, true, 5, {true, true, true}, {0, 1, 2, 3, 4, 5, 6, 7, 8})), // + {0, 1, 2, 3, 4, 5, 6, 7})); + // Destination mismatch + REQUIRE(!rs.update(mk_srv(123, 8, true, 6, {true, true, true}, {0, 1, 2, 3, 4, 5, 6, 7, 8}))); + // Partial CRC spillover + REQUIRE(!rs.update(mk_srv(123, 9, false, 9, {true, false, true}, {0, 1, 2, 3, 4, 5}))); + REQUIRE(check_result(rs.update(mk_srv(123, 9, false, 9, {false, true, false}, {6, 40, 194})), + {0, 1, 2, 3, 4, 5, 6})); + } +} + +TEST_CASE("can::SimplifiedTransferReassemblerV0") +{ + using kocherga::can::detail::SimplifiedTransferReassemblerV0; + using kocherga::can::detail::SimplifiedMessageTransferReassemblerV0; + using kocherga::can::detail::SimplifiedServiceTransferReassemblerV0; + using kocherga::can::detail::MessageFrameModel; + using kocherga::can::detail::ServiceFrameModel; + using Buf = std::vector; + + const auto msg = [](const std::optional source_node_id, + const std::uint8_t transfer_id, + const std::array& sot_eot_tog, + const std::vector& payload) -> MessageFrameModel { + static std::vector payload_storage; + payload_storage = payload; + MessageFrameModel out{}; + out.transfer_id = transfer_id; + out.source_node_id = source_node_id; + out.start_of_transfer = sot_eot_tog.at(0); + out.end_of_transfer = sot_eot_tog.at(1); + out.toggle = sot_eot_tog.at(2); + out.payload_size = payload_storage.size(); + out.payload = payload_storage.data(); + return out; + }; + const auto srv = [](std::uint8_t source_node_id, + std::uint8_t destination_node_id, + const bool request_not_response, + const std::uint8_t transfer_id, + const std::array& sot_eot_tog, + const std::vector& payload) -> ServiceFrameModel { + static std::vector payload_storage; + payload_storage = payload; + ServiceFrameModel out{}; + out.transfer_id = transfer_id; + out.source_node_id = source_node_id; + out.destination_node_id = destination_node_id; + out.request_not_response = request_not_response; + out.start_of_transfer = sot_eot_tog.at(0); + out.end_of_transfer = sot_eot_tog.at(1); + out.toggle = sot_eot_tog.at(2); + out.payload_size = payload_storage.size(); + out.payload = payload_storage.data(); + return out; + }; + const auto check_result = [](const std::optional> result, + const Buf& reference) -> bool { + if (result) + { + const auto [sz, ptr] = *result; + return (sz == reference.size()) && (0 == std::memcmp(ptr, reference.data(), reference.size())); + } + return false; + }; + // Messages. + { + SimplifiedMessageTransferReassemblerV0<17> rm(0x0B2A812620A11D40ULL); + // Anon + REQUIRE(check_result(rm.update(msg({}, 0, {true, true, false}, {1, 2, 3})), {1, 2, 3})); + REQUIRE(check_result(rm.update(msg({}, 0, {true, true, false}, {4})), {4})); + // SFT + REQUIRE(check_result(rm.update(msg(123, 5, {true, true, false}, {4})), {4})); + // MFT + REQUIRE(!rm.update(msg(123, 6, {true, false, false}, {0x8C, 0x7A, 0xFA, 0x35, 0xFF, 0xD5, 0x05}))); + REQUIRE(!rm.update(msg(123, 6, {false, false, true}, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23}))); + REQUIRE(!rm.update(msg(123, 6, {false, false, true}, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23}))); + REQUIRE(!rm.update(msg(124, 6, {false, true, false}, {0x43, 0x00, 0x00, 0x00, 0x00}))); + REQUIRE(!rm.update(msg(123, 4, {false, true, false}, {0x43, 0x00, 0x00, 0x00, 0x00}))); + const Buf + ref{0xFA, 0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x43, 0x00, 0x00, 0x00, 0x00}; + REQUIRE(check_result(rm.update(msg(123, 6, {false, true, false}, {0x43, 0x00, 0x00, 0x00, 0x00})), ref)); + // MFT with excessive data, discarded (no truncation) + REQUIRE(!rm.update(msg(125, 7, {true, false, false}, {0, 1, 2, 3, 4, 5, 6}))); + REQUIRE(!rm.update(msg(125, 7, {false, false, true}, {7, 8, 9, 10, 11, 12, 13}))); + REQUIRE(!rm.update(msg(125, 7, {false, true, false}, {14, 15, 16, 17, 18, 19, 20}))); + // CRC error + REQUIRE(!rm.update(msg(124, 8, {true, false, false}, {0, 1, 2, 3, 4, 5, 6}))); + REQUIRE(!rm.update(msg(124, 8, {false, true, true}, {7, 8, 9, 0, 0}))); + } + // Services. + { + SimplifiedServiceTransferReassemblerV0<100> rs(0xEE468A8121C46A9EULL, 9); + // Valid accepted + REQUIRE(check_result(rs.update(srv(123, 9, true, 5, {true, true, false}, {0, 1, 2, 3, 4, 5, 6, 7, 8})), // + {0, 1, 2, 3, 4, 5, 6, 7, 8})); + // Duplicate TID from same source, rejected + REQUIRE(!rs.update(srv(123, 9, true, 5, {true, true, false}, {0, 1, 2, 3, 4, 5, 6, 7, 8}))); + // Duplicate TID from a different source, accepted + REQUIRE(check_result(rs.update(srv(124, 9, true, 5, {true, true, false}, {0, 1, 2, 3, 4, 5, 6, 7, 8})), // + {0, 1, 2, 3, 4, 5, 6, 7, 8})); + // Destination mismatch + REQUIRE(!rs.update(srv(123, 8, true, 6, {true, true, false}, {0, 1, 2, 3, 4, 5, 6, 7, 8}))); + // Multiframe -- a real GetNodeInfo response. + const Buf ref{0x04, 0x00, 0x00, 0x00, 0xD0, 0xC1, 0xFE, 0x00, 0x04, 0x03, 0x6E, 0xFF, 0x55, 0x8E, 0x0D, + 0xCE, 0x43, 0x9D, 0x90, 0x5E, 0xD9, 0xF4, 0x01, 0x02, 0x3C, 0x00, 0x1E, 0x00, 0x0D, 0x50, + 0x53, 0x37, 0x54, 0x31, 0x37, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x6F, 0x6D, 0x2E, + 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x74, 0x65, 0x6C, 0x65, 0x67, 0x61}; + REQUIRE(!rs.update(srv(123, 9, false, 9, {true, false, false}, {0xAC, 0x11, 0x04, 0x00, 0x00, 0x00, 0xD0}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, true}, {0xC1, 0xFE, 0x00, 0x04, 0x03, 0x6E, 0xFF}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, false}, {0x55, 0x8E, 0x0D, 0xCE, 0x43, 0x9D, 0x90}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, true}, {0x5E, 0xD9, 0xF4, 0x01, 0x02, 0x3C, 0x00}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, false}, {0x1E, 0x00, 0x0D, 0x50, 0x53, 0x37, 0x54}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, true}, {0x31, 0x37, 0x20, 0x00, 0x00, 0x00, 0x00}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, false}, {0x00, 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75}))); + REQUIRE(!rs.update(srv(123, 9, false, 9, {false, false, true}, {0x62, 0x61, 0x78, 0x2E, 0x74, 0x65, 0x6C}))); + REQUIRE(check_result(rs.update(srv(123, 9, false, 9, {false, true, false}, {0x65, 0x67, 0x61})), ref)); + } +} + +TEST_CASE("can::transmit") +{ + using kocherga::can::detail::transmit; + using Buf = std::vector; + using MultiBuf = std::vector; + const auto once = [](const std::size_t transport_layer_mtu, + const std::uint8_t transfer_id, + const Buf& payload) -> std::optional { + MultiBuf frags; + // + const auto push = [&frags](const std::size_t frag_length, const std::uint8_t* const frag_ptr) -> bool { + frags.emplace_back(frag_ptr, frag_ptr + frag_length); + return true; + }; + const bool result = transmit(push, transport_layer_mtu, transfer_id, payload.size(), payload.data()); + return result ? frags : std::optional{}; + }; + const std::vector dummy(1024); + // Bad arguments + { + REQUIRE(!once(7, 0, {})); // MTU is too low + REQUIRE(!once(13, 0, {})); // Bad DLC + REQUIRE(!once(63, 0, {})); // Bad DLC + REQUIRE(!once(65, 0, {})); // MTU is too high + REQUIRE(!once(64, 32, {})); + } + // Single-frame + { + // Without padding + REQUIRE(MultiBuf{Buf{0b1110'0000}} == *once(8, 0, {})); + REQUIRE(MultiBuf{Buf{0, 1, 2, 3, 4, 5, 6, 0b1110'1111}} == *once(8, 15, {0, 1, 2, 3, 4, 5, 6})); + REQUIRE(MultiBuf{Buf{0, 1, 2, 3, 4, 5, 6, 0b1110'1111}} == *once(32, 15, {0, 1, 2, 3, 4, 5, 6})); + REQUIRE(MultiBuf{Buf{0, 1, 2, 3, 4, 5, 6, 0b1110'1111}} == *once(64, 15, {0, 1, 2, 3, 4, 5, 6})); + // 25 bytes of payload, closest DLC is 32 bytes, plus one tail byte -> 6 bytes of padding + REQUIRE( + MultiBuf{Buf{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, + 16, 17, 18, 19, 20, 21, 22, 23, 24, 0, 0, 0, 0, 0, 0, 0b1110'1010}} == + *once(64, 10, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24})); + // Push error, abort early + REQUIRE(!transmit([](auto, auto) { return false; }, 64, 0, 0, dummy.data())); + } + // Multi-frame + // Compute CRC using PyUAVCAN: + // >>> crc = pyuavcan.transport.commons.crc.CRC16CCITT + // >>> list(crc.new([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]).value_as_bytes) + { + // Classic, no padding + REQUIRE(MultiBuf{ + Buf{0, 1, 2, 3, 4, 5, 6, 0b1010'1010}, // 7 bytes + Buf{7, 8, 9, 10, 11, 12, 13, 0b0000'1010}, // 7 bytes + Buf{14, 15, 16, 17, 18, 19, 20, 0b0010'1010}, // 7 bytes + Buf{21, 22, 23, 24, 9, 184, 0b0100'1010}, // 4 bytes + CRC + } == *once(8, 10, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, + 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24})); + REQUIRE(MultiBuf{ + Buf{0, 1, 2, 3, 4, 5, 6, 0b1010'1010}, // 7 bytes + Buf{7, 8, 9, 10, 11, 12, 13, 0b0000'1010}, // 7 bytes + Buf{14, 15, 16, 17, 18, 19, 20, 0b0010'1010}, // 7 bytes + Buf{221, 10, 0b0100'1010}, // CRC + } == *once(8, 10, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20})); + REQUIRE(MultiBuf{ + Buf{0, 1, 2, 3, 4, 5, 6, 0b1010'1010}, // 7 bytes + Buf{7, 8, 9, 10, 11, 12, 13, 0b0000'1010}, // 7 bytes + Buf{14, 15, 16, 17, 18, 19, 90, 0b0010'1010}, // 6 bytes + CRC[0] + Buf{116, 0b0100'1010}, // CRC[1] + } == *once(8, 10, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19})); + REQUIRE(MultiBuf{ + Buf{0, 1, 2, 3, 4, 5, 6, 0b1010'1010}, // 7 bytes + Buf{7, 8, 9, 10, 11, 12, 13, 0b0000'1010}, // 7 bytes + Buf{14, 15, 16, 17, 18, 232, 4, 0b0110'1010}, // 5 bytes + CRC + } == *once(8, 10, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18})); + // CAN FD with padding: 120 bytes of payload, 4 bytes padding at the end, then CRC which includes the padding. + REQUIRE(MultiBuf{ + Buf{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, + 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 0b1010'1010}, // + Buf{63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, + 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, + 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, + 111, 112, 113, 114, 115, 116, 117, 118, 119, 0, 0, 0, 0, 119, 210, 0b0100'1010}, // + } == + *once(64, 10, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, + 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, + 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, + 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, + 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, + 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, + 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119})); + // Error handling -- ensure that errors are not suppressed at any stage. + for (auto i = 0; i < 4; i++) + { + auto remaining = i; + REQUIRE(!transmit([&remaining](auto, auto) { return remaining-- > 0; }, 8, 0, 20, dummy.data())); + } + } +} + +TEST_CASE("can::transmitV0") +{ + using kocherga::can::detail::transmitV0; + using Buf = std::vector; + using MultiBuf = std::vector; + const auto once = [](const std::uint64_t signature, + const std::uint8_t transfer_id, + const Buf& payload) -> std::optional { + MultiBuf frags; + return transmitV0( + [&frags](const std::size_t frag_length, const std::uint8_t* const frag_ptr) -> bool { + frags.emplace_back(frag_ptr, frag_ptr + frag_length); + return true; + }, + signature, + transfer_id, + payload.size(), + payload.data()) + ? frags + : std::optional{}; + }; + const std::vector dummy(1024); + // Bad arguments + { + REQUIRE(!once(64, 32, {})); // Bad transfer-ID + } + // Single-frame + { + REQUIRE(MultiBuf{Buf{0b1100'0000}} == *once(8, 0, {})); + REQUIRE(MultiBuf{Buf{0, 1, 2, 3, 4, 5, 6, 0b1100'1111}} == *once(0, 15, {0, 1, 2, 3, 4, 5, 6})); + // Push error, abort early + REQUIRE(!transmitV0([](auto, auto) { return false; }, 0, 0, 0, dummy.data())); + } + // Multi-frame + { + // GetNodeInfo response + REQUIRE(MultiBuf{ + Buf{0xAC, 0x11, 0x04, 0x00, 0x00, 0x00, 0xD0, 0b1000'1010}, + Buf{0xC1, 0xFE, 0x00, 0x04, 0x03, 0x6E, 0xFF, 0b0010'1010}, + Buf{0x55, 0x8E, 0x0D, 0xCE, 0x43, 0x9D, 0x90, 0b0000'1010}, + Buf{0x5E, 0xD9, 0xF4, 0x01, 0x02, 0x3C, 0x00, 0b0010'1010}, + Buf{0x1E, 0x00, 0x0D, 0x50, 0x53, 0x37, 0x54, 0b0000'1010}, + Buf{0x31, 0x37, 0x20, 0x00, 0x00, 0x00, 0x00, 0b0010'1010}, + Buf{0x00, 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75, 0b0000'1010}, + Buf{0x62, 0x61, 0x78, 0x2E, 0x74, 0x65, 0x6C, 0b0010'1010}, + Buf{0x65, 0x67, 0x61, 0b0100'1010}, + } == *once(0xEE468A8121C46A9EULL, // Signature of GetNodeInfo + 10, + {0x04, 0x00, 0x00, 0x00, 0xD0, 0xC1, 0xFE, 0x00, 0x04, 0x03, 0x6E, 0xFF, 0x55, 0x8E, 0x0D, + 0xCE, 0x43, 0x9D, 0x90, 0x5E, 0xD9, 0xF4, 0x01, 0x02, 0x3C, 0x00, 0x1E, 0x00, 0x0D, 0x50, + 0x53, 0x37, 0x54, 0x31, 0x37, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x6F, 0x6D, 0x2E, + 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x74, 0x65, 0x6C, 0x65, 0x67, 0x61})); + // Error handling -- ensure that errors are not suppressed at any stage. + for (auto i = 0; i < 4; i++) + { + auto remaining = i; + REQUIRE(!transmitV0([&remaining](auto, auto) { return remaining-- > 0; }, 0, 0, 20, dummy.data())); + } + } +} + +TEST_CASE("CAN transfer roundtrip") +{ + using kocherga::can::detail::MessageFrameModel; + using kocherga::can::detail::SimplifiedMessageTransferReassembler; + using kocherga::can::detail::transmit; + using kocherga::can::detail::parseFrame; + using Buf = std::vector; + + static const auto synth_buf = []() -> Buf { + Buf out(util::getRandomInteger()); + for (auto& x : out) + { + x = util::getRandomInteger(); + } + return out; + }; + + // Generate many random transfers. + std::vector original_transfers(10); + for (auto& x : original_transfers) + { + x = synth_buf(); + } + + // Run the send/receive roundtrip loop + std::vector reassembled_transfers; + SimplifiedMessageTransferReassembler<0xFFFF> reassembler; + constexpr std::uint32_t extended_can_id = 0; + std::uint8_t transfer_id = 0; + for (const auto& in : original_transfers) + { + std::cout << "transfer=["; + for (auto x : in) + { + std::cout << static_cast(x) << ","; + } + std::cout << "]" << std::endl; + REQUIRE(transmit( + [&reassembler, &reassembled_transfers](const std::size_t size, const std::uint8_t* const data) -> bool { + MessageFrameModel fr{std::get(*parseFrame(extended_can_id, size, data))}; + std::cout << "frame " // + << (fr.start_of_transfer ? "S" : " ") // + << (fr.end_of_transfer ? "E" : " ") // + << (fr.toggle ? "T" : " ") // + << " " // + << "tid=" << unsigned(fr.transfer_id) // + << " ["; + for (auto i = 0U; i < fr.payload_size; i++) + { + std::cout << unsigned(fr.payload[i]) << ","; + } + std::cout << "]" << std::endl; + if (const auto result = reassembler.update(fr)) + { + const auto [rs, rb] = *result; + reassembled_transfers.emplace_back(rb, rb + rs); + std::cout << "RECEIVED TRANSFER OF " << rs << " BYTES" << std::endl; + } + if (util::getRandomInteger() > 128) // Introduce random duplicates + { + REQUIRE(!reassembler.update(fr)); // Ensure duplicates are always ignored. + } + return true; + }, + 64, + (transfer_id++) & 31U, + in.size(), + in.data())); + } + + // Ensure we received the same number of transfers. Can't compare the payloads directly because UAVCAN/CAN may add + // spurious zeroes at the end due to the low CAN FD DLC granularity. + REQUIRE(reassembled_transfers.size() == original_transfers.size()); + for (auto i = 0U; i < reassembled_transfers.size(); i++) + { + auto& org = original_transfers.at(i); + auto& res = reassembled_transfers.at(i); + REQUIRE(org.size() <= res.size()); + REQUIRE(std::equal(org.begin(), org.end(), res.begin())); + } +} diff --git a/tests/unit/can/test_node.cpp b/tests/unit/can/test_node.cpp new file mode 100644 index 0000000..b47a155 --- /dev/null +++ b/tests/unit/can/test_node.cpp @@ -0,0 +1,1175 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2021 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga_can.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "util.hpp" // NOLINT include order +#include "catch.hpp" +#include +#include +#include + +namespace +{ +using Bitrate = kocherga::can::ICANDriver::Bitrate; +using kocherga::can::CANAcceptanceFilterConfig; +using Allocator = kocherga::can::detail::BlockAllocator<1024, 2>; + +class CANDriverMock : public kocherga::can::ICANDriver +{ +public: + struct Config + { + Bitrate bitrate{}; + bool silent{}; + CANAcceptanceFilterConfig filter{}; + }; + + struct Frame + { + std::uint32_t extended_can_id{}; + std::vector payload; + }; + + struct TxFrame : public Frame + { + bool force_classic_can{}; + }; + + void setMode(const std::optional m) { mode_ = m; } + + [[nodiscard]] auto getConfig() const -> std::optional { return config_; } + + [[nodiscard]] auto popTx() -> std::optional + { + if (!tx_.empty()) + { + const TxFrame f = tx_.front(); + tx_.pop_front(); + return f; + } + return {}; + } + + void pushRx(const Frame& fr) { rx_.push_back(fr); } + + [[nodiscard]] auto configure(const Bitrate& bitrate, const bool silent, const CANAcceptanceFilterConfig& filter) + -> std::optional override + { + config_ = {bitrate, silent, filter}; + return mode_; + } + +private: + [[nodiscard]] auto push(const bool force_classic_can, + const std::uint32_t extended_can_id, + const std::uint8_t payload_size, + const void* const payload) -> bool override + { + if (!mode_) + { + return false; + } + TxFrame f{}; + f.force_classic_can = force_classic_can; + f.extended_can_id = extended_can_id; + std::copy_n(static_cast(payload), payload_size, std::back_insert_iterator(f.payload)); + tx_.emplace_back(f); + return true; + } + + [[nodiscard]] auto pop(PayloadBuffer& payload_buffer) + -> std::optional> override + { + if (mode_ && !rx_.empty()) + { + const Frame f = rx_.front(); + rx_.pop_front(); + std::copy(f.payload.begin(), f.payload.end(), payload_buffer.begin()); + return {{f.extended_can_id, f.payload.size()}}; + } + return {}; + } + + std::optional mode_; + std::optional config_; + std::deque tx_; + std::deque rx_; +}; + +class ReactorMock : public kocherga::IReactor +{ +public: + struct IncomingRequest + { + kocherga::PortID service_id = 0xFFFF; + std::uint8_t client_node_id{}; + std::vector data; + }; + + /// Accepts request, returns the serialized response payload or nothing if no response should be sent. + using IncomingRequestHandler = std::function>(const IncomingRequest&)>; + + void setIncomingRequestHandler(const IncomingRequestHandler& irh) { request_handler_ = irh; } + + [[nodiscard]] auto popPendingResponse() -> std::optional> + { + if (!pending_responses_.empty()) + { + const auto out = pending_responses_.front(); + pending_responses_.pop_front(); + return out; + } + return {}; + } + +private: + [[nodiscard]] auto processRequest(const kocherga::PortID service_id, + const kocherga::NodeID client_node_id, + const std::size_t request_length, + const std::uint8_t* const request, + std::uint8_t* const out_response) -> std::optional override + { + const auto client_node_id_uint8 = static_cast(client_node_id); + REQUIRE(client_node_id_uint8 == client_node_id); + IncomingRequest ir{service_id, client_node_id_uint8, {}}; + std::copy_n(request, request_length, std::back_insert_iterator(ir.data)); + REQUIRE(request_handler_); + if (const auto response_payload = request_handler_(ir)) + { + std::copy(response_payload->begin(), response_payload->end(), out_response); + return response_payload->size(); + } + return {}; + } + + void processResponse(const std::size_t response_length, const std::uint8_t* const response) override + { + pending_responses_.emplace_back(); + std::copy_n(response, response_length, std::back_insert_iterator(pending_responses_.back())); + } + + IncomingRequestHandler request_handler_; + std::deque> pending_responses_; +}; + +} // namespace + +TEST_CASE("can::detail::BitrateDetectionActivity") +{ + using kocherga::can::detail::IActivity; + using kocherga::can::detail::BitrateDetectionActivity; + using kocherga::can::detail::ProtocolVersionDetectionActivity; + Allocator alloc; + CANDriverMock driver; + ReactorMock reactor; + std::shared_ptr act; + + driver.setMode(CANDriverMock::Mode::FD); + + act = std::make_shared(alloc, driver, kocherga::SystemInfo::UniqueID{}); + REQUIRE(!driver.getConfig()); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(1'000))); + REQUIRE(driver.getConfig()->bitrate == Bitrate{1'000'000, 4'000'000}); + REQUIRE(driver.getConfig()->silent); + REQUIRE(driver.getConfig()->filter == CANAcceptanceFilterConfig{0, 0}); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(1'200'000))); + REQUIRE(driver.getConfig()->bitrate == Bitrate{500'000, 2'000'000}); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(2'400'000))); + REQUIRE(driver.getConfig()->bitrate == Bitrate{250'000, 1'000'000}); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(3'600'000))); + REQUIRE(driver.getConfig()->bitrate == Bitrate{125'000, 500'000}); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(4'800'000))); + REQUIRE(driver.getConfig()->bitrate == Bitrate{1'000'000, 4'000'000}); // Loop over! + driver.pushRx(CANDriverMock::Frame{123456, {}}); + IActivity* const proto_ver_act = act->poll(reactor, std::chrono::microseconds(5'000'000)); + REQUIRE(proto_ver_act); + REQUIRE(driver.getConfig()->bitrate == Bitrate{1'000'000, 4'000'000}); // This is the detected bitrate. + REQUIRE(driver.getConfig()->silent); + REQUIRE(driver.getConfig()->filter == CANAcceptanceFilterConfig{0, 0}); + REQUIRE(dynamic_cast(proto_ver_act)); +} + +TEST_CASE("can::detail::ProtocolVersionDetectionActivity") +{ + using kocherga::can::detail::IActivity; + using kocherga::can::detail::ProtocolVersionDetectionActivity; + using kocherga::can::detail::V0NodeIDAllocationActivity; + using kocherga::can::detail::V1NodeIDAllocationActivity; + Allocator alloc; + CANDriverMock driver; + ReactorMock reactor; + std::shared_ptr act; + driver.setMode(CANDriverMock::Mode::FD); + const Bitrate br{1'000'000, 4'000'000}; + // Detect v1 if both v0 and v1 are present at the same time. + { + REQUIRE(driver.configure(br, true, CANAcceptanceFilterConfig::makePromiscuous())); + act = std::make_shared(alloc, driver, kocherga::SystemInfo::UniqueID{}, br); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(1'000))); + // No matter how long we wait, if there are no valid frames, the protocol version detection will never end. + REQUIRE(!act->poll(reactor, std::chrono::microseconds(1'000'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(2'000'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(3'000'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(4'000'000))); + driver.pushRx(CANDriverMock::Frame{123456, {}}); // This is not an acceptable UAVCAN frame (either version). + REQUIRE(!act->poll(reactor, std::chrono::microseconds(5'000'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(6'000'000))); + driver.pushRx(CANDriverMock::Frame{123456, {0, 1, 2, 3, 0}}); // Not a start frame, no version info in it. + REQUIRE(!act->poll(reactor, std::chrono::microseconds(7'000'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(8'000'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(9'000'000))); + driver.pushRx(CANDriverMock::Frame{123456, {0, 1, 2, 3, 0b1000'0000}}); // UAVCAN/CAN v0 + driver.pushRx(CANDriverMock::Frame{123456, {0, 1, 2, 3, 0b1010'0000}}); // UAVCAN/CAN v1 + driver.pushRx(CANDriverMock::Frame{123456, {0, 1, 2, 3, 0b1000'0000}}); // UAVCAN/CAN v0 + driver.pushRx(CANDriverMock::Frame{123456, {0, 1, 2, 3, 0b1010'0000}}); // UAVCAN/CAN v1 + REQUIRE(!act->poll(reactor, std::chrono::microseconds(10'000'000))); // Detection timeout not yet expired + IActivity* const v1_pnp_act = act->poll(reactor, std::chrono::microseconds(12'000'000)); + REQUIRE(v1_pnp_act); + REQUIRE(dynamic_cast(v1_pnp_act)); + // The silent flag must be reset before invoking the next activity! + REQUIRE(driver.getConfig()->bitrate == br); + REQUIRE(!driver.getConfig()->silent); // Not silent! + REQUIRE(driver.getConfig()->filter == kocherga::can::detail::makeAcceptanceFilter<1>({})); // v1! + } + // Detect v0 if only v0 is present. + { + REQUIRE(driver.configure(br, true, CANAcceptanceFilterConfig::makePromiscuous())); + act = std::make_shared(alloc, driver, kocherga::SystemInfo::UniqueID{}, br); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(1'000))); + REQUIRE(!act->poll(reactor, std::chrono::microseconds(1'000'000))); + driver.pushRx(CANDriverMock::Frame{123456, {0, 1, 2, 3, 0b1000'0000}}); // UAVCAN/CAN v0 + REQUIRE(!act->poll(reactor, std::chrono::microseconds(2'000'000))); + IActivity* const v0_pnp_act = act->poll(reactor, std::chrono::microseconds(4'000'000)); + REQUIRE(v0_pnp_act); + REQUIRE(dynamic_cast(v0_pnp_act)); + // The silent flag must be reset before invoking the next activity! + REQUIRE(driver.getConfig()->bitrate == br); + REQUIRE(!driver.getConfig()->silent); // Not silent! + REQUIRE(driver.getConfig()->filter == kocherga::can::detail::makeAcceptanceFilter<0>({})); // v0! + } +} + +TEST_CASE("can::detail::V0NodeIDAllocationActivity") +{ + using kocherga::can::detail::IActivity; + using kocherga::can::detail::V0NodeIDAllocationActivity; + using kocherga::can::detail::V0MainActivity; + using std::chrono_literals::operator""us; + using Buf = std::vector; + + Allocator alloc; + CANDriverMock driver; + ReactorMock reactor; + driver.setMode(CANDriverMock::Mode::FD); + const Bitrate br{1'000'000, 4'000'000}; + const kocherga::SystemInfo::UniqueID uid{ + {0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x43, 0x00, 0x00, 0x00, 0x00}}; + + static const auto compute_pnp_request_can_id = [](const std::vector& frame_payload) -> std::uint32_t { + REQUIRE(!frame_payload.empty()); + REQUIRE(frame_payload.size() <= 8); + kocherga::can::detail::CRC16CCITT discriminator_crc; + discriminator_crc.update(frame_payload.size(), frame_payload.data()); + const std::uint32_t discriminator = discriminator_crc.get() & ((1UL << 14U) - 1U); + return 0x1E'0001'00UL | (discriminator << 10U); + }; + + // Successful allocation. + { + V0NodeIDAllocationActivity act(alloc, driver, uid, br); + REQUIRE(act.getDeadline().count() == 0); + REQUIRE(act.getStage() == 0); + // First poll -- set up the deadline with randomization. + REQUIRE(!act.poll(reactor, 1'000'000us)); + const auto deadline_a = act.getDeadline(); + REQUIRE(deadline_a >= 1'600'000us); + REQUIRE(deadline_a <= 2'000'000us); + REQUIRE(act.getStage() == 0); + REQUIRE(!act.poll(reactor, deadline_a - 1us)); // No change yet + REQUIRE(act.getDeadline() == deadline_a); + REQUIRE(act.getStage() == 0); + REQUIRE(!driver.popTx()); + // Deadline reached, state updated; first stage allocation request sent. + REQUIRE(!act.poll(reactor, deadline_a)); + auto fr = driver.popTx(); + REQUIRE(fr); + REQUIRE(!driver.popTx()); + REQUIRE(fr->force_classic_can); + Buf payload{0x01, 0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0b1100'0000U}; + REQUIRE(fr->extended_can_id == compute_pnp_request_can_id(payload)); + REQUIRE(fr->payload == payload); + // If we don't send a response, the allocatee will continue re-sending the first stage request forever. + const auto deadline_b = act.getDeadline(); + REQUIRE(deadline_b >= deadline_a + 600'000us); + REQUIRE(deadline_b <= deadline_a + 1'000'000us); + REQUIRE(!act.poll(reactor, deadline_b - 2us)); + REQUIRE(!act.poll(reactor, deadline_b - 1us)); + REQUIRE(!driver.popTx()); + REQUIRE(!act.poll(reactor, deadline_b)); + fr = driver.popTx(); + REQUIRE(fr); + REQUIRE(!driver.popTx()); + REQUIRE(fr->force_classic_can); + payload = {0x01, 0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0b1100'0001U}; + REQUIRE(fr->extended_can_id == compute_pnp_request_can_id(payload)); + REQUIRE(fr->payload == payload); + // Send 1st stage allocation response matching this UID, prompting the 2nd stage request. + driver.pushRx({0x14'0001'7FUL, {0x00, 0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0b1110'0000U}}); // UAVCAN v1 ignore + driver.pushRx({0x14'0001'7FUL, {0x00, 0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0b1100'0000U}}); // Valid accepted + REQUIRE(0 == act.getStage()); + REQUIRE(!act.poll(reactor, deadline_b + 1000us)); + REQUIRE(1 == act.getStage()); // It's a match! + const auto deadline_c = act.getDeadline(); + REQUIRE(deadline_c >= deadline_b + 1000us); + REQUIRE(deadline_c <= deadline_b + 401'000us); + REQUIRE(!act.poll(reactor, deadline_c - 1us)); + REQUIRE(!driver.popTx()); + REQUIRE(!act.poll(reactor, deadline_c)); + fr = driver.popTx(); + REQUIRE(fr); + REQUIRE(!driver.popTx()); + REQUIRE(fr->force_classic_can); + payload = {0x00, 0x31, 0x34, 0x61, 0x41, 0x23, 0x43, 0b1100'0010U}; + REQUIRE(fr->extended_can_id == compute_pnp_request_can_id(payload)); + REQUIRE(fr->payload == payload); + // After the 2nd stage request is sent, the schedule should be armed to send the 1st stage request on timeout. + REQUIRE(0 == act.getStage()); + const auto deadline_d = act.getDeadline(); + REQUIRE(deadline_d >= deadline_c + 600'000us); + REQUIRE(deadline_d <= deadline_c + 1'000'000us); + // Let the timeout expire and ensure that another 1st stage request is out. + REQUIRE(!act.poll(reactor, deadline_d)); + fr = driver.popTx(); + REQUIRE(fr); + REQUIRE(!driver.popTx()); + REQUIRE(fr->force_classic_can); + payload = {0x01, 0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0b1100'0011U}; + REQUIRE(fr->extended_can_id == compute_pnp_request_can_id(payload)); + REQUIRE(fr->payload == payload); + REQUIRE(0 == act.getStage()); + // Regardless of the current stage, reception of a matching UID sub-sequence will advance the process. + driver.pushRx({0x1E'01FD'FFUL, {0xC0}}); // Ignore service transfer (this is a GetNodeInfo request) + driver.pushRx({0x14'0001'7FUL, {0x9E, 0x3D, 0x00, 0x35, 0xFF, 0xD5, 0x05, 0x81}}); + driver.pushRx({0x14'0001'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x21}}); + driver.pushRx({0x14'0001'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x21}}); // Ignore duplicate + driver.pushRx({0x14'0002'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x21}}); // Ignore unrelated + driver.pushRx({0x14'0001'77UL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x21}}); // Ignore wrong sender + driver.pushRx({0x14'0001'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x22}}); // Ignore wrong transfer-ID + driver.pushRx({0x14'0001'7FUL, {0x43, 0x41}}); + driver.pushRx({0x14'0001'7FUL, {0x43, 0x41}}); // Ignore duplicate + REQUIRE(!act.poll(reactor, deadline_d)); + REQUIRE(!driver.popTx()); + REQUIRE(2 == act.getStage()); + const auto deadline_e = act.getDeadline(); + REQUIRE(deadline_e >= deadline_d + 0us); + REQUIRE(deadline_e <= deadline_d + 400'000us); + REQUIRE(!act.poll(reactor, deadline_e)); + REQUIRE(0 == act.getStage()); // Reset back to zero again after publication. + REQUIRE(act.getDeadline() >= deadline_e + 600'000us); + REQUIRE(act.getDeadline() <= deadline_e + 1'000'000us); + fr = driver.popTx(); + REQUIRE(fr); + REQUIRE(!driver.popTx()); + REQUIRE(fr->force_classic_can); + payload = {0x00, 0x00, 0x00, 0x00, 0x00, 0b1100'0100U}; // 3rd stage request + REQUIRE(fr->extended_can_id == compute_pnp_request_can_id(payload)); + REQUIRE(fr->payload == payload); + // Receive a matching response, which completes the process. + driver.pushRx({0x14'0001'7FUL, {0xFF, 0xFF, 0xEA, 0x35, 0xFF, 0xD5, 0x05, 0x82}}); // Bad CRC! Ignored. + driver.pushRx({0x14'0001'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x22}}); + driver.pushRx({0x14'0001'7FUL, {0x43, 0x00, 0x00, 0x00, 0x00, 0x42}}); + REQUIRE(!act.poll(reactor, deadline_e)); + REQUIRE(!driver.popTx()); + driver.pushRx({0x14'0001'7FUL, {0x57, 0x76, 0x00, 0x35, 0xFF, 0xD5, 0x05, 0x83}}); // Bad node-ID value! + driver.pushRx({0x14'0001'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x23}}); + driver.pushRx({0x14'0001'7FUL, {0x43, 0x00, 0x00, 0x00, 0x00, 0x43}}); + REQUIRE(!act.poll(reactor, deadline_e)); + REQUIRE(!driver.popTx()); + driver.pushRx({0x14'0001'7FUL, {0x8C, 0x7A, 0xFA, 0x35, 0xFF, 0xD5, 0x05, 0x84}}); // Correct CRC here. + driver.pushRx({0x14'0001'7FUL, {0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x24}}); + driver.pushRx({0x14'0001'7FUL, {0x43, 0x00, 0x00, 0x00, 0x00, 0x44}}); + auto* const new_act = act.poll(reactor, deadline_e); + REQUIRE(new_act != nullptr); + REQUIRE(!driver.popTx()); + REQUIRE(dynamic_cast(new_act)); + REQUIRE(dynamic_cast(new_act)->getLocalNodeID() == 125); + } +} + +TEST_CASE("can::detail::V1NodeIDAllocationActivity") +{ + using kocherga::SubjectID; + using kocherga::can::detail::IActivity; + using kocherga::can::detail::V1NodeIDAllocationActivity; + using kocherga::can::detail::V1MainActivity; + using std::chrono_literals::operator""us; + using Buf = std::vector; + + Allocator alloc; + CANDriverMock driver; + ReactorMock reactor; + const Bitrate br{1'000'000, 4'000'000}; + const kocherga::SystemInfo::UniqueID uid{ + {0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x43, 0x00, 0x00, 0x00, 0x00}}; + // The pseudo-UID is computed as CRC64WE of the UID using PyUAVCAN. + const std::array pseudo_uid_bytes{50, 191, 169, 46, 145, 226}; + + std::optional act; + std::chrono::microseconds uptime(0); + + const auto spin = [&reactor, &driver, &act, &uptime]() -> std::variant { + const auto uptime_increment = 1000us; + const auto deadline = uptime + 3'000'000us + uptime_increment; + while (uptime <= deadline) + { + uptime += uptime_increment; + if (auto* const res = act->poll(reactor, uptime)) + { + return res; + } + if (const auto f = driver.popTx()) + { + REQUIRE(!driver.popTx()); // Ensure there are no unexpected frames left. + return *f; + } + } + FAIL("TX FRAME NOT EMITTED BEFORE THE DEADLINE"); + return nullptr; // Unreachable + }; + + const auto ensure_subject_id = [](const std::uint32_t extended_can_id, const SubjectID sid) -> bool { + constexpr auto mask = 0b110'01'0110000000000000'0'1111111UL; + return (extended_can_id | 0x7FU) == (mask | (static_cast(sid) << 8U)); + }; + + // FD-capable bus; the allocator responds using v2 + { + driver.setMode(CANDriverMock::Mode::FD); + act.emplace(alloc, driver, uid, br, CANDriverMock::Mode::FD); + + // v2 is sent first because it is supposed to take precedence. + auto f = std::get(spin()); + REQUIRE(!f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v2)); + { + Buf ref(20, 0); // We only need 19 bytes; +1 is due to CAN FD DLC padding. + ref.at(0) = std::numeric_limits::max(); + ref.at(1) = std::numeric_limits::max(); + std::copy(uid.begin(), uid.end(), &ref.at(2)); + ref.back() = 0b1110'0000U; + REQUIRE(f.payload == ref); + } + + // v1 send afterwards + f = std::get(spin()); + REQUIRE(f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v1)); + { + Buf ref(8, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 0; + ref.at(7) = 0b1110'0000U; + REQUIRE(f.payload == ref); + } + REQUIRE(!driver.popTx()); // Ensure there are no unexpected frames left. + + // Wait for the next request pair to ensure the transfer-ID is being properly incremented and v1/v2 alternate. + f = std::get(spin()); + REQUIRE(!f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v2)); + { + Buf ref(20, 0); // We only need 19 bytes; +1 is due to CAN FD DLC padding. + ref.at(0) = std::numeric_limits::max(); + ref.at(1) = std::numeric_limits::max(); + std::copy(uid.begin(), uid.end(), &ref.at(2)); + ref.back() = 0b1110'0001U; // TID incremented! + REQUIRE(f.payload == ref); + } + f = std::get(spin()); + REQUIRE(f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v1)); + { + Buf ref(8, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 0; + ref.at(7) = 0b1110'0001U; + REQUIRE(f.payload == ref); + } + REQUIRE(!driver.popTx()); // Ensure there are no unexpected frames left. + + // Send v1 response with invalid node-ID to ensure it is ignored. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 1; + ref.at(7) = 0xFF; // Not a valid node-ID + ref.at(8) = 0xFF; // Second byte + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + // Send v1 response with mismatching UID to ensure it is ignored. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(5) = static_cast(~ref.at(5)); // Change one byte + ref.at(6) = 1; + ref.at(7) = 3; // LSB + ref.at(8) = 0; // MSB + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + // Send v1 response with bad array length to ensure it is ignored. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 2; // Shall be either 0 (if request) or 1 (if response) + ref.at(7) = 3; // LSB + ref.at(8) = 0; // MSB + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + // Send malformed v1 response to ensure it is ignored. + { + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, {0b1111'1111U}}); + } + // Send v2 response with invalid node-ID to ensure it is ignored. + { + Buf ref(20, 0); // We only need 19 bytes; +1 is due to CAN FD DLC padding. + ref.at(0) = 0xAA; + ref.at(1) = 0xAA; + std::copy(uid.begin(), uid.end(), &ref.at(2)); + ref.back() = 0b1111'1111U; + driver.pushRx({0b110'00'0111111111100101'0'1111110UL, ref}); + } + // Send v2 response with mismatching UID to ensure it is ignored. + { + Buf ref(20, 0); // We only need 19 bytes; +1 is due to CAN FD DLC padding. + ref.at(0) = 3; + ref.at(1) = 0; + std::copy(uid.begin(), uid.end(), &ref.at(2)); + ref.at(17) = static_cast(~ref.at(17)); // Change one byte + ref.back() = 0b1111'1111U; + driver.pushRx({0b110'00'0111111111100101'0'1111110UL, ref}); + } + // Send malformed v2 response to ensure it is ignored. + { + driver.pushRx({0b110'00'0111111111100101'0'1111110UL, {0b1111'1111U}}); + } + // Yup, we simply get the next request -- the bad responses are ignored. + f = std::get(spin()); + REQUIRE(!f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v2)); + // Send the correct v2 response and ensure it is accepted. + { + Buf ref(20, 0); // We only need 19 bytes; +1 is due to CAN FD DLC padding. + ref.at(0) = 7; + ref.at(1) = 0; + std::copy(uid.begin(), uid.end(), &ref.at(2)); + ref.back() = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100101'0'1111110UL, ref}); + } + const auto result = spin(); + REQUIRE(std::holds_alternative(result)); + const auto* const main_act = dynamic_cast(std::get(result)); + REQUIRE(main_act); + REQUIRE(main_act->getLocalNodeID() == 7); // Ensure the allocated value is extracted correctly. + } + + // Classic CAN bus + { + driver.setMode(CANDriverMock::Mode::Classic); + act.emplace(alloc, driver, uid, br, CANDriverMock::Mode::Classic); + + auto f = std::get(spin()); + REQUIRE(f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v1)); + { + Buf ref(8, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 0; + ref.at(7) = 0b1110'0000U; + REQUIRE(f.payload == ref); + } + REQUIRE(!driver.popTx()); // Ensure there are no unexpected frames left. + + // Wait for the next request to ensure the transfer-ID is being properly incremented. + f = std::get(spin()); + REQUIRE(f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v1)); + { + Buf ref(8, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 0; + ref.at(7) = 0b1110'0001U; // Incremented! + REQUIRE(f.payload == ref); + } + REQUIRE(!driver.popTx()); // Ensure there are no unexpected frames left. + + // Send v1 response with invalid node-ID to ensure it is ignored. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 1; + ref.at(7) = 0xFF; // Not a valid node-ID + ref.at(8) = 0xFF; // Second byte + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + // Send v1 response with mismatching UID to ensure it is ignored. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(5) = static_cast(~ref.at(5)); // Change one byte + ref.at(6) = 1; + ref.at(7) = 3; // LSB + ref.at(8) = 0; // MSB + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + // Send v1 response with bad array length to ensure it is ignored. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 2; // Shall be either 0 (if request) or 1 (if response) + ref.at(7) = 3; // LSB + ref.at(8) = 0; // MSB + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + // Send malformed v1 response to ensure it is ignored. + { + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, {0b1111'1111U}}); + } + // Yup, we simply get the next request -- the bad responses are ignored. + f = std::get(spin()); + REQUIRE(f.force_classic_can); + REQUIRE(ensure_subject_id(f.extended_can_id, SubjectID::PnPNodeIDAllocationData_v1)); + // Send the correct v1 response and ensure it is accepted. + { + Buf ref(10, 0); + std::copy(pseudo_uid_bytes.begin(), pseudo_uid_bytes.end(), ref.begin()); + ref.at(6) = 1; + ref.at(7) = 15; // LSB + ref.at(8) = 0; // MSB + ref.at(9) = 0b1110'0000U; + driver.pushRx({0b110'00'0111111111100110'0'1111110UL, ref}); + } + const auto result = spin(); + REQUIRE(std::holds_alternative(result)); + const auto* const main_act = dynamic_cast(std::get(result)); + REQUIRE(main_act); + REQUIRE(main_act->getLocalNodeID() == 15); // Ensure the allocated value is extracted correctly. + } +} + +TEST_CASE("can::CANNode v1") +{ + using kocherga::SubjectID; + using kocherga::ServiceID; + using kocherga::can::CANNode; + using std::chrono_literals::operator""us; + using Buf = std::vector; + + CANDriverMock driver; + ReactorMock reactor; + std::chrono::microseconds uptime(0); + + const Bitrate br{1'000'000, 4'000'000}; + const kocherga::SystemInfo::UniqueID uid{ + {0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x43, 0x00, 0x00, 0x00, 0x00}}; + + driver.setMode(kocherga::can::ICANDriver::Mode::FD); + CANNode node(driver, uid, br, 1, 123); + + const auto poll = [&reactor, &driver, &node, &uptime](const std::chrono::microseconds uptime_delta, + const std::optional& rx_frame = {}) + -> std::optional { + if (rx_frame) + { + driver.pushRx(*rx_frame); + } + static_cast(node).poll(reactor, uptime); + uptime += uptime_delta; + if (const auto f = driver.popTx()) + { + return *f; + } + return {}; + }; + + // Run a few dry polls, kick the wheels. + REQUIRE(!poll(1000us)); + REQUIRE(!poll(1000us)); + REQUIRE(!poll(1000us)); + REQUIRE(uptime == 3000us); + REQUIRE(!reactor.popPendingResponse()); + + // Check service requests. + reactor.setIncomingRequestHandler([](const ReactorMock::IncomingRequest& req) -> std::optional { + REQUIRE(req.client_node_id == 42); + switch (req.service_id) + { + case static_cast(ServiceID::NodeGetInfo): + { + return Buf{1, 2, 3}; + } + default: + { + FAIL("Unexpected service request"); + break; + } + } + return {}; + }); + // Send GetInfo request. + auto tx_frame = poll(1000us, + CANDriverMock::Frame{ + (0b110'11'0000000000'0000000'0000000UL | // Request + (static_cast(ServiceID::NodeGetInfo) << 14U) | // Service-ID + (static_cast(123) << 7U) | // Destination node-ID + (static_cast(42) << 0U)), // Source node-ID + { + 0b1110'0000U, // Tail byte + }, + }); + REQUIRE(tx_frame); + REQUIRE(!tx_frame->force_classic_can); + REQUIRE(tx_frame->payload == Buf{1, 2, 3, 0b1110'0000U}); + + // Do a request, send response back. + REQUIRE(static_cast(node) + .sendRequest(ServiceID::FileRead, 42, 15, 3, reinterpret_cast("\x04\x05\x06"))); + tx_frame = poll(1000us); + REQUIRE(tx_frame); + REQUIRE(!tx_frame->force_classic_can); + REQUIRE(tx_frame->payload == Buf{4, 5, 6, 0b1110'1111U}); + REQUIRE(!poll(1000us)); + REQUIRE(!reactor.popPendingResponse()); + REQUIRE(!poll(1000us, + CANDriverMock::Frame{ + (0b110'10'0000000000'0000000'0000000UL | // Response + (static_cast(ServiceID::FileRead) << 14U) | // Service-ID + (static_cast(123) << 7U) | // Destination node-ID + (static_cast(42) << 0U)), // Source node-ID + { + 3, 2, 1, 0b1110'1111U, // Tail byte at the end + }, + })); + auto resp = reactor.popPendingResponse(); + REQUIRE(resp); + REQUIRE(*resp == Buf{3, 2, 1}); + REQUIRE(!reactor.popPendingResponse()); + + // Same, but cancel the pending request midway, ensure response is ignored. + REQUIRE(static_cast(node) + .sendRequest(ServiceID::FileRead, 42, 31, 3, reinterpret_cast("\x04\x05\x06"))); + tx_frame = poll(1000us); + REQUIRE(tx_frame); + REQUIRE(!tx_frame->force_classic_can); + REQUIRE(tx_frame->payload == Buf{4, 5, 6, 0b1111'1111U}); + REQUIRE(!poll(1000us)); + REQUIRE(!reactor.popPendingResponse()); + static_cast(node).cancelRequest(); + REQUIRE(!poll(1000us, + CANDriverMock::Frame{ + (0b110'10'0000000000'0000000'0000000UL | // Response + (static_cast(ServiceID::FileRead) << 14U) | // Service-ID + (static_cast(123) << 7U) | // Destination node-ID + (static_cast(42) << 0U)), // Source node-ID + { + 3, 2, 1, 0b1111'1111U, // Tail byte at the end + }, + })); + REQUIRE(!reactor.popPendingResponse()); + + // Publish a message. + REQUIRE(static_cast(node).publishMessage(SubjectID::DiagnosticRecord, + 7, + 3, + reinterpret_cast("\x07\x08\x09"))); + tx_frame = poll(1000us); + REQUIRE(tx_frame); + REQUIRE(!tx_frame->force_classic_can); + REQUIRE(tx_frame->payload == Buf{7, 8, 9, 0b1110'0111U}); + REQUIRE(!poll(1000us)); +} + +TEST_CASE("can::CANNode v0") +{ + using kocherga::PortID; + using kocherga::SubjectID; + using kocherga::ServiceID; + using kocherga::can::CANNode; + using kocherga::can::detail::SimplifiedTransferReassemblerV0; + using kocherga::can::detail::SimplifiedMessageTransferReassemblerV0; + using kocherga::can::detail::SimplifiedServiceTransferReassemblerV0; + using kocherga::can::detail::MessageFrameModel; + using kocherga::can::detail::ServiceFrameModel; + using std::chrono_literals::operator""us; + using Buf = std::vector; + + CANDriverMock driver; + ReactorMock reactor; + std::chrono::microseconds uptime(0); + + const Bitrate br{1'000'000, 4'000'000}; + const kocherga::SystemInfo::UniqueID uid{ + {0x35, 0xFF, 0xD5, 0x05, 0x50, 0x59, 0x31, 0x34, 0x61, 0x41, 0x23, 0x43, 0x00, 0x00, 0x00, 0x00}}; + + driver.setMode(kocherga::can::ICANDriver::Mode::FD); + CANNode node(driver, uid, br, 0, 123); + + struct Transfer + { + std::uint8_t priority = std::numeric_limits::max(); + std::uint8_t transfer_id = std::numeric_limits::max(); + Buf payload; + + Transfer() = default; + Transfer(std::uint8_t priority, std::uint8_t transfer_id, const Buf& payload) : + priority(priority), transfer_id(transfer_id), payload(payload) + {} + + [[nodiscard]] virtual auto makeCANID() const -> std::uint32_t = 0; + + virtual ~Transfer() = default; + [[maybe_unused]] Transfer(const Transfer&) = delete; + [[maybe_unused]] Transfer(Transfer&&) = delete; + auto operator=(const Transfer&) -> Transfer& = delete; + auto operator=(Transfer&&) -> Transfer& = delete; + }; + + struct MessageTransfer : public Transfer + { + PortID subject_id = std::numeric_limits::max(); + std::optional source_node_id; + + MessageTransfer() = default; + MessageTransfer(std::uint8_t priority, + std::uint8_t transfer_id, + PortID subject_id, + std::optional source_node_id, + const Buf& payload) : + Transfer(priority, transfer_id, payload), subject_id(subject_id), source_node_id(source_node_id) + {} + + [[nodiscard]] auto makeCANID() const -> std::uint32_t override + { + if (source_node_id) + { + return (static_cast(priority) << 24U) | (static_cast(subject_id) << 8U) | + *source_node_id; + } + FAIL("Anonymous transfers not supported in this implementation"); + return 0; + } + }; + + struct ServiceTransfer : public Transfer + { + PortID service_id = std::numeric_limits::max(); + std::uint8_t source_node_id = std::numeric_limits::max(); + std::uint8_t destination_node_id = std::numeric_limits::max(); + bool request_not_response = false; + + ServiceTransfer() = default; + ServiceTransfer(std::uint8_t priority, + std::uint8_t transfer_id, + PortID service_id, + std::uint8_t source_node_id, + std::uint8_t destination_node_id, + const bool request_not_response, + const Buf& payload) : + Transfer(priority, transfer_id, payload), + service_id(service_id), + source_node_id(source_node_id), + destination_node_id(destination_node_id), + request_not_response(request_not_response) + {} + + [[nodiscard]] auto makeCANID() const -> std::uint32_t override + { + return (static_cast(priority) << 24U) | // Priority + (static_cast(service_id) << 16U) | // Service-ID + static_cast(request_not_response ? (1UL << 15U) : 0U) | // Request not response + static_cast(1UL << 7U) | // Service not message + (static_cast(destination_node_id) << 8U) | // To + static_cast(source_node_id); // From + } + }; + + std::unordered_map>> ra_msg; + ra_msg[341] = std::make_shared>(0x0F0868D0C1A7C6F1ULL); + ra_msg[16383] = std::make_shared>(0XD654A48E0C049D75ULL); + std::unordered_map>> ra_req; + ra_req[48] = std::make_shared>(0x8DCDCA939F33F678ULL, 42); + std::unordered_map>> ra_res; + ra_res[1] = std::make_shared>(0xEE468A8121C46A9EULL, 42); + ra_res[40] = std::make_shared>(0xB7D725DF72724126ULL, 42); + + using SignatureTransferPair = std::pair>; + // At the network layer, we are dealing with v0-translated transfers, not v1. + const auto poll = [&](const std::chrono::microseconds uptime_delta, + const std::optional& rx_transfer = {}) -> std::shared_ptr { + if (rx_transfer) + { + const auto [sig, tr] = *rx_transfer; + const std::uint32_t can_id = tr->makeCANID(); + REQUIRE(kocherga::can::detail::transmitV0( + [&driver, can_id](const std::size_t frame_payload_size, const std::uint8_t* const frame_payload) { + std::cout << "RX " << std::hex << can_id << std::dec << " " + << util::makeHexDump(frame_payload, frame_payload + frame_payload_size) << std::endl; + driver.pushRx(CANDriverMock::Frame{can_id, Buf{frame_payload, frame_payload + frame_payload_size}}); + return true; + }, + sig, + tr->transfer_id, + tr->payload.size(), + tr->payload.data())); + } + static_cast(node).poll(reactor, uptime); + uptime += uptime_delta; + while (const auto raw_frame = driver.popTx()) + { + std::cout << "TX " << std::hex << raw_frame->extended_can_id << std::dec << " " + << util::makeHexDump(raw_frame->payload) << std::endl; + REQUIRE(raw_frame->force_classic_can); // v0 shall use Classic only. + const auto frame = kocherga::can::detail::parseFrameV0(raw_frame->extended_can_id, + raw_frame->payload.size(), + raw_frame->payload.data()) + .value(); + if (const auto* const f_m = std::get_if(&frame)) + { + if (const auto res = ra_msg.at(f_m->subject_id)->update(*f_m)) + { + return std::make_shared(f_m->priority, + f_m->transfer_id, + f_m->subject_id, + f_m->source_node_id, + Buf(res->second, res->second + res->first)); + } + } + else if (const auto* const f_s = std::get_if(&frame)) + { + auto& ra = f_s->request_not_response ? ra_req : ra_res; + if (const auto res = ra.at(f_s->service_id)->update(*f_s)) + { + return std::make_shared(f_s->priority, + f_s->transfer_id, + f_s->service_id, + f_s->source_node_id, + f_s->destination_node_id, + f_s->request_not_response, + Buf(res->second, res->second + res->first)); + } + } + else + { + throw std::logic_error("Unexpected option"); + } + } + return {}; + }; + + REQUIRE(!poll(1000us)); + REQUIRE(!poll(1000us)); + REQUIRE(!poll(1000us)); + REQUIRE(uptime == 3000us); + + // Can't publish unknown messages. + REQUIRE(!static_cast(node).publishMessage(SubjectID::PnPNodeIDAllocationData_v2, 0, 0, nullptr)); + + // Publish heartbeat, check translation. + REQUIRE(static_cast(node).publishMessage( // + SubjectID::NodeHeartbeat, + 5, + 7, + reinterpret_cast("\x03\x00\x00\x00\x03\x03\x00"))); + auto tx = poll(1000us); + REQUIRE(tx); + REQUIRE(tx->transfer_id == 5); + REQUIRE(tx->priority == 16); + REQUIRE(tx->payload == Buf{0x03, 0x00, 0x00, 0x00, 0xd8, 0x00, 0x00}); // This is v0 format. + REQUIRE(dynamic_cast(*tx).subject_id == 341); // v0 NodeStatus, not v1 Heartbeat + REQUIRE(*dynamic_cast(*tx).source_node_id == 123); + + // Publish log message, check translation. + REQUIRE( + static_cast(node).publishMessage(SubjectID::DiagnosticRecord, + 14, + 23, + reinterpret_cast( + "\x00\x00\x00\x00\x00\x00\x00\x03\x0EUpdate started"))); + tx = poll(1000us); + REQUIRE(tx); + REQUIRE(tx->transfer_id == 14); + REQUIRE(tx->priority == 31); + REQUIRE( + tx->payload == + Buf{0b010'00100U, 'B', 'o', 'o', 't', 'U', 'p', 'd', 'a', 't', 'e', ' ', 's', 't', 'a', 'r', 't', 'e', 'd'}); + REQUIRE(dynamic_cast(*tx).subject_id == 16383); + REQUIRE(*dynamic_cast(*tx).source_node_id == 123); + + // Check service calls. + REQUIRE(!reactor.popPendingResponse()); + reactor.setIncomingRequestHandler([](const ReactorMock::IncomingRequest& req) -> std::optional { + REQUIRE(req.client_node_id == 42); + switch (req.service_id) + { + case static_cast(ServiceID::NodeGetInfo): + { + return Buf{ + 0x01, 0x00, // Protocol version + 0x0A, 0x1E, // Hardware version + 0x07, 0x99, // Software version + 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0, // Software VCS revision ID + 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, // UID + 0x23, // Name length: "com.zubax.kocherga.test.integration" + 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x6B, 0x6F, 0x63, 0x68, 0x65, 0x72, + 0x67, 0x61, 0x2E, 0x74, 0x65, 0x73, 0x74, 0x2E, 0x69, 0x6E, 0x74, 0x65, 0x67, 0x72, 0x61, 0x74, + 0x69, 0x6F, 0x6E, // End of name + 0x00, // No image CRC + 0x15, // CoA length, then the CoA itself + 0x74, 0x68, 0x69, 0x73, 0x20, 0x69, 0x73, 0x20, 0x61, 0x20, 0x63, 0x65, 0x72, 0x74, 0x69, 0x66, + 0x69, 0x63, 0x61, 0x74, 0x65, + }; + } + case static_cast(ServiceID::NodeExecuteCommand): + { + REQUIRE(req.data == Buf{ + 0xFD, 0xFF, // Command == COMMAND_BEGIN_SOFTWARE_UPDATE + 0x5C, // Path length + 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x6B, 0x6F, 0x63, 0x68, + 0x65, 0x72, 0x67, 0x61, 0x2E, 0x74, 0x65, 0x73, 0x74, 0x2E, 0x69, 0x6E, 0x74, 0x65, + 0x67, 0x72, 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x2D, 0x31, 0x30, 0x2D, 0x33, 0x2E, 0x31, + 0x2E, 0x62, 0x61, 0x64, 0x63, 0x30, 0x66, 0x66, 0x65, 0x65, 0x30, 0x64, 0x64, 0x66, + 0x30, 0x30, 0x64, 0x2E, 0x34, 0x35, 0x32, 0x61, 0x34, 0x32, 0x36, 0x37, 0x39, 0x37, + 0x31, 0x61, 0x33, 0x39, 0x32, 0x38, 0x2E, 0x61, 0x70, 0x70, 0x2E, 0x72, 0x65, 0x6C, + 0x65, 0x61, 0x73, 0x65, 0x2E, 0x62, 0x69, 0x6E, + }); + return Buf{1}; // FAILURE will be translated into ERROR_UNKNOWN. + } + default: + { + FAIL("Unexpected service request"); + break; + } + } + return {}; + }); + + // Validate GetInfo -- ensure the service is operational and v1/v0 translation is correct. + tx = poll(1000us, + SignatureTransferPair{0xEE468A8121C46A9EULL, + std::make_shared(4, 15, 1, 42, 123, true, Buf{})}); + REQUIRE(tx); + REQUIRE(tx->priority == 4); + REQUIRE(tx->transfer_id == 15); + const Buf translated_node_info{ + 0x03, 0x00, 0x00, 0x00, 0xd8, 0x00, 0x00, // NodeStatus + 0x07, 0x99, // Software version major.minor + 0x00, // Optional field flags (none available) + 0x00, 0x00, 0x00, 0x00, // VCS commit + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Image CRC + 0x0a, 0x1e, // Hardware version major minor + 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, // UID + 0x00, // CoA is not translated for simplicity; the next field is the name + 0x63, 0x6f, 0x6d, 0x2e, 0x7a, 0x75, 0x62, 0x61, 0x78, 0x2e, 0x6b, 0x6f, 0x63, 0x68, 0x65, 0x72, 0x67, 0x61, + 0x2e, 0x74, 0x65, 0x73, 0x74, 0x2e, 0x69, 0x6e, 0x74, 0x65, 0x67, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, + }; + REQUIRE(tx->payload == translated_node_info); + REQUIRE(dynamic_cast(*tx).service_id == 1); + REQUIRE(dynamic_cast(*tx).source_node_id == 123); + REQUIRE(dynamic_cast(*tx).destination_node_id == 42); + REQUIRE(!dynamic_cast(*tx).request_not_response); + + // Validate firmware update request. + const Buf update_request_v0{ + 0x00, // Source node-ID unset, meaning that the file should be downloaded from the caller. + 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x6B, 0x6F, 0x63, 0x68, 0x65, 0x72, + 0x67, 0x61, 0x2E, 0x74, 0x65, 0x73, 0x74, 0x2E, 0x69, 0x6E, 0x74, 0x65, 0x67, 0x72, 0x61, 0x74, + 0x69, 0x6F, 0x6E, 0x2D, 0x31, 0x30, 0x2D, 0x33, 0x2E, 0x31, 0x2E, 0x62, 0x61, 0x64, 0x63, 0x30, + 0x66, 0x66, 0x65, 0x65, 0x30, 0x64, 0x64, 0x66, 0x30, 0x30, 0x64, 0x2E, 0x34, 0x35, 0x32, 0x61, + 0x34, 0x32, 0x36, 0x37, 0x39, 0x37, 0x31, 0x61, 0x33, 0x39, 0x32, 0x38, 0x2E, 0x61, 0x70, 0x70, + 0x2E, 0x72, 0x65, 0x6C, 0x65, 0x61, 0x73, 0x65, 0x2E, 0x62, 0x69, 0x6E, + }; + tx = poll(1000us, + SignatureTransferPair{0xB7D725DF72724126ULL, + std::make_shared(31, 31, 40, 42, 123, true, update_request_v0)}); + REQUIRE(tx); + REQUIRE(tx->priority == 31); + REQUIRE(tx->transfer_id == 31); + REQUIRE(tx->payload == Buf{255}); + REQUIRE(dynamic_cast(*tx).service_id == 40); + REQUIRE(dynamic_cast(*tx).source_node_id == 123); + REQUIRE(dynamic_cast(*tx).destination_node_id == 42); + REQUIRE(!dynamic_cast(*tx).request_not_response); + + // Validate file read request/response. + const Buf file_read_request_v1{ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x5C, 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x6B, + 0x6F, 0x63, 0x68, 0x65, 0x72, 0x67, 0x61, 0x2E, 0x74, 0x65, 0x73, 0x74, 0x2E, 0x69, 0x6E, 0x74, 0x65, + 0x67, 0x72, 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x2D, 0x31, 0x30, 0x2D, 0x33, 0x2E, 0x31, 0x2E, 0x62, 0x61, + 0x64, 0x63, 0x30, 0x66, 0x66, 0x65, 0x65, 0x30, 0x64, 0x64, 0x66, 0x30, 0x30, 0x64, 0x2E, 0x34, 0x35, + 0x32, 0x61, 0x34, 0x32, 0x36, 0x37, 0x39, 0x37, 0x31, 0x61, 0x33, 0x39, 0x32, 0x38, 0x2E, 0x61, 0x70, + 0x70, 0x2E, 0x72, 0x65, 0x6C, 0x65, 0x61, 0x73, 0x65, 0x2E, 0x62, 0x69, 0x6E, + }; + // Same but without the length field (removed by TAO): + const Buf file_read_request_v0{ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x6F, 0x6D, 0x2E, 0x7A, 0x75, 0x62, 0x61, 0x78, 0x2E, 0x6B, 0x6F, + 0x63, 0x68, 0x65, 0x72, 0x67, 0x61, 0x2E, 0x74, 0x65, 0x73, 0x74, 0x2E, 0x69, 0x6E, 0x74, 0x65, 0x67, + 0x72, 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x2D, 0x31, 0x30, 0x2D, 0x33, 0x2E, 0x31, 0x2E, 0x62, 0x61, 0x64, + 0x63, 0x30, 0x66, 0x66, 0x65, 0x65, 0x30, 0x64, 0x64, 0x66, 0x30, 0x30, 0x64, 0x2E, 0x34, 0x35, 0x32, + 0x61, 0x34, 0x32, 0x36, 0x37, 0x39, 0x37, 0x31, 0x61, 0x33, 0x39, 0x32, 0x38, 0x2E, 0x61, 0x70, 0x70, + 0x2E, 0x72, 0x65, 0x6C, 0x65, 0x61, 0x73, 0x65, 0x2E, 0x62, 0x69, 0x6E, + }; + REQUIRE(static_cast(node) + .sendRequest(ServiceID::FileRead, 42, 22, file_read_request_v1.size(), file_read_request_v1.data())); + tx = poll(1000us); + REQUIRE(tx); + REQUIRE(tx->transfer_id == 22); + REQUIRE(tx->priority == 30); + REQUIRE(tx->payload == file_read_request_v0); + REQUIRE(dynamic_cast(*tx).service_id == 48); + REQUIRE(dynamic_cast(*tx).source_node_id == 123); + REQUIRE(dynamic_cast(*tx).destination_node_id == 42); + REQUIRE(dynamic_cast(*tx).request_not_response); + const Buf file_read_response_v0{ + 0x00, 0x00, // Error code; no length prefix due to TAO. + 0x48, 0x65, 0x6C, 0x6C, 0x6F, 0x20, 0x77, 0x6F, 0x72, 0x6C, 0x64, 0x21, 0x20, 0x20, 0x20, 0x20, 0xC7, 0xC4, + 0xC0, 0x6F, 0x14, 0x15, 0x44, 0x5E, 0x41, 0x50, 0x44, 0x65, 0x73, 0x63, 0x30, 0x30, 0x28, 0x39, 0x1A, 0x97, + 0x67, 0x42, 0x2A, 0x45, 0x90, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x01, 0x01, 0x00, 0xD2, 0x02, + 0x96, 0x49, 0x0D, 0xF0, 0xDD, 0xE0, 0xFE, 0x0F, 0xDC, 0xBA, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x54, 0x68, 0x65, 0x20, 0x61, 0x70, 0x70, 0x6C, 0x69, 0x63, + 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x20, 0x69, 0x6D, 0x61, 0x67, 0x65, 0x20, 0x67, 0x6F, 0x65, 0x73, 0x20, 0x68, + 0x65, 0x72, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, + }; + // Translated response v0 --> v1. + const Buf file_read_response_v1{ + 0x00, 0x00, // Error code. + 0x90, 0x00, // Length prefix. + 0x48, 0x65, 0x6C, 0x6C, 0x6F, 0x20, 0x77, 0x6F, 0x72, 0x6C, 0x64, 0x21, 0x20, 0x20, 0x20, 0x20, 0xC7, 0xC4, + 0xC0, 0x6F, 0x14, 0x15, 0x44, 0x5E, 0x41, 0x50, 0x44, 0x65, 0x73, 0x63, 0x30, 0x30, 0x28, 0x39, 0x1A, 0x97, + 0x67, 0x42, 0x2A, 0x45, 0x90, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x01, 0x01, 0x00, 0xD2, 0x02, + 0x96, 0x49, 0x0D, 0xF0, 0xDD, 0xE0, 0xFE, 0x0F, 0xDC, 0xBA, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x54, 0x68, 0x65, 0x20, 0x61, 0x70, 0x70, 0x6C, 0x69, 0x63, + 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x20, 0x69, 0x6D, 0x61, 0x67, 0x65, 0x20, 0x67, 0x6F, 0x65, 0x73, 0x20, 0x68, + 0x65, 0x72, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, + }; + REQUIRE(!poll(1000us)); + REQUIRE(!reactor.popPendingResponse()); + REQUIRE(!poll(1000us, + SignatureTransferPair{0x8DCDCA939F33F678ULL, + std::make_shared< + ServiceTransfer>(30, 22, 48, 42, 123, false, file_read_response_v0)})); + auto response = reactor.popPendingResponse(); + REQUIRE(response); + REQUIRE(*response == file_read_response_v1); // Translated correctly. + + // Validate unknown service. + REQUIRE(!static_cast(node).sendRequest(ServiceID::NodeExecuteCommand, 124, 22, 0, nullptr)); + REQUIRE(!poll(1000us)); +} diff --git a/tests/unit/images/bad-le-crc-x3.bin b/tests/unit/images/bad-le-crc-x3.bin new file mode 100644 index 0000000000000000000000000000000000000000..c33e15409da3776b5b02ec7809e321485234e76b GIT binary patch literal 576 zcmeZB&B@7ED9DiH zP4ndaaQDGK{yV!MqR3<&Nx*}U{ag_zn1d&6A>MwFTNnix7)WwP9U*tTi}dZD7x|Zs L*6#Q}v;YGDQJ*xH literal 0 HcmV?d00001 diff --git a/tests/unit/images/bad-le-short.bin b/tests/unit/images/bad-le-short.bin new file mode 100644 index 0000000000000000000000000000000000000000..881cbc9162bb961eb1453c40c6fb4b09cd61f3cd GIT binary patch literal 200 zcmeZB&B@7ED9DiH zP4ndaaQDGK{yV!Mq6pG4Ai&4d*)hb^-%r8W-^EqIGgu)wG$6n~*wsZL#9zV5Rl&nG z$W_59G(^D@h=LXT{6iFc{QcY&T>bn*-912xd>uoafea5 + +#pragma once + +#include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include "util.hpp" +#include +#include + +namespace mock +{ +struct Transfer +{ + std::optional remote_node_id; + kocherga::TransferID transfer_id{}; + std::vector payload; + + Transfer() = default; + Transfer(const kocherga::TransferID arg_transfer_id, + const std::vector& arg_payload, + const std::optional arg_remote_node_id = std::optional()) : + remote_node_id(arg_remote_node_id), transfer_id(arg_transfer_id), payload(arg_payload) + {} + Transfer(const kocherga::TransferID arg_transfer_id, + const std::size_t arg_payload_length, + const std::uint8_t* const arg_payload, + const std::optional arg_remote_node_id = std::optional()) : + remote_node_id(arg_remote_node_id), transfer_id(arg_transfer_id) + { + (void) std::copy_n(arg_payload, arg_payload_length, std::back_inserter(payload)); + } + + [[nodiscard]] auto toString() const -> std::string + { + return "remote_nid=" + (remote_node_id ? std::to_string(static_cast(*remote_node_id)) : "?") + + " tid=" + std::to_string(transfer_id) + " payload:\n" + util::makeHexDump(payload) + "\n"; + } + + auto operator==(const Transfer& rhs) const -> bool + { + return (remote_node_id == rhs.remote_node_id) && (transfer_id == rhs.transfer_id) && + std::equal(std::begin(payload), std::end(payload), std::begin(rhs.payload), std::end(rhs.payload)); + } + auto operator!=(const Transfer& rhs) const -> bool { return !operator==(rhs); } +}; + +class Node : public kocherga::INode +{ +public: + enum class Output + { + ExecuteCommandResponse, + NodeInfoResponse, + FileReadRequest, + HeartbeatMessage, + LogRecordMessage, + }; + + enum class Input + { + ExecuteCommandRequest, + NodeInfoRequest, + FileReadResponse, + }; + + [[nodiscard]] auto getLastPollTime() const { return last_poll_at_; } + + [[nodiscard]] auto wasRequestCanceled() + { + const auto out = request_canceled_; + request_canceled_ = false; + return out; + } + + void setFileReadResult(const bool value) { file_read_result_ = value; } + + /// Retrieve a previously received transfer under the specified session. + /// Return an empty option if no such transfer was received since the last retrieval. + /// The read is destructive -- the transfer is removed afterwards. + [[nodiscard]] auto popOutput(const Output ses) -> std::optional + { + if (const auto it = outputs_.find(ses); it != std::end(outputs_)) + { + const Transfer ret = it->second; + outputs_.erase(it); + return ret; + } + return {}; + } + + /// Store the transfer for reception when the next poll() is invoked. + /// The invocation fails if such transfer is already pending. + void pushInput(const Input ses, const Transfer& tr) + { + REQUIRE(inputs_.find(ses) == std::end(inputs_)); + inputs_[ses] = tr; + } + +private: + void poll(kocherga::IReactor& reactor, const std::chrono::microseconds uptime) override + { + const auto proc = [&reactor, this](const Output ses, const kocherga::ServiceID service_id, const Transfer& tr) { + std::vector buffer(kocherga::MaxSerializedRepresentationSize); + // + const auto size = reactor.processRequest(static_cast(service_id), + *tr.remote_node_id, + tr.payload.size(), + tr.payload.data(), + buffer.data()); + if (size) + { + buffer.resize(*size); + store(ses, Transfer(tr.transfer_id, buffer, *tr.remote_node_id)); + } + }; + + last_poll_at_ = uptime; + for (auto [key, tr] : inputs_) + { + switch (key) + { + case Input::ExecuteCommandRequest: + { + proc(Output::ExecuteCommandResponse, kocherga::ServiceID::NodeExecuteCommand, tr); + break; + } + case Input::NodeInfoRequest: + { + proc(Output::NodeInfoResponse, kocherga::ServiceID::NodeGetInfo, tr); + break; + } + case Input::FileReadResponse: + { + reactor.processResponse(std::size(tr.payload), tr.payload.data()); + break; + } + default: + { + FAIL("UNHANDLED CASE"); + } + } + } + inputs_.clear(); + } + + [[nodiscard]] auto sendRequest(const kocherga::ServiceID service_id, + const kocherga::NodeID server_node_id, + const kocherga::TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + switch (service_id) + { + case kocherga::ServiceID::FileRead: + { + store(Output::FileReadRequest, Transfer(transfer_id, payload_length, payload, server_node_id)); + return file_read_result_; + } + case kocherga::ServiceID::NodeGetInfo: + case kocherga::ServiceID::NodeExecuteCommand: + default: + { + FAIL("UNEXPECTED SERVICE REQUEST"); + return false; + } + } + } + + void cancelRequest() override { request_canceled_ = true; } + + [[nodiscard]] auto publishMessage(const kocherga::SubjectID subject_id, + const kocherga::TransferID transfer_id, + const std::size_t payload_length, + const std::uint8_t* const payload) -> bool override + { + bool out = false; + switch (subject_id) + { + case kocherga::SubjectID::NodeHeartbeat: + { + store(Output::HeartbeatMessage, Transfer(transfer_id, payload_length, payload)); + out = true; + break; + } + case kocherga::SubjectID::DiagnosticRecord: + { + store(Output::LogRecordMessage, Transfer(transfer_id, payload_length, payload)); + out = true; + break; + } + case kocherga::SubjectID::PnPNodeIDAllocationData_v1: + case kocherga::SubjectID::PnPNodeIDAllocationData_v2: + default: + { + FAIL("UNEXPECTED MESSAGE"); + break; + } + } + return out; + } + + void store(const Output ses, const Transfer& tr) + { + INFO("output: " << static_cast(ses)); + INFO("transfer: " << tr.toString()); + REQUIRE(outputs_.find(ses) == std::end(outputs_)); + outputs_[ses] = tr; + } + + std::chrono::microseconds last_poll_at_{}; + bool file_read_result_ = true; + bool request_canceled_ = false; + std::map outputs_; + std::map inputs_; +}; + +} // namespace mock diff --git a/tests/unit/serial/test_misc.cpp b/tests/unit/serial/test_misc.cpp new file mode 100644 index 0000000..76f5410 --- /dev/null +++ b/tests/unit/serial/test_misc.cpp @@ -0,0 +1,35 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga_serial.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include + +TEST_CASE("serial::CRC") +{ + kocherga::serial::detail::CRC32C crc; + crc.update(static_cast('1')); + crc.update(static_cast('2')); + crc.update(static_cast('3')); + crc.update(static_cast('4')); + crc.update(static_cast('5')); + crc.update(static_cast('6')); + crc.update(static_cast('7')); + crc.update(static_cast('8')); + crc.update(static_cast('9')); + + REQUIRE(0xE306'9283UL == crc.get()); + REQUIRE(crc.getBytes().at(0) == 0x83U); + REQUIRE(crc.getBytes().at(1) == 0x92U); + REQUIRE(crc.getBytes().at(2) == 0x06U); + REQUIRE(crc.getBytes().at(3) == 0xE3U); + + REQUIRE(!crc.isResidueCorrect()); + crc.update(0x83U); + crc.update(0x92U); + crc.update(0x06U); + crc.update(0xE3U); + REQUIRE(crc.isResidueCorrect()); + REQUIRE(0xB798'B438UL == (~crc.get())); +} diff --git a/tests/unit/serial/test_node.cpp b/tests/unit/serial/test_node.cpp new file mode 100644 index 0000000..cb0bee4 --- /dev/null +++ b/tests/unit/serial/test_node.cpp @@ -0,0 +1,323 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2021 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga_serial.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include +#include + +namespace +{ +class SerialPortMock : public kocherga::serial::ISerialPort +{ +public: + void pushRx(const kocherga::serial::detail::Transfer& transfer) + { + REQUIRE(kocherga::serial::detail::transmit( + [this](const std::uint8_t bt) { + rx_.push_back(bt); + return true; + }, + transfer)); + } + + /// The payload pointer of the returned transfer is invalidated on the 32-nd call to ISerialPort::send(). + [[nodiscard]] auto popTx() -> std::optional + { + while (!tx_.empty()) + { + const auto bt = tx_.front(); + tx_.pop_front(); + if (const auto tr = stream_parser_.update(bt)) + { + return tr; + } + } + return {}; + } + +private: + [[nodiscard]] auto receive() -> std::optional override + { + if (!rx_.empty()) + { + const auto out = rx_.front(); + rx_.pop_front(); + return out; + } + return {}; + } + + [[nodiscard]] auto send(const std::uint8_t b) -> bool override + { + tx_.push_back(b); + return true; + } + + std::deque tx_; + std::deque rx_; + kocherga::serial::detail::StreamParser<1024> stream_parser_; +}; + +class ReactorMock : public kocherga::IReactor +{ +public: + struct IncomingRequest + { + kocherga::PortID service_id = 0xFFFF; + kocherga::NodeID client_node_id = 0xFFFF; + std::vector data; + }; + + /// Accepts request, returns the serialized response payload or nothing if no response should be sent. + using IncomingRequestHandler = std::function>(IncomingRequest)>; + + void setIncomingRequestHandler(const IncomingRequestHandler& irh) { request_handler_ = irh; } + + [[nodiscard]] auto popPendingResponse() -> std::optional> + { + if (!pending_responses_.empty()) + { + const auto out = pending_responses_.front(); + pending_responses_.pop_front(); + return out; + } + return {}; + } + +private: + [[nodiscard]] auto processRequest(const kocherga::PortID service_id, + const kocherga::NodeID client_node_id, + const std::size_t request_length, + const std::uint8_t* const request, + std::uint8_t* const out_response) -> std::optional override + { + IncomingRequest ir{service_id, client_node_id, {}}; + std::copy_n(request, request_length, std::back_insert_iterator(ir.data)); + REQUIRE(request_handler_); + if (const auto response_payload = request_handler_(ir)) + { + std::copy(response_payload->begin(), response_payload->end(), out_response); + return response_payload->size(); + } + return {}; + } + + void processResponse(const std::size_t response_length, const std::uint8_t* const response) override + { + pending_responses_.emplace_back(); + std::copy_n(response, response_length, std::back_insert_iterator(pending_responses_.back())); + } + + IncomingRequestHandler request_handler_; + std::deque> pending_responses_; +}; + +} // namespace + +TEST_CASE("kocherga_serial::SerialNode service") +{ + SerialPortMock port; + kocherga::serial::SerialNode node(port, {}); + node.setLocalNodeID(2222); + ReactorMock reactor; + + // Send a request transfer and then pop a response. + { + REQUIRE(static_cast(node).sendRequest(kocherga::ServiceID::NodeExecuteCommand, + 1111, + 0xCAFE'CAFE, + 3, + reinterpret_cast("\x03\x03\x03"))); + const auto request = port.popTx(); + REQUIRE(request); + REQUIRE(request->meta.source == 2222); + REQUIRE(request->meta.destination == 1111); + REQUIRE(request->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag)); + REQUIRE(request->meta.transfer_id == 0xCAFE'CAFE); + REQUIRE(request->payload_len == 3); + REQUIRE(0 == std::memcmp(request->payload, "\x03\x03\x03", 3)); + REQUIRE(!port.popTx()); // Ensure no extra requests are sent. + // Ensure non-matching responses are simply ignored. + { + kocherga::serial::detail::Transfer response; + // BAD SOURCE + response.meta.source = 1110; + response.meta.destination = 2222; + response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag; + response.meta.transfer_id = 0xCAFE'CAFE; + port.pushRx(response); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(!reactor.popPendingResponse()); + // BAD DESTINATION + response.meta.source = 1111; + response.meta.destination = 2221; + response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag; + response.meta.transfer_id = 0xCAFE'CAFE; + port.pushRx(response); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(!reactor.popPendingResponse()); + // BAD DATA SPEC + response.meta.source = 1111; + response.meta.destination = 2222; + response.meta.data_spec = static_cast(kocherga::ServiceID::NodeGetInfo) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag; + response.meta.transfer_id = 0xCAFE'CAFE; + port.pushRx(response); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(!reactor.popPendingResponse()); + // BAD TRANSFER ID + response.meta.source = 1111; + response.meta.destination = 2222; + response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag; + response.meta.transfer_id = 0xCAFE'CAFA; + port.pushRx(response); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(!reactor.popPendingResponse()); + } + // Receive the matching response transfer. + kocherga::serial::detail::Transfer response; + response.meta.source = 1111; + response.meta.destination = 2222; + response.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag; + response.meta.transfer_id = 0xCAFE'CAFE; + response.payload_len = 6; + response.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); + port.pushRx(response); // First response shall be processed. + port.pushRx(response); // Deterministic data loss mitigation; shall be ignored. + port.pushRx(response); // Same here. + static_cast(node).poll(reactor, std::chrono::microseconds(9'000)); + const auto rp = reactor.popPendingResponse(); + REQUIRE(rp); + REQUIRE(*rp == std::vector{5, 4, 3, 2, 1, 0}); + REQUIRE(!reactor.popPendingResponse()); // Ensure duplicates are removed. + } + + // Send a request multiple times emulating the deterministic data loss mitigation, ensure repetitions are ignored. + { + kocherga::serial::detail::Transfer request; + request.meta.source = 1111; + request.meta.destination = 2222; + request.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; + request.meta.transfer_id = 0xCAFE'BABE; + request.payload_len = 6; + request.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); + port.pushRx(request); // First request shall be processed. + port.pushRx(request); // Deterministic data loss mitigation; shall be ignored. + port.pushRx(request); // Same here. + auto num_requests = 0; + reactor.setIncomingRequestHandler( + [&num_requests](const ReactorMock::IncomingRequest& ir) -> std::optional> { + num_requests++; + REQUIRE(ir.service_id == static_cast(kocherga::ServiceID::NodeExecuteCommand)); + REQUIRE(ir.client_node_id == 1111); + REQUIRE(ir.data == std::vector{5, 4, 3, 2, 1, 0}); + return std::vector{1, 2, 3, 4, 5}; + }); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(num_requests == 1); // Ensure extras are ignored. + const auto response = port.popTx(); + REQUIRE(response); + REQUIRE(response->meta.source == 2222); + REQUIRE(response->meta.destination == 1111); + REQUIRE(response->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag)); + REQUIRE(response->meta.transfer_id == 0xCAFE'BABE); + REQUIRE(response->payload_len == 5); + REQUIRE(0 == std::memcmp(response->payload, "\x01\x02\x03\x04\x05", 5)); + REQUIRE(!port.popTx()); // Ensure no extra responses are sent. + } + + // Ensure transfer-ID timeouts are handled properly -- repetitions separated by a long time interval are accepted. + { + kocherga::serial::detail::Transfer request; + request.meta.source = 3210; + request.meta.destination = 2222; + request.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; + request.meta.transfer_id = 0xBABA'CACA; + request.payload_len = 6; + request.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); + port.pushRx(request); // First request shall be processed. + port.pushRx(request); // Deterministic data loss mitigation; shall be ignored. + port.pushRx(request); // Same here. + auto num_requests = 0; + reactor.setIncomingRequestHandler( + [&num_requests](const ReactorMock::IncomingRequest& ir) -> std::optional> { + num_requests++; + REQUIRE(ir.service_id == static_cast(kocherga::ServiceID::NodeExecuteCommand)); + REQUIRE(ir.client_node_id == 3210); + REQUIRE(ir.data == std::vector{5, 4, 3, 2, 1, 0}); + return std::vector{1, 2, 3, 4, 5}; + }); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(num_requests == 1); // Ensure extras are ignored. + auto response = port.popTx(); + REQUIRE(response); + REQUIRE(response->meta.source == 2222); + REQUIRE(response->meta.destination == 3210); + REQUIRE(response->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag)); + REQUIRE(response->meta.transfer_id == 0xBABA'CACA); + REQUIRE(response->payload_len == 5); + REQUIRE(0 == std::memcmp(response->payload, "\x01\x02\x03\x04\x05", 5)); + REQUIRE(!port.popTx()); // Ensure no extra responses are sent. + // Now, we send the very same transfer but a long time has passed so it is accepted anyway. + port.pushRx(request); + port.pushRx(request); + static_cast(node).poll(reactor, std::chrono::microseconds(100'000'000)); + REQUIRE(num_requests == 2); // Ensure extras are ignored. + response = port.popTx(); + REQUIRE(response); + REQUIRE(response->meta.source == 2222); + REQUIRE(response->meta.destination == 3210); + REQUIRE(response->meta.data_spec == (static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag | + kocherga::serial::detail::Transfer::Metadata::DataSpecResponseFlag)); + REQUIRE(response->meta.transfer_id == 0xBABA'CACA); + REQUIRE(response->payload_len == 5); + REQUIRE(0 == std::memcmp(response->payload, "\x01\x02\x03\x04\x05", 5)); + REQUIRE(!port.popTx()); // Ensure no extra responses are sent. + } + + // Send an unknown request transfer, ensure no response is sent back. + { + kocherga::serial::detail::Transfer request; + request.meta.source = 3333; + request.meta.destination = 2222; + request.meta.data_spec = static_cast(kocherga::ServiceID::NodeExecuteCommand) | + kocherga::serial::detail::Transfer::Metadata::DataSpecServiceFlag; + request.meta.transfer_id = 0xBABA'BABA; + request.payload_len = 6; + request.payload = reinterpret_cast("\x05\x04\x03\x02\x01\x00"); + port.pushRx(request); // First request shall be processed. + port.pushRx(request); // Deterministic data loss mitigation; shall be ignored. + port.pushRx(request); // Same here. + auto num_requests = 0; + reactor.setIncomingRequestHandler( + [&num_requests](const ReactorMock::IncomingRequest& ir) -> std::optional> { + num_requests++; + REQUIRE(ir.service_id == static_cast(kocherga::ServiceID::NodeExecuteCommand)); + REQUIRE(ir.client_node_id == 3333); + REQUIRE(ir.data == std::vector{5, 4, 3, 2, 1, 0}); + return {}; // Return no response. + }); + static_cast(node).poll(reactor, std::chrono::microseconds(1'000)); + REQUIRE(num_requests == 1); // Ensure extras are ignored. + REQUIRE(!port.popTx()); // Ensure no responses are sent. + } +} diff --git a/tests/unit/serial/test_stream.cpp b/tests/unit/serial/test_stream.cpp new file mode 100644 index 0000000..20b8330 --- /dev/null +++ b/tests/unit/serial/test_stream.cpp @@ -0,0 +1,667 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga_serial.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "util.hpp" // NOLINT include order +#include "catch.hpp" +#include +#include + +TEST_CASE("serial::COBSEncoder") +{ + using kocherga::serial::detail::COBSEncoder; + using Buf = std::vector; + + struct Rig + { + Rig() : + enc([this](const std::uint8_t x) { + this->out.push_back(x); + return true; + }) + {} + + Buf out; + COBSEncoder> enc; + }; + + { // import cobs.cobs; cobs.cobs.encode(b'\x00') + Rig rig; + REQUIRE(rig.enc.push(0)); + REQUIRE(rig.enc.end()); + REQUIRE(rig.out == Buf{0, 1, 1, 0}); + } + + { // import cobs.cobs; cobs.cobs.encode(b'\x01') + Rig rig; + REQUIRE(rig.enc.push(1)); + REQUIRE(rig.enc.end()); + REQUIRE(rig.out == Buf{0, 2, 1, 0}); + } + + { // Reference taken from https://github.com/cmcqueen/cobs-c + Rig rig; + REQUIRE(rig.enc.push(0x2F)); + REQUIRE(rig.enc.push(0xA2)); + REQUIRE(rig.enc.push(0x00)); + REQUIRE(rig.enc.push(0x92)); + REQUIRE(rig.enc.push(0x73)); + REQUIRE(rig.enc.push(0x02)); + REQUIRE(rig.enc.end()); + REQUIRE(rig.out == Buf{0x00, 0x03, 0x2F, 0xA2, 0x04, 0x92, 0x73, 0x02, 0x00}); + } + + { // import cobs.cobs; cobs.cobs.encode(b'\x01' * 600) + Rig rig; + for (auto i = 0; i < 600; i++) + { + REQUIRE(rig.enc.push(1)); + } + REQUIRE(rig.enc.end()); + const Buf ref{ + 0, // head delimiter + 255, // length code + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // + 255, // length code + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // + 93, // length code + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // + 0, // tail delimiter + }; + assert(ref.size() == 603 + 2); + REQUIRE(rig.out == ref); + } +} + +TEST_CASE("serial::transmit") +{ + using kocherga::serial::detail::transmit; + using kocherga::serial::detail::Transfer; + using Buf = std::vector; + + // The reference dump has been obtained as follows: + // import pyuavcan.serial + // tr = pyuavcan.transport.serial.SerialTransport('loop://', local_node_id=1234, baudrate=115200) + // pm = pyuavcan.transport.PayloadMetadata(1024) + // ds = pyuavcan.transport.MessageDataSpecifier(2345) + // pub = tr.get_output_session(pyuavcan.transport.OutputSessionSpecifier(ds, None), pm) + // caps = [] + // tr.begin_capture(caps.append) + // pub.send(pyuavcan.transport.Transfer(pyuavcan.transport.Timestamp.now(), pyuavcan.transport.Priority.LOW, + // 1111, fragmented_payload=[]), + // tr.loop.time() + 1.0) + // print(caps) + { + const Buf reference = { + 0x00, // starting delimiter + 0x01, // COBS starting stuff byte, next byte zero + 0x08, // version 0, next zero 8 bytes later + 0x05, // priority low + 0xd2, 0x04, // src node-ID 1234 + 0xff, 0xff, // dst node-ID broadcast + 0x29, 0x09, // subject-ID 2345 + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x03, // reserved zeros + 0x57, 0x04, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, // transfer-ID 1111 + 0x01, 0x01, 0x06, 0x80, // frame index EOT 0x80000000 (single-frame transfer) + 0x02, 0xf4, 0x6f, 0x2a, // header CRC 0x2a6ff402 + 0x01, 0x01, 0x01, 0x01, // payload CRC 0x00000000 + 0x00 // final delimiter + }; + Buf history; + REQUIRE(transmit( + [&history](const auto bt) { + history.push_back(bt); + return true; + }, + Transfer{ + Transfer::Metadata{ + 5, + 1234, + Transfer::Metadata::AnonymousNodeID, // Broadcast + 2345, + 1111, + }, + 0, + nullptr, + })); + REQUIRE(reference == history); + } + + // The second test is like above but with the payload set to b'\x00\x01\x02' + { + const Buf reference = { + 0x00, // starting delimiter + 0x01, // COBS starting stuff byte, next byte zero + 0x08, // version 0, next zero 8 bytes later + 0x05, // priority low + 0xd2, 0x04, // src node-ID 1234 + 0xff, 0xff, // dst node-ID broadcast + 0x29, 0x09, // subject-ID 2345 + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x03, // reserved zeros + 0x57, 0x04, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, // transfer-ID 1111 + 0x01, 0x01, 0x06, 0x80, // frame index EOT 0x80000000 (single-frame transfer) + 0x02, 0xf4, 0x6f, 0x2a, // header CRC 0x2a6ff402 + 0x07, 0x01, 0x02, // payload [0, 1, 2] + 0xfa, 0x4b, 0xfd, 0x92, // payload CRC + 0x00, // final delimiter + }; + Buf history; + REQUIRE(transmit( + [&history](const auto bt) { + history.push_back(bt); + return true; + }, + Transfer{ + Transfer::Metadata{ + 5, + 1234, + Transfer::Metadata::AnonymousNodeID, // Broadcast + 2345, + 1111, + }, + 3, + reinterpret_cast("\x00\x01\x02"), + })); + REQUIRE(reference == history); + } + + // Failure case + { + std::array buf{}; + std::uint32_t fail_after = 0; + std::uint32_t num_sent = 0; + const auto feed_abort = [&fail_after, &num_sent](const auto bt) { + (void) bt; + if (fail_after > 0) + { + --fail_after; + ++num_sent; + return true; + } + return false; + }; + for (auto i = 0U; i < 300U; i++) + { + num_sent = 0; + fail_after = i; + REQUIRE(!transmit(feed_abort, + Transfer{ + Transfer::Metadata{ + static_cast(i % 8U), + static_cast(i | 0x8E00U), + static_cast(i | 0x9E00U), + 0, + 0, + }, + std::min(buf.size(), i), + buf.data(), + })); + REQUIRE(num_sent == i); + } + } +} + +TEST_CASE("serial::COBSDecoder") +{ + using kocherga::serial::detail::COBSDecoder; + using Buf = std::vector; + + COBSDecoder dec; + + // If we only feed delimiters, we get empty packets out. + REQUIRE(std::holds_alternative(dec.feed(0))); + REQUIRE(std::holds_alternative(dec.feed(0))); + + const auto decode = [&dec](const auto encoded) -> std::optional { + Buf out; + for (std::uint8_t bt : encoded) + { + const auto res = dec.feed(bt); + if (const auto* out_byte = std::get_if(&res)) + { + out.push_back(*out_byte); + } + else if (std::holds_alternative(res)) + { + return out; + } + else + { + assert(std::holds_alternative(res)); + } + } + return {}; // Return empty because unterminated. + }; + + // Valid packets. + REQUIRE(*decode(Buf{1, 1, 0}) == Buf{0}); + REQUIRE(*decode(Buf{2, 1, 0}) == Buf{1}); + REQUIRE(decode(Buf{0})->empty()); + REQUIRE(*decode(Buf{0x03, 0x2F, 0xA2, 0x04, 0x92, 0x73, 0x02, 0x00}) == Buf{0x2F, 0xA2, 0x00, 0x92, 0x73, 0x02}); + { + const Buf input{ + 255, // length code + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // + 255, // length code + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // + 93, // length code + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // + 0, // tail delimiter + }; + assert(input.size() == 603 - 1 + 2); + const Buf ref(600, 1); + REQUIRE(*decode(input) == ref); + } + // Malformed packets are not detected, best effort handling. + REQUIRE(decode(Buf{3, 0})->empty()); + // Successful recovery. + REQUIRE(*decode(Buf{0x03, 0x2F, 0xA2, 0x04, 0x92, 0x73, 0x02, 0x00}) == Buf{0x2F, 0xA2, 0x00, 0x92, 0x73, 0x02}); +} + +TEST_CASE("serial::COBS* roundtrip") +{ + using kocherga::serial::detail::COBSDecoder; + using kocherga::serial::detail::COBSEncoder; + using Buf = std::vector; + + static const auto synth_buf = []() -> Buf { + Buf out(util::getRandomInteger()); + for (auto& x : out) + { + x = util::getRandomInteger(); + } + return out; + }; + + // Generate many random packets. + std::vector original_packets(20); + for (auto& x : original_packets) + { + x = synth_buf(); + } + + // Serialize all packets into one stream representing the transmission medium. + Buf encoded_stream; + for (const auto& in : original_packets) + { + COBSEncoder> enc([&encoded_stream](const std::uint8_t x) { + encoded_stream.push_back(x); + return true; + }); + for (auto x : in) + { + REQUIRE(enc.push(x)); + } + REQUIRE(enc.end()); + } + std::cout << "Encoded stream length: " << encoded_stream.size() << std::endl; + + // Deserialize the packets back from the stream. + std::vector decoded_packets; + COBSDecoder dec; + Buf out; + for (const auto& bt : encoded_stream) + { + const auto res = dec.feed(bt); + if (const auto* out_byte = std::get_if(&res)) + { + out.push_back(*out_byte); + } + else if (std::holds_alternative(res)) + { + if (!out.empty()) + { + decoded_packets.push_back(out); + out.clear(); + } + } + else + { + assert(std::holds_alternative(res)); + } + } + + // Compare the packets we've just deserialized against the original ones. + REQUIRE(decoded_packets.size() == original_packets.size()); + REQUIRE(decoded_packets == original_packets); +} + +TEST_CASE("serial::StreamParser basic") +{ + using kocherga::serial::detail::StreamParser; + using kocherga::serial::detail::Transfer; + using Buf = std::vector; + + StreamParser<10> sp; + + const auto feed = [&sp](const auto& data) { + std::optional out; + for (const std::uint8_t x : data) + { + REQUIRE(!out); + out = sp.update(x); + } + return out; + }; + + // VALID MESSAGE + // crc = lambda x: pyuavcan.transport.commons.crc.CRC32C.new(x).value_as_bytes + // ', '.join(f'0x{x:02x}' for x in cobs.cobs.encode(bytes(hdr) + crc(hdr) + bytes(payload) + crc(bytes(payload)))) + { + const Buf chunk{ + 0x01, // COBS starting overhead byte + 0x03, // Version 0 + 0x05, // Priority 5 + 0x7b, 0x15, // Source NID 123 + 0xff, 0xff, // Destination NID 456 + 0xe1, 0x10, // Data specifier 4321 + 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, // + 0xd2, 0x0a, 0x1f, 0xeb, 0x8c, 0xa9, 0x54, 0xab, // Transfer ID 12345678901234567890 + 0x01, 0x01, 0x0f, 0x80, // Frame index, EOT 0 with EOT flag set + 0xad, 0x13, 0xce, 0xc6, // Header CRC computed with the help of PyUAVCAN + 0x01, 0x02, 0x03, 0x04, 0x05, // Payload 1 2 3 4 5 + 0xab, 0x8f, 0x51, 0x53, // Payload CRC + }; + REQUIRE(!feed(Buf{0})); + REQUIRE(!feed(chunk)); + const auto tr = feed(Buf{0}); + REQUIRE(tr); + REQUIRE(tr->payload_len == 5); + REQUIRE(0 == std::memcmp("\x01\x02\x03\x04\x05", tr->payload, 5)); + REQUIRE(tr->meta.priority == 5); + REQUIRE(tr->meta.source == 123); + REQUIRE(tr->meta.destination == Transfer::Metadata::AnonymousNodeID); + REQUIRE(tr->meta.data_spec == 4321); + REQUIRE(tr->meta.transfer_id == 12'345'678'901'234'567'890ULL); + REQUIRE(!tr->meta.isRequest()); + REQUIRE(!tr->meta.isResponse()); + } + + // VALID RESPONSE + { + const Buf chunk = { + 0x01, + 0x18, // Version 0 + 0x02, // Priority 1 + 0x8e, 0x01, // Source NID 398 + 0x11, 0x01, // Destination NID 273 + 0x9e, 0xc0, // Data specifier response 158 + 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, // + 0xd2, 0x0a, 0x1f, 0xeb, 0x8c, 0xa9, 0x54, 0xab, // Transfer ID 12345678901234567890 + 0x01, 0x01, 0x0f, 0x80, // Frame index, EOT 0 with EOT flag set + 0x50, 0x42, 0x9b, 0x1f, // Header CRC + 0x01, 0x02, 0x03, 0x04, 0x05, // Payload 1 2 3 4 5 + 0xab, 0x8f, 0x51, 0x53 // Payload CRC + }; + // Empty frames ignored. + REQUIRE(!feed(Buf{0})); + REQUIRE(!feed(Buf{0})); + REQUIRE(!feed(chunk)); + const auto tr = feed(Buf{0}); + REQUIRE(tr); + REQUIRE(tr->payload_len == 5); + REQUIRE(0 == std::memcmp("\x01\x02\x03\x04\x05", tr->payload, 5)); + REQUIRE(tr->meta.priority == 2); + REQUIRE(tr->meta.source == 398); + REQUIRE(tr->meta.destination == 273); + REQUIRE(tr->meta.data_spec == 0xC09E); + REQUIRE(tr->meta.transfer_id == 12'345'678'901'234'567'890ULL); + REQUIRE(!tr->meta.isRequest()); + REQUIRE(tr->meta.isResponse() == 158); + } + + // BAD HEADER CRC + { + const Buf chunk = { + 0x01, + 0x18, // Version 0 + 0x02, // Priority 1 + 0x8e, 0x01, // Source NID 398 + 0x11, 0x01, // Destination NID 273 + 0x9e, 0xc0, // Data specifier response 158 + 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, // + 0xd2, 0x0a, 0x1f, 0xeb, 0x8c, 0xa9, 0x54, 0xab, // Transfer ID 12345678901234567890 + 0x01, 0x01, 0x0f, 0x80, // Frame index, EOT 0 with EOT flag set + 0x50, 0x42, 0x9b, 0x0f, // Header CRC MSB FLIP ERROR + 0x01, 0x02, 0x03, 0x04, 0x05, // Payload 1 2 3 4 5 + 0xab, 0x8f, 0x51, 0x53 // Payload CRC + }; + REQUIRE(!feed(Buf{0})); + REQUIRE(!feed(chunk)); + REQUIRE(!feed(Buf{0})); // CRC ERROR + } + + // BAD PAYLOAD CRC + { + const Buf chunk = { + 0x01, + 0x18, // Version 0 + 0x02, // Priority 1 + 0x8e, 0x01, // Source NID 398 + 0x11, 0x01, // Destination NID 273 + 0x9e, 0xc0, // Data specifier response 158 + 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, // + 0xd2, 0x0a, 0x1f, 0xeb, 0x8c, 0xa9, 0x54, 0xab, // Transfer ID 12345678901234567890 + 0x01, 0x01, 0x0f, 0x80, // Frame index, EOT 0 with EOT flag set + 0x50, 0x42, 0x9b, 0x1f, // Header CRC + 0x01, 0x02, 0x03, 0x04, 0x05, // Payload 1 2 3 4 5 + 0xaa, 0x8f, 0x51, 0x53 // Payload CRC LSB FLIP ERROR + }; + REQUIRE(!feed(Buf{0})); + REQUIRE(!feed(chunk)); + REQUIRE(!feed(Buf{0})); // CRC ERROR + } +} + +TEST_CASE("serial::StreamParser error") +{ + using kocherga::serial::detail::StreamParser; + using kocherga::serial::detail::Transfer; + using Buf = std::vector; + + StreamParser<10> sp; + + const auto feed = [&sp](const Buf& header, const Buf& payload) { + std::optional out; + kocherga::serial::detail::COBSEncoder enc([&out, &sp](const std::uint8_t x) { + REQUIRE(!out); + out = sp.update(x); + return true; + }); + const auto inject = [&enc](const auto& data) { + kocherga::serial::detail::CRC32C crc_computer; + for (const std::uint8_t x : data) + { + crc_computer.update(x); + REQUIRE(enc.push(x)); + } + for (const std::uint8_t x : crc_computer.getBytes()) + { + REQUIRE(enc.push(x)); + } + }; + inject(header); + inject(payload); + REQUIRE(enc.end()); + return out; + }; + + // Self-test with max length payload + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0x8a, 0x0c, // Destination NID + 0x4d, 0x81, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}); + REQUIRE(tr); + REQUIRE(tr->meta.priority == 7); + REQUIRE(tr->meta.source == 1234); + REQUIRE(tr->meta.destination == 3210); + REQUIRE(tr->meta.data_spec == (Transfer::Metadata::DataSpecServiceFlag | 333U)); + REQUIRE(tr->meta.transfer_id == 0x1234'5678'9012'3456ULL); + REQUIRE(tr->payload_len == 10); + REQUIRE(0 == std::memcmp(tr->payload, "\x00\x01\x02\x03\x04\x05\x06\x07\x08\x09", 10)); + REQUIRE(tr->meta.isRequest() == 333); + REQUIRE(!tr->meta.isResponse()); + } + + // Self-test with zero length payload + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0x8a, 0x0c, // Destination NID + 0x4d, 0xc1, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {}); + REQUIRE(tr); + REQUIRE(tr->meta.priority == 7); + REQUIRE(tr->meta.source == 1234); + REQUIRE(tr->meta.destination == 3210); + REQUIRE(tr->meta.data_spec == + (333U | Transfer::Metadata::DataSpecServiceFlag | Transfer::Metadata::DataSpecResponseFlag)); + REQUIRE(tr->meta.transfer_id == 0x1234'5678'9012'3456ULL); + REQUIRE(tr->payload_len == 0); + REQUIRE(!tr->meta.isRequest()); + REQUIRE(tr->meta.isResponse() == 333); + } + + // Payload one byte too long + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0x8a, 0x0c, // Destination NID + 0x4d, 0x81, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}); + REQUIRE(!tr); + } + + // Bad version + { + const Buf header{ + 0x01, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0x8a, 0x0c, // Destination NID + 0x4d, 0x81, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {}); + REQUIRE(!tr); + } + + // UAVCAN/serial transfers cannot be multi-frame + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0x8a, 0x0c, // Destination NID + 0x4d, 0x81, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x00, // Frame index, EOT (cleared) + }; + const auto tr = feed(header, {}); + REQUIRE(!tr); + } + + // Service transfers cannot be broadcast + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0xff, 0xff, // Destination NID + 0x4d, 0xc1, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {}); + REQUIRE(!tr); + } + + // Service transfers cannot be anonymous + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xff, 0xff, // Source NID + 0x01, 0x01, // Destination NID + 0x4d, 0xc1, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {}); + REQUIRE(!tr); + } + + // Message transfers cannot be unicast + { + const Buf header{ + 0x00, // Version + 0x07, // Priority + 0xd2, 0x04, // Source NID + 0x8a, 0x0c, // Destination NID + 0x4d, 0x01, // Data specifier + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // + 0x56, 0x34, 0x12, 0x90, 0x78, 0x56, 0x34, 0x12, // Transfer ID + 0x00, 0x00, 0x00, 0x80, // Frame index, EOT + }; + const auto tr = feed(header, {}); + REQUIRE(!tr); + } +} diff --git a/tests/unit/test_app_locator.cpp b/tests/unit/test_app_locator.cpp new file mode 100644 index 0000000..468637b --- /dev/null +++ b/tests/unit/test_app_locator.cpp @@ -0,0 +1,66 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include "util.hpp" +#include + +TEST_CASE("AppLocator-good-simple") +{ + const util::FileROMBackend rom( + util::getImagePath("good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin")); + + const kocherga::detail::AppLocator loc_ok(rom, 1024U); + + auto info = loc_ok.identifyApplication(); + REQUIRE(info); + REQUIRE(info->image_size == 144); + REQUIRE(info->image_crc == 0x452A'4267'971A'3928ULL); + REQUIRE(info->timestamp_utc == 1234567890); + REQUIRE(info->vcs_revision_id == 0xBADC'0FFE'E0DD'F00DULL); + REQUIRE(info->isReleaseBuild()); + REQUIRE(!info->isDirtyBuild()); + REQUIRE(info->version.at(0) == 3); + REQUIRE(info->version.at(1) == 1); + + const kocherga::detail::AppLocator loc_too_small(rom, 64U); + REQUIRE(!loc_too_small.identifyApplication()); +} + +TEST_CASE("AppLocator-good-3rd-entry") +{ + const util::FileROMBackend rom( + util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin")); + + const kocherga::detail::AppLocator loc_ok(rom, 1024U); + + auto info = loc_ok.identifyApplication(); + REQUIRE(info); + REQUIRE(info->image_size == 576); + REQUIRE(info->image_crc == 0x8B61'938E'E5F9'0B1FULL); + REQUIRE(info->timestamp_utc == 1234567890); + REQUIRE(info->vcs_revision_id == 0x3333'3333'3333'3333ULL); + REQUIRE(!info->isReleaseBuild()); + REQUIRE(info->isDirtyBuild()); + REQUIRE(info->version.at(0) == 5); + REQUIRE(info->version.at(1) == 6); + + const kocherga::detail::AppLocator loc_too_small(rom, 64U); + REQUIRE(!loc_too_small.identifyApplication()); +} + +TEST_CASE("AppLocator-bad-crc-x3") +{ + const util::FileROMBackend rom(util::getImagePath("bad-le-crc-x3.bin")); + REQUIRE(!kocherga::detail::AppLocator(rom, 1024U).identifyApplication()); + REQUIRE(!kocherga::detail::AppLocator(rom, 100U).identifyApplication()); +} + +TEST_CASE("AppLocator-bad-short") +{ + const util::FileROMBackend rom(util::getImagePath("bad-le-short.bin")); + REQUIRE(!kocherga::detail::AppLocator(rom, 1024U).identifyApplication()); + REQUIRE(!kocherga::detail::AppLocator(rom, 10U).identifyApplication()); +} diff --git a/tests/unit/test_core.cpp b/tests/unit/test_core.cpp new file mode 100644 index 0000000..cfd2369 --- /dev/null +++ b/tests/unit/test_core.cpp @@ -0,0 +1,509 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include "mock.hpp" +#include + +namespace +{ +inline auto getSysInfo() +{ + static constexpr auto coa_capacity = 222U; + std::array coa{}; + for (auto i = 0U; i < coa_capacity; i++) + { + coa.at(i) = static_cast(coa.size() - i); + } + return kocherga::SystemInfo{ + kocherga::SemanticVersion{33, 11}, + {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}, + "com.zubax.kocherga.test.core", + 222, + coa.data(), + }; +} + +template +inline auto checkHeartbeat(std::array& nodes, + const std::optional tid, + const std::uint32_t uptime, + const kocherga::detail::dsdl::Heartbeat::Health health, + const std::uint8_t vssc) -> bool +{ + using kocherga::detail::dsdl::Heartbeat; + for (auto& n : nodes) + { + const mock::Transfer tr = *n.popOutput(mock::Node::Output::HeartbeatMessage); + const mock::Transfer ref(tid ? *tid : tr.transfer_id, + { + static_cast(uptime >> 0U), + static_cast(uptime >> 8U), + static_cast(uptime >> 16U), + static_cast(uptime >> 24U), + static_cast(health), + static_cast(Heartbeat::ModeSoftwareUpdate), + static_cast(vssc), + }); + if (tr != ref) + { + std::cout << ref.toString() << tr.toString() << std::endl; + return false; + } + } + return true; +} + +} // namespace + +TEST_CASE("Bootloader-fast-boot") +{ + using std::chrono_literals::operator""ms; + + const auto sys = getSysInfo(); + const auto img = util::getImagePath("good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin"); + util::FileROMBackend rom(img); + std::array nodes; + kocherga::Bootloader bl(rom, sys, static_cast(std::filesystem::file_size(img)), false); + REQUIRE(bl.addNode(&nodes.at(0))); + REQUIRE(bl.addNode(&nodes.at(1))); + REQUIRE(bl.addNode(&nodes.at(2))); + REQUIRE(!bl.addNode(&nodes.at(2))); // Double registration has no effect. + + REQUIRE(bl.poll(500ms) == kocherga::Final::BootApp); + REQUIRE(bl.getState() == kocherga::State::BootDelay); + + auto ai = *bl.getAppInfo(); + REQUIRE(0x452A'4267'971A'3928ULL == ai.image_crc); + REQUIRE(0xBADC'0FFE'E0DD'F00DULL == ai.vcs_revision_id); + REQUIRE(3 == ai.version.at(0)); + REQUIRE(1 == ai.version.at(1)); + REQUIRE(ai.isReleaseBuild()); + REQUIRE(!ai.isDirtyBuild()); +} + +TEST_CASE("Bootloader-boot-delay") +{ + using std::chrono_literals::operator""s; + using std::chrono_literals::operator""ms; + + const auto sys = getSysInfo(); + const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); + util::FileROMBackend rom(img); + mock::Node node; + kocherga::Bootloader bl(rom, sys, static_cast(std::filesystem::file_size(img)), false, 1s); + REQUIRE(bl.addNode(&node)); + + REQUIRE(!bl.poll(500ms)); + REQUIRE(bl.getState() == kocherga::State::BootDelay); + + auto ai = *bl.getAppInfo(); + REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); + REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); + REQUIRE(5 == ai.version.at(0)); + REQUIRE(6 == ai.version.at(1)); + REQUIRE(!ai.isReleaseBuild()); + REQUIRE(ai.isDirtyBuild()); + + REQUIRE(!bl.poll(900ms)); + REQUIRE(bl.getState() == kocherga::State::BootDelay); + + REQUIRE(bl.poll(1900ms) == kocherga::Final::BootApp); + REQUIRE(bl.getState() == kocherga::State::BootDelay); +} + +TEST_CASE("Bootloader-linger-reboot") +{ + using std::chrono_literals::operator""ms; + using mock::Node; + using mock::Transfer; + + const auto sys = getSysInfo(); + const auto img = util::getImagePath("good-le-simple-3.1.badc0ffee0ddf00d.452a4267971a3928.app.release.bin"); + util::FileROMBackend rom(img); + std::array nodes; + kocherga::Bootloader bl(rom, sys, static_cast(std::filesystem::file_size(img)), true); + REQUIRE(bl.addNode(&nodes.at(0))); + REQUIRE(bl.addNode(&nodes.at(1))); + + REQUIRE(!bl.poll(500ms)); + REQUIRE(bl.getState() == kocherga::State::BootCanceled); // LINGER -- NO BOOT + + auto ai = *bl.getAppInfo(); + REQUIRE(0x452A'4267'971A'3928ULL == ai.image_crc); + REQUIRE(0xBADC'0FFE'E0DD'F00DULL == ai.vcs_revision_id); + REQUIRE(3 == ai.version.at(0)); + REQUIRE(1 == ai.version.at(1)); + REQUIRE(ai.isReleaseBuild()); + REQUIRE(!ai.isDirtyBuild()); + + nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, Transfer(444, {255, 255, 0}, 2222)); + REQUIRE(bl.poll(600ms) == kocherga::Final::Restart); + REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(444, {0, 0, 0, 0, 0, 0, 0}, 2222)); +} + +TEST_CASE("Bootloader-update-valid") +{ + using std::chrono_literals::operator""ms; + using mock::Node; + using mock::Transfer; + using kocherga::detail::dsdl::Heartbeat; + + const auto sys = getSysInfo(); + const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); + REQUIRE(std::filesystem::copy_file(img, "rom.img.tmp", std::filesystem::copy_options::overwrite_existing)); + util::FileROMBackend rom("rom.img.tmp"); + std::array nodes; + kocherga::Bootloader bl(rom, sys, static_cast(std::filesystem::file_size(img)), true); + REQUIRE(bl.addNode(&nodes.at(0))); + REQUIRE(bl.addNode(&nodes.at(1))); + + REQUIRE(!bl.poll(1'500ms)); + REQUIRE(bl.getState() == kocherga::State::BootCanceled); + REQUIRE(checkHeartbeat(nodes, 0, 1, Heartbeat::Health::Advisory, 0)); + + auto ai = *bl.getAppInfo(); + REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); + REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); + REQUIRE(5 == ai.version.at(0)); + REQUIRE(6 == ai.version.at(1)); + REQUIRE(!ai.isReleaseBuild()); + REQUIRE(ai.isDirtyBuild()); + + // REQUEST UPDATE + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( + // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_BEGIN_SOFTWARE_UPDATE, + // 'good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin')))) + nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, + Transfer(444, + {253, 255, 69, 103, 111, 111, 100, 45, 108, 101, 45, 51, 114, 100, 45, + 101, 110, 116, 114, 121, 45, 53, 46, 54, 46, 51, 51, 51, 51, 51, + 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, 46, 56, 98, 54, + 49, 57, 51, 56, 101, 101, 53, 102, 57, 48, 98, 49, 102, 46, 97, + 112, 112, 46, 100, 105, 114, 116, 121, 46, 98, 105, 110}, + 0)); + REQUIRE(!bl.poll(2'100ms)); + REQUIRE(checkHeartbeat(nodes, 1, 2, Heartbeat::Health::Nominal, 0)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == Transfer(444, {0, 0, 0, 0, 0, 0, 0}, 0)); + + // FIRST READ REQUEST + REQUIRE(!bl.poll(2'200ms)); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(nodes.at(1).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.file.Read_1_1.Request(0, + // uavcan.file.Path_2_0('good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin'))))) + const auto received = *nodes.at(1).popOutput(Node::Output::FileReadRequest); + const auto reference = + Transfer(1, + {0, 0, 0, 0, 0, 69, 103, 111, 111, 100, 45, 108, 101, 45, 51, 114, 100, 45, 101, + 110, 116, 114, 121, 45, 53, 46, 54, 46, 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, + 51, 51, 51, 51, 51, 51, 46, 56, 98, 54, 49, 57, 51, 56, 101, 101, 53, 102, 57, + 48, 98, 49, 102, 46, 97, 112, 112, 46, 100, 105, 114, 116, 121, 46, 98, 105, 110}, + 0); + std::cout << received.toString() << reference.toString() << std::endl; + REQUIRE(received == reference); + + // READ RESPONSE + // The serialized representation was constructed manually from the binary file + nodes.at(1).pushInput(Node::Input::FileReadResponse, + Transfer(0, + {0, 0, 128, 0, 72, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100, 63, 32, + 32, 32, 32, 199, 196, 192, 111, 20, 21, 68, 94, 65, 80, 68, 101, 115, 99, + 48, 48, 124, 194, 145, 71, 22, 198, 82, 134, 64, 2, 0, 0, 0, 0, 0, + 0, 1, 16, 0, 0, 210, 2, 150, 73, 13, 240, 221, 224, 254, 15, 220, 186, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, + 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, + 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126}, + 0)); + (void) bl.poll(2'300ms); // Results will appear on the SECOND poll. + REQUIRE(kocherga::Final::BootApp == *bl.poll(2'400ms)); + std::cout << nodes.at(0).popOutput(Node::Output::LogRecordMessage)->toString() << std::endl; + std::cout << nodes.at(1).popOutput(Node::Output::LogRecordMessage)->toString() << std::endl; + REQUIRE(bl.getState() == kocherga::State::BootDelay); + REQUIRE(kocherga::Final::BootApp == *bl.poll(2'500ms)); // All subsequent calls yield the same Final. + REQUIRE(kocherga::Final::BootApp == *bl.poll(2'600ms)); // Yep. + + // NEW APPLICATION IS NOW AVAILABLE + ai = *bl.getAppInfo(); + REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); + REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); + REQUIRE(5 == ai.version.at(0)); + REQUIRE(6 == ai.version.at(1)); + REQUIRE(!ai.isReleaseBuild()); + REQUIRE(ai.isDirtyBuild()); +} + +TEST_CASE("Bootloader-update-invalid") // NOLINT NOSONAR complexity threshold +{ + using std::chrono_literals::operator""s; + using std::chrono_literals::operator""ms; + using mock::Node; + using mock::Transfer; + using kocherga::detail::dsdl::Heartbeat; + + const auto sys = getSysInfo(); + const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); + REQUIRE(std::filesystem::copy_file(img, "rom.img.tmp", std::filesystem::copy_options::overwrite_existing)); + util::FileROMBackend rom("rom.img.tmp"); + std::array nodes; + kocherga::Bootloader bl(rom, sys, static_cast(std::filesystem::file_size(img)), false, 2s); + REQUIRE(bl.addNode(&nodes.at(0))); + + REQUIRE(!bl.poll(1'100ms)); + REQUIRE(bl.getState() == kocherga::State::BootDelay); + REQUIRE(checkHeartbeat(nodes, 0, 1, Heartbeat::Health::Nominal, 0)); + + auto ai = *bl.getAppInfo(); + REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); + REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); + REQUIRE(5 == ai.version.at(0)); + REQUIRE(6 == ai.version.at(1)); + REQUIRE(!ai.isReleaseBuild()); + REQUIRE(ai.isDirtyBuild()); + + // REQUEST UPDATE + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( + // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_BEGIN_SOFTWARE_UPDATE, 'bad-le-crc-x3.bin')))) + nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, + Transfer(111, + {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, + 99, 114, 99, 45, 120, 51, 46, 98, 105, 110}, + 1111)); + REQUIRE(!bl.poll(1'600ms)); + REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(111, {0, 0, 0, 0, 0, 0, 0}, 1111)); + + REQUIRE(!bl.poll(2'100ms)); + REQUIRE(checkHeartbeat(nodes, 1, 2, Heartbeat::Health::Nominal, 1)); + + // FIRST READ REQUEST + REQUIRE(!bl.poll(2'200ms)); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.file.Read_1_1.Request(0, + // uavcan.file.Path_2_0('bad-le-crc-x3.bin'))))) + auto received = *nodes.at(0).popOutput(Node::Output::FileReadRequest); + auto reference = + Transfer(1, + {0, 0, 0, 0, 0, 17, 98, 97, 100, 45, 108, 101, 45, 99, 114, 99, 45, 120, 51, 46, 98, 105, 110}, + 1111); + std::cout << received.toString() << reference.toString() << std::endl; + REQUIRE(received == reference); + + // READ RESPONSE + // The serialized representation was constructed manually from the binary file: bad-le-crc-x3.bin + nodes.at(0).pushInput(Node::Input::FileReadResponse, + Transfer(0, + { + 0x00, 0x00, 0x00, 0x01, 0x48, 0x65, 0x6c, 0x6c, 0x6f, 0x20, 0x77, 0x6f, 0x72, + 0x6c, 0x64, 0x3f, 0x20, 0x20, 0x20, 0x20, 0xc7, 0xc4, 0xc0, 0x6f, 0x14, 0x15, + 0x44, 0x5e, 0x88, 0x7a, 0x2e, 0xd0, 0x7e, 0xb1, 0x8c, 0xbe, 0xff, 0xff, 0xff, + 0xff, 0xff, 0x00, 0x00, 0x00, 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, + 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x10, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x48, + 0x65, 0x6c, 0x6c, 0x6f, 0x20, 0x77, 0x6f, 0x72, 0x6c, 0x64, 0x3f, 0x20, 0x20, + 0x20, 0x20, 0xc7, 0xc4, 0xc0, 0x6f, 0x14, 0x15, 0x44, 0x5e, 0x88, 0x7a, 0x2e, + 0xd0, 0x7e, 0xb1, 0x8c, 0xbe, 0x68, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x0d, 0xf0, 0xdd, 0xe0, 0xfe, 0x0f, 0xdc, 0xba, 0x00, 0x00, 0x00, 0x00, 0x01, + 0x00, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x00, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, + 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x48, 0x65, 0x6c, 0x6c, 0x6f, 0x20, + 0x77, 0x6f, 0x72, 0x6c, 0x64, 0x3f, 0x20, 0x20, 0x20, 0x20, 0xc7, 0xc4, 0xc0, + 0x6f, 0x14, 0x15, 0x44, 0x5e, 0x88, 0x7a, 0x2e, 0xd0, 0x7e, 0xb1, 0x8c, 0xbe, + }, + 1111)); + REQUIRE(!bl.poll(3'100ms)); + REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); + REQUIRE(checkHeartbeat(nodes, 2, 3, Heartbeat::Health::Nominal, 1)); + REQUIRE(!bl.poll(3'200ms)); + REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); + + // SECOND READ REQUEST + REQUIRE(!bl.poll(3'300ms)); + received = *nodes.at(0).popOutput(Node::Output::FileReadRequest); + reference = Transfer(2, + {0, 1, 0, 0, 0, 17, 98, 97, 100, 45, 108, 101, 45, 99, 114, 99, 45, 120, 51, 46, 98, 105, 110}, + 1111); + std::cout << received.toString() << reference.toString() << std::endl; + REQUIRE(received == reference); + + // READ TIMEOUT, NO VALID APPLICATION TO BOOT + // Due to the large time leap, we will be getting a heartbeat every poll() from now on until the leap is corrected. + REQUIRE(!bl.poll(10'100ms)); + REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); + REQUIRE(checkHeartbeat(nodes, 3, 10, Heartbeat::Health::Warning, 0)); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + + // RESPONSE ERROR CODE + nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, + Transfer(112, + {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, + 99, 114, 99, 45, 120, 51, 46, 105, 109, 103}, + 2222)); + REQUIRE(!bl.poll(10'200ms)); + REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(112, {0, 0, 0, 0, 0, 0, 0}, 2222)); + REQUIRE(checkHeartbeat(nodes, 4, 10, Heartbeat::Health::Nominal, 0)); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + nodes.at(0).pushInput(Node::Input::FileReadResponse, Transfer(2, {0xAD, 0xDE, 0x00, 0x00}, 2222)); + REQUIRE(!bl.poll(10'300ms)); + REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); + REQUIRE(checkHeartbeat(nodes, 5, 10, Heartbeat::Health::Warning, 0)); + REQUIRE(nodes.at(0).popOutput(Node::Output::FileReadRequest)); // Not checked + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + + // SEND REQUEST FAILURE + nodes.at(0).setFileReadResult(false); + nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, + Transfer(113, + {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, + 99, 114, 99, 45, 120, 51, 46, 105, 109, 103}, + 3333)); + REQUIRE(!bl.poll(10'400ms)); + REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(113, {0, 0, 0, 0, 0, 0, 0}, 3333)); + REQUIRE(checkHeartbeat(nodes, 6, 10, Heartbeat::Health::Nominal, 0)); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(!bl.poll(10'500ms)); + REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); + (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); + REQUIRE(nodes.at(0).popOutput(Node::Output::FileReadRequest)); // Not checked + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + + // ROM WRITE FAILURE + nodes.at(0).setFileReadResult(true); + rom.enableFailureInjection(true); + nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, + Transfer(114, + {253, 255, 17, 98, 97, 100, 45, 108, 101, 45, + 99, 114, 99, 45, 120, 51, 46, 105, 109, 103}, + 3210)); + REQUIRE(!bl.poll(10'600ms)); + (void) nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse); + (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); + (void) nodes.at(0).popOutput(Node::Output::LogRecordMessage); + REQUIRE(!bl.poll(10'700ms)); + REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); + (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); + (void) nodes.at(0).popOutput(Node::Output::LogRecordMessage); + REQUIRE(nodes.at(0).popOutput(Node::Output::FileReadRequest)); + nodes.at(0).pushInput(Node::Input::FileReadResponse, + Transfer(3, + {0x00, 0x00, 0x09, 0x00, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa}, + 3210)); + REQUIRE(!bl.poll(10'800ms)); + (void) nodes.at(0).popOutput(Node::Output::HeartbeatMessage); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(!bl.poll(11'100ms)); + REQUIRE(checkHeartbeat(nodes, {}, 11, Heartbeat::Health::Warning, 0)); + REQUIRE(bl.getState() == kocherga::State::NoAppToBoot); + REQUIRE(!bl.getAppInfo()); +} + +TEST_CASE("Bootloader-trigger") +{ + using std::chrono_literals::operator""s; + using std::chrono_literals::operator""ms; + using mock::Node; + using mock::Transfer; + using kocherga::detail::dsdl::Heartbeat; + + const auto sys = getSysInfo(); + const auto img = util::getImagePath("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); + REQUIRE(std::filesystem::copy_file(img, "rom.img.tmp", std::filesystem::copy_options::overwrite_existing)); + util::FileROMBackend rom("rom.img.tmp"); + std::array nodes; + kocherga::Bootloader bl(rom, sys, static_cast(std::filesystem::file_size(img)), false, 1s); + REQUIRE(bl.addNode(&nodes.at(0))); + REQUIRE(bl.addNode(&nodes.at(1))); + REQUIRE(bl.addNode(&nodes.at(2))); + REQUIRE(!bl.addNode(&nodes.at(2))); // Double registration has no effect. + + REQUIRE(!bl.poll(100ms)); + REQUIRE(bl.getState() == kocherga::State::BootDelay); + + auto ai = *bl.getAppInfo(); + REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); + REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); + REQUIRE(5 == ai.version.at(0)); + REQUIRE(6 == ai.version.at(1)); + REQUIRE(!ai.isReleaseBuild()); + REQUIRE(ai.isDirtyBuild()); + + // MANUAL UPDATE TRIGGER + const auto* const path = + reinterpret_cast("good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin"); + REQUIRE(bl.trigger(2, 2222, 69, path)); + REQUIRE(!bl.trigger(222, 2222, 69, path)); // No such node + REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); + REQUIRE(!bl.poll(1'100ms)); + REQUIRE(checkHeartbeat(nodes, 0, 1, Heartbeat::Health::Nominal, 1)); + + // FIRST READ REQUEST + REQUIRE(!bl.poll(1'200ms)); + REQUIRE(nodes.at(0).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(nodes.at(1).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(nodes.at(2).popOutput(Node::Output::LogRecordMessage)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); + REQUIRE(!nodes.at(1).popOutput(Node::Output::FileReadRequest)); + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.file.Read_1_1.Request(0, + // uavcan.file.Path_2_0('good-le-3rd-entry-5.6.3333333333333333.8b61938ee5f90b1f.app.dirty.bin'))))) + const auto received = *nodes.at(2).popOutput(Node::Output::FileReadRequest); + const auto reference = + Transfer(1, + {0, 0, 0, 0, 0, 69, 103, 111, 111, 100, 45, 108, 101, 45, 51, 114, 100, 45, 101, + 110, 116, 114, 121, 45, 53, 46, 54, 46, 51, 51, 51, 51, 51, 51, 51, 51, 51, 51, + 51, 51, 51, 51, 51, 51, 46, 56, 98, 54, 49, 57, 51, 56, 101, 101, 53, 102, 57, + 48, 98, 49, 102, 46, 97, 112, 112, 46, 100, 105, 114, 116, 121, 46, 98, 105, 110}, + 2222); + std::cout << received.toString() << reference.toString() << std::endl; + REQUIRE(received == reference); + + // READ RESPONSE + // The serialized representation was constructed manually from the binary file + nodes.at(2).pushInput(Node::Input::FileReadResponse, + Transfer(0, + {0, 0, 128, 0, 72, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100, 63, 32, + 32, 32, 32, 199, 196, 192, 111, 20, 21, 68, 94, 65, 80, 68, 101, 115, 99, + 48, 48, 124, 194, 145, 71, 22, 198, 82, 134, 64, 2, 0, 0, 0, 0, 0, + 0, 1, 16, 0, 0, 210, 2, 150, 73, 13, 240, 221, 224, 254, 15, 220, 186, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, + 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, + 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126}, + 2222)); + (void) bl.poll(1'300ms); // Results will appear on the SECOND poll. + REQUIRE(bl.getState() == kocherga::State::BootDelay); + REQUIRE(kocherga::Final::BootApp == *bl.poll(2'400ms)); + std::cout << nodes.at(0).popOutput(Node::Output::LogRecordMessage)->toString() << std::endl; + std::cout << nodes.at(1).popOutput(Node::Output::LogRecordMessage)->toString() << std::endl; + std::cout << nodes.at(2).popOutput(Node::Output::LogRecordMessage)->toString() << std::endl; + REQUIRE(kocherga::Final::BootApp == *bl.poll(2'500ms)); // All subsequent calls yield the same Final. + REQUIRE(kocherga::Final::BootApp == *bl.poll(2'600ms)); // Yep. + + // NEW APPLICATION IS NOW AVAILABLE + ai = *bl.getAppInfo(); + REQUIRE(0x8B61'938E'E5F9'0B1FULL == ai.image_crc); + REQUIRE(0x3333'3333'3333'3333ULL == ai.vcs_revision_id); + REQUIRE(5 == ai.version.at(0)); + REQUIRE(6 == ai.version.at(1)); + REQUIRE(!ai.isReleaseBuild()); + REQUIRE(ai.isDirtyBuild()); + + // BAD TRIGGER, GOOD TRIGGER + REQUIRE(bl.trigger(&nodes.at(0), 0, 69, path)); + REQUIRE(bl.getState() == kocherga::State::AppUpdateInProgress); + const mock::Node stray_node; + REQUIRE(!bl.trigger(&stray_node, 0, 69, path)); +} diff --git a/tests/unit/test_main.cpp b/tests/unit/test_main.cpp new file mode 100644 index 0000000..6be5d12 --- /dev/null +++ b/tests/unit/test_main.cpp @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch.hpp" // NOLINT diff --git a/tests/unit/test_misc.cpp b/tests/unit/test_misc.cpp new file mode 100644 index 0000000..92e78aa --- /dev/null +++ b/tests/unit/test_misc.cpp @@ -0,0 +1,93 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include "util.hpp" +#include +#include +#include + +TEST_CASE("CRC") +{ + kocherga::detail::CRC64 crc; + const char* val = "12345"; + crc.update(reinterpret_cast(val), 5); // NOSONAR NOLINT reinterpret_cast + crc.update(nullptr, 0); + val = "6789"; + crc.update(reinterpret_cast(val), 4); // NOSONAR NOLINT reinterpret_cast + + REQUIRE(0x62EC'59E3'F1A4'F00AULL == crc.get()); + REQUIRE(crc.getBytes().at(0) == 0x62U); + REQUIRE(crc.getBytes().at(1) == 0xECU); + REQUIRE(crc.getBytes().at(2) == 0x59U); + REQUIRE(crc.getBytes().at(3) == 0xE3U); + REQUIRE(crc.getBytes().at(4) == 0xF1U); + REQUIRE(crc.getBytes().at(5) == 0xA4U); + REQUIRE(crc.getBytes().at(6) == 0xF0U); + REQUIRE(crc.getBytes().at(7) == 0x0AU); + + REQUIRE(!crc.isResidueCorrect()); + crc.update(crc.getBytes().data(), 8); + REQUIRE(crc.isResidueCorrect()); + REQUIRE(0xFCAC'BEBD'5931'A992ULL == (~crc.get())); +} + +TEST_CASE("VolatileStorage") +{ + struct Data + { + std::uint64_t a; + std::uint8_t b; + std::array c; + }; + static_assert(sizeof(Data) <= 16); + static_assert(kocherga::VolatileStorage::StorageSize == (sizeof(Data) + 8U)); + + std::array::StorageSize> arena{}; + + kocherga::VolatileStorage marshaller(arena.data()); + + // The storage is empty, checking + REQUIRE(!marshaller.take()); + + // Writing zeros and checking the representation + marshaller.store(Data()); + std::cout << util::makeHexDump(arena) << std::endl; + REQUIRE(std::all_of(arena.begin(), arena.begin() + 12, [](auto x) { return x == 0; })); + REQUIRE(std::any_of(arena.begin() + sizeof(Data), arena.end(), [](auto x) { return x != 0; })); + + // Reading and making sure it's erased afterwards + auto rd = marshaller.take(); + REQUIRE(rd); + REQUIRE(rd->a == 0); + REQUIRE(rd->b == 0); + REQUIRE(rd->c[0] == 0); + REQUIRE(rd->c[1] == 0); + REQUIRE(rd->c[2] == 0); + + std::cout << util::makeHexDump(arena) << std::endl; + REQUIRE(std::all_of(arena.begin(), arena.end(), [](auto x) { return x == 0xCAU; })); + REQUIRE(!marshaller.take()); + + // Writing non-zeros and checking the representation + marshaller.store({ + 0x11AD'EADB'ADC0'FFEE, + 123, + {{1, 2, 3}}, + }); + std::cout << util::makeHexDump(arena) << std::endl; + + // Reading and making sure it's erased afterwards + rd = marshaller.take(); + REQUIRE(rd); + REQUIRE(rd->a == 0x11AD'EADB'ADC0'FFEE); + REQUIRE(rd->b == 123); + REQUIRE(rd->c[0] == 1); + REQUIRE(rd->c[1] == 2); + REQUIRE(rd->c[2] == 3); + std::cout << util::makeHexDump(arena) << std::endl; + REQUIRE(std::all_of(arena.begin(), arena.end(), [](auto x) { return x == 0xCAU; })); + REQUIRE(!marshaller.take()); +} diff --git a/tests/unit/test_presenter.cpp b/tests/unit/test_presenter.cpp new file mode 100644 index 0000000..2427f03 --- /dev/null +++ b/tests/unit/test_presenter.cpp @@ -0,0 +1,388 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "kocherga.hpp" // NOLINT include order: include Kocherga first to ensure no headers are missed. +#include "catch.hpp" +#include "mock.hpp" +#include +#include + +/// GENERATION OF REFERENCE SERIALIZED REPRESENTATIONS. +/// Prepare the DSDL-generated packages: +/// $ yakut compile https://github.com/UAVCAN/public_regulated_data_types/archive/master.zip +/// Run PyUAVCAN: +/// >>> import pyuavcan, uavcan.node, uavcan.diagnostic, uavcan.file +/// >>> list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.Heartbeat_1_0( +/// mode=uavcan.node.Mode_1_0(uavcan.node.Mode_1_0.SOFTWARE_UPDATE), +/// health=uavcan.node.Health_1_0(uavcan.node.Health_1_0.ADVISORY), +/// uptime=1)))) +/// [1, 0, 0, 0, 1, 3, 0] +namespace +{ +class MockController : public kocherga::detail::IController +{ +public: + [[nodiscard]] auto popRebootRequestedFlag() + { + const auto out = reboot_requested_; + reboot_requested_ = false; + return out; + } + + [[nodiscard]] auto popUpdateRequestedFlag() + { + const auto out = update_requested_; + update_requested_ = false; + return out; + } + + [[nodiscard]] auto popFileReadResult() + { + const auto out = file_read_result_; + file_read_result_.reset(); + return out; + } + + void setAppInfo(const std::optional& value) { app_info_ = value; } + +private: + void reboot() override { reboot_requested_ = true; } + + void beginUpdate() override { update_requested_ = true; } + + void handleFileReadResult(const std::optional response) override + { + file_read_result_.emplace(response); + } + + [[nodiscard]] auto getAppInfo() const -> std::optional override { return app_info_; } + + bool reboot_requested_ = false; + bool update_requested_ = false; + + std::optional app_info_; + std::optional> file_read_result_; +}; + +} // namespace + +TEST_CASE("Presenter") // NOLINT NOSONAR complexity threshold +{ + using mock::Node; + using mock::Transfer; + + static constexpr auto coa_capacity = 222U; + std::array coa{}; + for (auto i = 0U; i < coa_capacity; i++) + { + coa.at(i) = static_cast(coa.size() - i); + } + + const kocherga::SystemInfo sys_info{ + kocherga::SemanticVersion{33, 11}, + {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}, + "com.zubax.kocherga.test.presenter", + 222, + coa.data(), + }; + + MockController controller; + std::array nodes; + kocherga::detail::Presenter pres(sys_info, controller); + REQUIRE(pres.addNode(&nodes.at(0))); + REQUIRE(pres.addNode(&nodes.at(1))); + REQUIRE(!pres.addNode(&nodes.at(1))); // Double registration has no effect. + + auto ts = std::chrono::microseconds{500'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.Heartbeat_1_0( + // mode=uavcan.node.Mode_1_0(uavcan.node.Mode_1_0.SOFTWARE_UPDATE), + // health=uavcan.node.Health_1_0(uavcan.node.Health_1_0.ADVISORY), + // uptime=1, vendor_specific_status_code=0x7d)))) + pres.setNodeHealth(kocherga::detail::dsdl::Heartbeat::Health::Advisory); + pres.setNodeVSSC(125); + ts = std::chrono::microseconds{1'500'000}; + pres.poll(ts); + for (auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + const auto tr = *n.popOutput(Node::Output::HeartbeatMessage); + std::cout << tr.toString() << std::endl; + REQUIRE(tr == Transfer(0, {1, 0, 0, 0, 1, 3, 125})); + } + + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.diagnostic.Record_1_1( + // severity=uavcan.diagnostic.Severity_1_0(uavcan.diagnostic.Severity_1_0.NOTICE), + // text='Hello world!')))) + pres.publishLogRecord(kocherga::detail::dsdl::Diagnostic::Severity::Notice, "Hello world!"); + for (auto& n : nodes) + { + const auto tr = *n.popOutput(Node::Output::LogRecordMessage); + std::cout << tr.toString() << std::endl; + REQUIRE(tr == + Transfer(0, {0, 0, 0, 0, 0, 0, 0, 3, 12, 72, 101, 108, 108, 111, 32, 119, 111, 114, 108, 100, 33})); + } + + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.diagnostic.Record_1_1( + // severity=uavcan.diagnostic.Severity_1_0(uavcan.diagnostic.Severity_1_0.CRITICAL), + // text='We are going to die :)')))) + pres.publishLogRecord(kocherga::detail::dsdl::Diagnostic::Severity::Critical, "We are going to die :)"); + for (auto& n : nodes) + { + const auto tr = *n.popOutput(Node::Output::LogRecordMessage); + std::cout << tr.toString() << std::endl; + REQUIRE(tr == Transfer(1, {0, 0, 0, 0, 0, 0, 0, 6, 22, 87, 101, 32, 97, 114, 101, 32, + 103, 111, 105, 110, 103, 32, 116, 111, 32, 100, 105, 101, 32, 58, 41})); + } + + // Fails because the remote node-ID is not yet known. + REQUIRE(!pres.requestFileRead(0xB'ADBA'DBADULL)); + + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( + // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_BEGIN_SOFTWARE_UPDATE, '/foo/bar/baz.app.bin')))) + nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, + Transfer(123, + {253, 255, 20, 47, 102, 111, 111, 47, 98, 97, 114, 47, + 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, + 3210)); + ts = std::chrono::microseconds{1'600'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(controller.popUpdateRequestedFlag()); + REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(123, {0, 0, 0, 0, 0, 0, 0}, 3210)); + + // The file location is known now. + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.file.Read_1_1.Request(0xbadbadbad, + // uavcan.file.Path_2_0('/foo/bar/baz.app.bin'))))) + REQUIRE(pres.requestFileRead(0xB'ADBA'DBADULL)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == + Transfer(1, + {173, 219, 186, 173, 11, 20, 47, 102, 111, 111, 47, 98, 97, + 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, + 3210)); + + // Invalid request serialization. File location specifier not updated. + nodes.at(0).pushInput(Node::Input::ExecuteCommandRequest, Transfer(333, std::vector(), 3210)); + ts = std::chrono::microseconds{1'700'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(!controller.popUpdateRequestedFlag()); + REQUIRE((*nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(333, {6, 0, 0, 0, 0, 0, 0}, 3210)); // Internal error. + REQUIRE(!nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)); + + // Reboot request. + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.ExecuteCommand_1_1.Request( + // uavcan.node.ExecuteCommand_1_1.Request.COMMAND_RESTART, '')))) + nodes.at(1).pushInput(Node::Input::ExecuteCommandRequest, Transfer(444, {255, 255, 0}, 2222)); + ts = std::chrono::microseconds{1'800'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(controller.popRebootRequestedFlag()); + REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)) == + Transfer(444, {0, 0, 0, 0, 0, 0, 0}, 2222)); + + // Node info request; app info not available. SR generation not shown. + nodes.at(0).pushInput(Node::Input::NodeInfoRequest, Transfer(555, std::vector{}, 3333)); + ts = std::chrono::microseconds{1'900'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE( + (*nodes.at(0).popOutput(Node::Output::NodeInfoResponse)) == + Transfer(555, + {1, 0, 33, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, + 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 33, 99, 111, 109, 46, 122, 117, 98, 97, 120, + 46, 107, 111, 99, 104, 101, 114, 103, 97, 46, 116, 101, 115, 116, 46, 112, 114, 101, 115, 101, + 110, 116, 101, 114, 0, 222, 222, 221, 220, 219, 218, 217, 216, 215, 214, 213, 212, 211, 210, 209, + 208, 207, 206, 205, 204, 203, 202, 201, 200, 199, 198, 197, 196, 195, 194, 193, 192, 191, 190, 189, + 188, 187, 186, 185, 184, 183, 182, 181, 180, 179, 178, 177, 176, 175, 174, 173, 172, 171, 170, 169, + 168, 167, 166, 165, 164, 163, 162, 161, 160, 159, 158, 157, 156, 155, 154, 153, 152, 151, 150, 149, + 148, 147, 146, 145, 144, 143, 142, 141, 140, 139, 138, 137, 136, 135, 134, 133, 132, 131, 130, 129, + 128, 127, 126, 125, 124, 123, 122, 121, 120, 119, 118, 117, 116, 115, 114, 113, 112, 111, 110, 109, + 108, 107, 106, 105, 104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 90, 89, + 88, 87, 86, 85, 84, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, 72, 71, 70, 69, + 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, + 48, 47, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 32, 31, 30, 29, + 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, + 8, 7, 6, 5, 4, 3, 2, 1}, + 3333)); + REQUIRE(!nodes.at(1).popOutput(Node::Output::ExecuteCommandResponse)); + + // Node info request; app info is available. + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.node.GetInfo_1_0.Response( + // protocol_version=uavcan.node.Version_1_0(1, 0), hardware_version=uavcan.node.Version_1_0(33, 11), + // software_version=uavcan.node.Version_1_0(3, 11), software_vcs_revision_id=0xDEADDEADDEADDEAD, + // unique_id=bytes(range(1, 17)), name='com.zubax.kocherga.test.presenter', + // software_image_crc=[0xBADC0FFEE0DDF00D], certificate_of_authenticity=bytes(range(1, 223)[::-1]))))) + controller.setAppInfo(kocherga::AppInfo{ + 0xBADC'0FFE'E0DD'F00DULL, + 0xD'EADBULL, + {}, + {3, 11}, + 2, + {}, + 1234567890, + 0xDEAD'DEAD'DEAD'DEADULL, + {}, + }); + nodes.at(1).pushInput(Node::Input::NodeInfoRequest, Transfer(666, std::vector{}, 1111)); + ts = std::chrono::microseconds{1'910'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(!nodes.at(0).popOutput(Node::Output::ExecuteCommandResponse)); + const Transfer node_info_reference(666, + {1, 0, 33, 11, 3, 11, 173, 222, 173, 222, 173, 222, 173, 222, 1, 2, + 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 33, 99, + 111, 109, 46, 122, 117, 98, 97, 120, 46, 107, 111, 99, 104, 101, 114, 103, + 97, 46, 116, 101, 115, 116, 46, 112, 114, 101, 115, 101, 110, 116, 101, 114, + 1, 13, 240, 221, 224, 254, 15, 220, 186, 222, 222, 221, 220, 219, 218, 217, + 216, 215, 214, 213, 212, 211, 210, 209, 208, 207, 206, 205, 204, 203, 202, 201, + 200, 199, 198, 197, 196, 195, 194, 193, 192, 191, 190, 189, 188, 187, 186, 185, + 184, 183, 182, 181, 180, 179, 178, 177, 176, 175, 174, 173, 172, 171, 170, 169, + 168, 167, 166, 165, 164, 163, 162, 161, 160, 159, 158, 157, 156, 155, 154, 153, + 152, 151, 150, 149, 148, 147, 146, 145, 144, 143, 142, 141, 140, 139, 138, 137, + 136, 135, 134, 133, 132, 131, 130, 129, 128, 127, 126, 125, 124, 123, 122, 121, + 120, 119, 118, 117, 116, 115, 114, 113, 112, 111, 110, 109, 108, 107, 106, 105, + 104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 90, 89, + 88, 87, 86, 85, 84, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, + 72, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, + 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, + 40, 39, 38, 37, 36, 35, 34, 33, 32, 31, 30, 29, 28, 27, 26, 25, + 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, + 8, 7, 6, 5, 4, 3, 2, 1}, + 1111); + const auto node_info_response = *nodes.at(1).popOutput(Node::Output::NodeInfoResponse); + REQUIRE(node_info_reference == node_info_response); + + // It's time for another heartbeat. + pres.setNodeHealth(kocherga::detail::dsdl::Heartbeat::Health::Warning); + pres.setNodeVSSC(0x0000); + ts = std::chrono::microseconds{2'100'000}; + pres.poll(ts); + for (auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + const auto tr = *n.popOutput(Node::Output::HeartbeatMessage); + std::cout << tr.toString() << std::endl; + REQUIRE(tr == Transfer(1, {2, 0, 0, 0, 3, 3, 0})); + } + + // Deliver the file read response. + // Remember that the second update request was rejected so the actual file location is the old one. + // This one will be rejected because it's the wrong node. + // list(b''.join(pyuavcan.dsdl.serialize(uavcan.file.Read_1_1.Response(uavcan.file.Error_1_0(0), b'The data.')))) + nodes.at(0).pushInput(Node::Input::FileReadResponse, + Transfer(0, {0, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); + ts = std::chrono::microseconds{2'200'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(!controller.popFileReadResult()); + // Correct node -- accepted. + nodes.at(1).pushInput(Node::Input::FileReadResponse, + Transfer(0, {0, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); + ts = std::chrono::microseconds{2'200'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + const kocherga::detail::dsdl::File::ReadResponse read_result = **controller.popFileReadResult(); + REQUIRE(read_result.isSuccessful()); + REQUIRE(9 == read_result.data_length); // The pointer is invalidated here, don't check it. + REQUIRE(nullptr != read_result.data); + // Further responses not accepted because no request is pending. + nodes.at(1).pushInput(Node::Input::FileReadResponse, + Transfer(0, {0, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); + ts = std::chrono::microseconds{2'300'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(!controller.popFileReadResult()); + + // File read error handling. + nodes.at(1).setFileReadResult(false); + REQUIRE(!pres.requestFileRead(123'456)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == + Transfer(2, + {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, + 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, + 3210)); + + // Successful request, but the response specifies a non-zero error code. + nodes.at(1).setFileReadResult(true); + REQUIRE(pres.requestFileRead(123'456)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == + Transfer(3, + {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, + 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, + 3210)); + nodes.at(1).pushInput(Node::Input::FileReadResponse, + Transfer(1, {1, 0, 9, 0, 84, 104, 101, 32, 100, 97, 116, 97, 46}, 3210)); + ts = std::chrono::microseconds{2'400'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(!*controller.popFileReadResult()); // Empty option. + + // Successful request, but the response times out. + nodes.at(1).setFileReadResult(true); + REQUIRE(pres.requestFileRead(123'456)); + REQUIRE(!nodes.at(0).popOutput(Node::Output::FileReadRequest)); + REQUIRE((*nodes.at(1).popOutput(Node::Output::FileReadRequest)) == + Transfer(4, + {64, 226, 1, 0, 0, 20, 47, 102, 111, 111, 47, 98, 97, + 114, 47, 98, 97, 122, 46, 97, 112, 112, 46, 98, 105, 110}, + 3210)); + ts = std::chrono::microseconds{2'500'000}; + pres.poll(ts); + for (const auto& n : nodes) + { + REQUIRE(n.getLastPollTime() == ts); + } + REQUIRE(!controller.popFileReadResult()); // Nothing yet. + ts = std::chrono::microseconds{9'000'000}; + pres.poll(ts); + for (auto& n : nodes) + { + REQUIRE(n.popOutput(Node::Output::HeartbeatMessage)); + REQUIRE(n.getLastPollTime() == ts); + } + pres.poll(ts); + REQUIRE(!*controller.popFileReadResult()); // Empty option. + REQUIRE(!nodes.at(0).wasRequestCanceled()); + REQUIRE(nodes.at(1).wasRequestCanceled()); +} diff --git a/tests/unit/test_util.cpp b/tests/unit/test_util.cpp new file mode 100644 index 0000000..af5afa6 --- /dev/null +++ b/tests/unit/test_util.cpp @@ -0,0 +1,101 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#include "util.hpp" +#include "catch.hpp" + +TEST_CASE("util::makeHexDump") +{ + REQUIRE("00000000 31 32 33 123 " == + util::makeHexDump(std::string("123"))); + + REQUIRE("00000000 30 31 32 33 34 35 36 37 38 39 61 62 63 64 65 66 0123456789abcdef\n" + "00000010 67 68 69 6a 6b 6c 6d 6e 6f 70 71 72 73 74 75 76 ghijklmnopqrstuv\n" + "00000020 77 78 79 7a 41 42 43 44 45 46 47 48 49 4a 4b 4c wxyzABCDEFGHIJKL\n" + "00000030 4d 4e 4f 50 51 52 53 54 55 56 57 58 59 5a MNOPQRSTUVWXYZ " == + util::makeHexDump(std::string("0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ"))); +} + +TEST_CASE("util::FileROMBackend") +{ + (void) std::filesystem::remove("test_rom.bin.tmp"); + + util::FileROMBackend back("test_rom.bin.tmp", 1024, 0xFFU); + + const std::vector ref(1024, std::byte{0xFF}); + REQUIRE(back.isSameImage(ref.data(), ref.size())); + + const std::vector ref2(123, std::byte{0xFF}); + REQUIRE(back.isSameImage(ref2.data(), ref2.size())); + + const std::vector ref3(123, std::byte{0xAA}); + REQUIRE(!back.isSameImage(ref3.data(), ref3.size())); + + const util::FileROMBackend existing("test_rom.bin.tmp"); + REQUIRE(existing.getSize() == 1024U); + REQUIRE(existing.isSameImage(ref.data(), ref.size())); + + kocherga::IROMBackend& interface = back; + + std::array buf1{}; + + REQUIRE(0 == back.getReadCount()); + REQUIRE(0 == back.getWriteCount()); + + // Reading + REQUIRE(32 == interface.read(0, buf1.data(), 32)); + REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 32, [](auto x) { return x == std::byte{0xFF}; })); + + REQUIRE(1024 == interface.read(0, buf1.data(), 1024)); + REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 1024, [](auto x) { return x == std::byte{0xFF}; })); + + REQUIRE(512 == interface.read(512, buf1.data(), 1024)); + REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 512, [](auto x) { return x == std::byte{0xFF}; })); + + REQUIRE(128 == interface.read(512, buf1.data(), 128)); + REQUIRE(std::all_of(buf1.begin(), buf1.begin() + 1024, [](auto x) { return x == std::byte{0xFF}; })); + + REQUIRE(0 == interface.read(1024, buf1.data(), 1)); + + REQUIRE(1024 == interface.read(0, buf1.data(), 2000)); + std::all_of(buf1.begin(), buf1.begin() + 1024, [](auto x) { return x == std::byte{0xFF}; }); + REQUIRE(buf1[1024] == std::byte{0}); + REQUIRE(buf1[1025] == std::byte{0}); + REQUIRE(buf1[1026] == std::byte{0}); + + REQUIRE(6 == back.getReadCount()); + REQUIRE(0 == back.getWriteCount()); + + // Writing + REQUIRE_THROWS_AS(interface.write(0, buf1.data(), 123), std::runtime_error); + REQUIRE_THROWS_AS(interface.endWrite(), std::runtime_error); + interface.beginWrite(); + buf1.fill(std::byte{0xAA}); + REQUIRE(128 == *interface.write(0, buf1.data(), 128)); + REQUIRE(128 == *interface.write(512, buf1.data(), 128)); + interface.endWrite(); + + REQUIRE(1024 == interface.read(0, buf1.data(), 2000)); + REQUIRE(std::all_of(buf1.begin() + 0, buf1.begin() + 128, [](auto x) { return x == std::byte{0xAA}; })); + REQUIRE(std::all_of(buf1.begin() + 128, buf1.begin() + 512, [](auto x) { return x == std::byte{0xFF}; })); + REQUIRE(std::all_of(buf1.begin() + 512, buf1.begin() + 640, [](auto x) { return x == std::byte{0xAA}; })); + + REQUIRE(1024 == static_cast(existing).read(0, buf1.data(), 2000)); + REQUIRE(std::all_of(buf1.begin() + 0, buf1.begin() + 128, [](auto x) { return x == std::byte{0xAA}; })); + REQUIRE(std::all_of(buf1.begin() + 128, buf1.begin() + 512, [](auto x) { return x == std::byte{0xFF}; })); + REQUIRE(std::all_of(buf1.begin() + 512, buf1.begin() + 640, [](auto x) { return x == std::byte{0xAA}; })); + + REQUIRE(7 == back.getReadCount()); + REQUIRE(3 == back.getWriteCount()); + + // Failure injection + interface.beginWrite(); + back.enableFailureInjection(true); + REQUIRE(!interface.write(0, buf1.data(), 128)); + back.enableFailureInjection(false); + REQUIRE(128 == *interface.write(0, buf1.data(), 128)); + + REQUIRE(7 == back.getReadCount()); + REQUIRE(5 == back.getWriteCount()); +} diff --git a/tests/util/util.hpp b/tests/util/util.hpp new file mode 100644 index 0000000..c429323 --- /dev/null +++ b/tests/util/util.hpp @@ -0,0 +1,281 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2020 Zubax Robotics. +// Author: Pavel Kirienko + +#pragma once + +#include "kocherga.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace util +{ +static constexpr std::uint8_t BitsPerByte = 8U; +static constexpr std::pair PrintableASCIIRange{32, 126}; + +template +inline auto makeHexDump(InputIterator begin, const InputIterator end) -> std::string +{ + static constexpr std::uint8_t BytesPerRow = BitsPerByte * 2; + std::uint32_t offset = 0; + std::ostringstream output; + bool first = true; + output << std::hex << std::setfill('0'); + do + { + if (first) + { + first = false; + } + else + { + output << std::endl; + } + + output << std::setw(BitsPerByte) << offset << " "; + offset += BytesPerRow; + + auto it = begin; + for (std::uint8_t i = 0; i < BytesPerRow; ++i) + { + if (i == BitsPerByte) + { + output << ' '; + } + + if (it != end) + { + output << std::setw(2) << static_cast(*it) << ' '; + ++it; + } + else + { + output << " "; + } + } + + output << " "; + for (std::uint8_t i = 0; i < BytesPerRow; ++i) + { + if (begin != end) + { + output << (((static_cast(*begin) >= PrintableASCIIRange.first) && + (static_cast(*begin) <= PrintableASCIIRange.second)) + ? static_cast(*begin) // NOSONAR intentional conversion to plain char + : '.'); + ++begin; + } + else + { + output << ' '; + } + } + } while (begin != end); + return output.str(); +} + +template +inline auto makeHexDump(const Container& cont) +{ + return makeHexDump(std::begin(cont), std::end(cont)); +} + +/// A ROM backend mapped onto a local file. +class FileROMBackend : public kocherga::IROMBackend +{ +public: + class Error : public std::runtime_error + { + public: + explicit Error(const std::string& x) : std::runtime_error(x) {} + }; + + /// This overload creates a new file filled as specified. If the file exists, it will be written over. + FileROMBackend(const std::filesystem::path& file_path, + const std::size_t rom_size, + const std::uint8_t memory_fill = static_cast((1U << BitsPerByte) - 1U)) : + path_(file_path), rom_size_(rom_size) + { + std::ofstream f(path_, std::ios::binary | std::ios::out | std::ios::trunc); + if (f) + { + const std::vector empty_image(rom_size, + static_cast( + memory_fill)); // NOSONAR intentional conversion to plain char + f.write(empty_image.data(), static_cast(empty_image.size())); + f.flush(); + } + else + { + throw Error("Could not init the ROM mapping file"); + } + } + + /// This overload opens an existing file. + explicit FileROMBackend(const std::filesystem::path& file_path) : + path_(file_path), rom_size_(static_cast(std::filesystem::file_size(file_path))) + { + checkFileHealth(); + } + + void enableFailureInjection(const bool enabled) { trigger_failure_ = enabled; } + + auto isSameImage(const std::byte* const reference, const std::size_t reference_size) const + { + checkFileHealth(); + if (std::ifstream f(path_, std::ios::binary | std::ios::in); f) + { + f.seekg(0); + std::vector buffer(reference_size, '\0'); + f.read(buffer.data(), static_cast(reference_size)); + return std::memcmp(reference, buffer.data(), reference_size) == 0; + } + throw Error("Could not open the ROM mapping file for verification"); + } + + [[nodiscard]] auto read(const std::size_t offset, std::byte* const out_data, const std::size_t size) const + -> std::size_t override + { + read_count_++; + const std::size_t out = ((offset + size) > rom_size_) ? (rom_size_ - offset) : size; + assert(out <= size); + assert((out + offset) <= rom_size_); + checkFileHealth(); + if (std::ifstream f(path_, std::ios::binary | std::ios::in); f) + { + f.seekg(static_cast(offset)); + f.read(reinterpret_cast(out_data), // NOLINT NOSONAR reinterpret_cast + static_cast(out)); + return out; + } + throw Error("Could not open the ROM emulation file for reading"); + } + + auto getReadCount() const { return read_count_; } + auto getWriteCount() const { return write_count_; } + + auto getSize() const { return rom_size_; } + +private: + void checkFileHealth() const + { + if (std::filesystem::file_size(path_) != rom_size_) + { + throw Error("Invalid size of the ROM mapping file"); + } + } + + void beginWrite() override + { + checkFileHealth(); + if (upgrade_in_progress_) + { + throw Error("Bad sequencing"); + } + upgrade_in_progress_ = true; + } + + void endWrite() override + { + checkFileHealth(); + if (!upgrade_in_progress_) + { + throw Error("Bad sequencing"); + } + upgrade_in_progress_ = false; + } + + [[nodiscard]] auto write(const std::size_t offset, const std::byte* const data, const std::size_t size) + -> std::optional override + { + write_count_++; + if (!upgrade_in_progress_) + { + throw Error("Bad sequencing"); + } + checkFileHealth(); + if (trigger_failure_) + { + // Can't use {} because of a bug in GCC: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=92092 + return std::optional(std::nullopt); // NOLINT + } + const std::size_t out = ((offset + size) > rom_size_) ? (rom_size_ - offset) : size; + assert(out <= size); + assert((out + offset) <= rom_size_); + if (std::ofstream f(path_, std::ios::binary | std::ios::out | std::ios::in); f) + { + f.seekp(static_cast(offset)); + f.write(reinterpret_cast(data), // NOLINT NOSONAR reinterpret_cast + static_cast(out)); + f.flush(); + return out; + } + throw Error("Could not open the ROM emulation file for writing"); + } + + const std::filesystem::path path_; + const std::size_t rom_size_; + + mutable std::uint64_t read_count_ = 0; + std::uint64_t write_count_ = 0; + bool upgrade_in_progress_ = false; + bool trigger_failure_ = false; +}; + +class EnvironmentError : public std::runtime_error +{ +public: + explicit EnvironmentError(const std::string& x) : std::runtime_error(x) {} +}; + +/// A safe wrapper over the standard getenv() that returns an empty option if the variable is not set. +inline auto getEnvironmentVariableMaybe(const std::string& name) -> std::optional +{ + if (auto* const st = std::getenv(name.c_str()); st != nullptr) // NOSONAR getenv is considered unsafe. + { + return std::string(st); + } + return {}; +} + +/// Like above but throws EnvironmentError if the variable is missing. +inline auto getEnvironmentVariable(const std::string& name) -> std::string +{ + if (const auto v = getEnvironmentVariableMaybe(name)) + { + return *v; + } + throw EnvironmentError("Environment variable is required but not set: " + name); +} + +inline auto getSourceDir() -> std::filesystem::path +{ + return getEnvironmentVariable("SOURCE_DIR"); +} + +inline auto getImagePath(const std::string& name) -> std::filesystem::path +{ + return util::getSourceDir() / "images" / name; +} + +template +inline auto getRandomInteger() -> std::enable_if_t, T> +{ + static std::random_device rd; + static std::mt19937 gen(rd()); + static std::uniform_int_distribution dis(0, std::numeric_limits::max()); + return static_cast(dis(gen)); +} + +} // namespace util diff --git a/tools/kocherga_image.py b/tools/kocherga_image.py new file mode 100755 index 0000000..b717e4b --- /dev/null +++ b/tools/kocherga_image.py @@ -0,0 +1,775 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2021 Zubax Robotics . All rights reserved. +# +# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +# following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +# disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the +# following disclaimer in the documentation and/or other materials provided with the distribution. +# 3. Neither the name PX4 nor the names of its contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE +# USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Based on make_can_boot_descriptor.py originally created by Ben Dyer, David Sidrane and Pavel Kirienko for PX4: +# https://github.com/PX4/PX4-Autopilot/blob/0033c0fc510a52deabe10ba769e39f97bec86f10/src/drivers/bootloaders/make_can_boot_descriptor.py +# Redesigned for Kocherga by Pavel Kirienko . + +""" +This script is a part of the Kocherga project. Kocherga is a compact, portable, MIT-licensed multi-transport UAVCAN +bootloader for deeply embedded systems. It is designed to be compatible with any UAVCAN transport, such as UAVCAN/CAN, +UAVCAN/serial, and so on. The UAVCAN/CAN transport in particular is designed to support the experimental version of +the protocol labeled UAVCAN v0, which is now obsolete and supported here only to ease the migration of legacy systems +to the stable version of the protocol. + +Kocherga requires that the application image (i.e., firmware image -- in this documentation, the terms 'application' +and 'firmware' are used interchangeably) contains a particular data structure called "application descriptor". +The structure may be located anywhere within the image provided that its offset from the beginning is aligned at +8 bytes. It is recommended to place it near the beginning of the image (i.e., at a lower offset) to reduce the +boot-up time. The definition of the application descriptor data structure is given below in the DSDL notation. +The byte order is native for the target platform; external tools can determine it by checking the value of the magic. +If the image contains more than one descriptor, only that which is located at the lower offset will be used. + + uint64 magic # Constant value used for locating the descriptor and detecting the byte order. + uint8[8] signature # Set to "APDesc00"; used for compatibility with legacy deployments. + uint64 image_crc64we # Shall be set to zero when building the image. + uint32 image_size # Shall be set to zero when building the image. + void32 # Used to contain 32-bit vcs_revision_id, now deprecated; see replacement below. + uint8[2] version_major_minor + uint8 flags # See below. + void8 + uint32 timestamp_utc # UTC UNIX timestamp when the application was built. + uint64 vcs_revision_id # The version control system revision identifier, e.g., a git hash. + void64 + void64 + + uint8 FLAG_RELEASE = 1 # Set if this is a release build, not debug. + uint8 FLAG_DIRTY = 2 # Set if the image is built from an uncommitted revision of the sources. + + uint64 MAGIC = 0x5E4415146FC0C4C7 + +In the hex editor, the structure appears as follows: + + | 01234567 89ABCDEF + --+ + 0| MMMMMMMM AAAAAAAA + 16| CCCCCCCC SSSS---- + 32| JNF-TTTT VVVVVVVV + 48| -------- -------- + 64| -------- -------- + +Every field except image size and image CRC can be set either at compile time or by this script. By default, the +script only sets the size and the CRC, leaving all other fields at their original values; the user can specify which +other fields to overwrite by setting the appropriate command-line options when invoking the script. + +The image size and image CRC shall be zeroed at compile time (this implies that the magic with the signature +shall be followed by 12 zero bytes); the values will be assigned by this script always. +Structures where these fields are non-zero are ignored by the script (considered invalid) in order to support the +use case where images containing valid magics are nested (e.g., when the application image contains a bootloader image). + +The script does not modify the source image file; instead, it copies the data into a new file which is named using +the following naming convention: + -....app[.flag[.flag[.flag]]].bin +Where: + basename -- the name of the input file without extension (may contain "-" as well). + major, minor -- application version number pair in the output descriptor (decimal). + revision -- the VCS revision ID in the output descriptor (hexadecimal, 16 chars). + crc -- the image CRC; it is also the unique identifier of this build (hexadecimal, 16 chars). + flag -- list of flag names: 'dirty', etc. +For example, an input file named 'com.zubax.telega-1.bin' results in: + com.zubax.telega-1-1.2.1122334455667788.1122334455667788.app.release.dirty.bin +This format is compatible with the automatic firmware update server implemented in the Yakut CLI tool. + +The script can be requested to copy the same descriptor into additional files. In this case, it would re-use the same +descriptor instead of computing it anew for every additional file. This feature is designed for patching ELF files +after linking to simplify debugging: after an ELF file is patched in this way, it can be simply uploaded onto the +target and it will boot successfully by virtue of having the correct application descriptor copied over from the raw +binary image. This procedure is called "side-patching". + +This script has no external dependencies. The script can be used as a module as well. +""" + +from __future__ import annotations +import os +import sys +import typing +import struct +import logging +import argparse +import functools +import dataclasses + +_logger = logging.getLogger(__name__) + + +@dataclasses.dataclass +class Flags: + # The flags shall be listed here from the least significant bit upwards. The serialization is automated. + release: bool + dirty: bool + + def pack(self) -> int: + return functools.reduce( + lambda a, b: a | b, ((1 << i) for i, f in enumerate(dataclasses.fields(self)) if getattr(self, f.name)), 0 + ) + + @staticmethod + def unpack(value: int) -> Flags: + return Flags(**{f.name: (value & (1 << idx)) != 0 for idx, f in enumerate(dataclasses.fields(Flags))}) + + +@dataclasses.dataclass +class AppDescriptor: + SIZE = 64 + ALIGNMENT = 8 + MAGIC = 0x5E44_1514_6FC0_C4C7 + SIGNATURE = b"APDesc00" + + image_crc: int + image_size: int + version: typing.Tuple[int, int] + flags: Flags + timestamp_utc: int + vcs_revision_id: int + + def pack(self, byte_order: str) -> bytes: + return self._get_marshaller(byte_order).pack( + self.MAGIC, + self.SIGNATURE, + self.image_crc, + self.image_size, + *self.version, + self.flags.pack(), + self.timestamp_utc, + self.vcs_revision_id, + ) + + @staticmethod + def unpack_from( + image: typing.Union[bytes, bytearray, memoryview], byte_order: str, offset: int = 0 + ) -> typing.Optional[AppDescriptor]: + try: + ( + magic, + signature, + image_crc, + image_size, + v_major, + v_minor, + flags, + ts_utc, + vcs_revision_id, + ) = AppDescriptor._get_marshaller(byte_order).unpack_from(image, offset) + except struct.error: + return None + return ( + AppDescriptor( + image_crc=image_crc, + image_size=image_size, + version=(v_major, v_minor), + flags=Flags.unpack(flags), + timestamp_utc=ts_utc, + vcs_revision_id=vcs_revision_id, + ) + if magic == AppDescriptor.MAGIC and signature == AppDescriptor.SIGNATURE + else None + ) + + @staticmethod + def get_search_prefix(byte_order: str, uninitialized_only: bool = False) -> bytes: + """ + An app descriptor can be found by scanning the image for this byte pattern, depending on the byte order. + If the uninitialized_only flag is set, the search prefix will match only unpopulated descriptors where + the CRC and the size are zero. Otherwise, any descriptor will match. + """ + tail = bytes(12) if uninitialized_only else b"" + return AppDescriptor.MAGIC.to_bytes(8, byte_order) + AppDescriptor.SIGNATURE + tail + + @staticmethod + def _get_marshaller(byte_order: str) -> struct.Struct: + byte_order = {"big": ">", "little": "<"}[byte_order] + out = struct.Struct(byte_order + "Q 8s Q L 4x BB B x L Q 16x") + assert out.size == AppDescriptor.SIZE + return out + + def __str__(self) -> str: + out = f"{self.version[0]}.{self.version[1]}.{self.vcs_revision_id:016x}.{self.image_crc:016x}.app" + for flag, value in dataclasses.asdict(self.flags).items(): + if value: + out += f".{flag}" + return out + + +class ImageModel: + PADDING_BYTE = b"\x00" + + def __init__(self, image: bytearray, app_descriptor_offset: int, byte_order: str): + """Do not construct instances manually. Use the factory method instead.""" + self._image = bytearray(image) + self._offset = int(app_descriptor_offset) + self._byte_order = str(byte_order) + + if AppDescriptor.unpack_from(self._image, self._byte_order, self._offset) is None: + raise ValueError("The provided image does not contain an app descriptor at the specified offset") + + if len(self._image) % AppDescriptor.ALIGNMENT != 0 or self._offset % AppDescriptor.ALIGNMENT != 0: + raise ValueError("Bad alignment") + + if len(self._image) <= AppDescriptor.SIZE or not (0 <= self._offset < len(self._image)): + raise ValueError("Bad sizing") + + @property + def byte_order(self) -> str: + return self._byte_order + + @property + def image(self) -> bytes: + return bytes(self._image) # Return copy to prevent mutation + + @property + def app_descriptor_offset(self) -> int: + return self._offset + + @property + def app_descriptor(self) -> AppDescriptor: + desc = AppDescriptor.unpack_from(self._image, self._byte_order, self._offset) + assert desc is not None, "Internal protocol violation" + return desc + + @app_descriptor.setter + def app_descriptor(self, value: AppDescriptor) -> None: + self._image[self._offset : self._offset + AppDescriptor.SIZE] = value.pack(self._byte_order) + + def update(self) -> None: + """ + Compute and overwrite the image size and the image CRC fields in the app descriptor. + """ + desc = self.app_descriptor + desc.image_crc = 0 + desc.image_size = len(self._image) + self._image[self._offset : self._offset + AppDescriptor.SIZE] = desc.pack(self._byte_order) + desc.image_crc = CRCComputer().add(self._image).value + self._image[self._offset : self._offset + AppDescriptor.SIZE] = desc.pack(self._byte_order) + + def validate_app_descriptor(self) -> bool: + image = bytearray(self.image) + desc = self.app_descriptor + expected_crc, desc.image_crc = desc.image_crc, 0 + image[self._offset : self._offset + AppDescriptor.SIZE] = desc.pack(self._byte_order) + return expected_crc == CRCComputer().add(image).value + + @staticmethod + def construct_from_image( + image: typing.Union[memoryview, bytes, bytearray], uninitialized_only: bool = False + ) -> typing.Optional[ImageModel]: + """ + The byte order will be detected automatically. + :param image: The source application image. + :param uninitialized_only: True if the image does not contain a valid descriptor yet: + expect CRC and size to be zero. This option is designed for use with freshly built binary images + which have not been processed yet. + """ + image = bytearray(image) + while len(image) % AppDescriptor.ALIGNMENT != 0: + image += ImageModel.PADDING_BYTE + assert len(image) % AppDescriptor.ALIGNMENT == 0 + + offset: int + byte_order: str + for bo in ["big", "little"]: + offset = image.find(AppDescriptor.get_search_prefix(bo, uninitialized_only=uninitialized_only)) + if (offset >= 0) and (offset % AppDescriptor.ALIGNMENT == 0): # TODO Look further if invalid + byte_order = bo + break + else: + return None + assert 0 <= offset < len(image) + + if AppDescriptor.unpack_from(image, byte_order, offset) is not None: + return ImageModel( + image=image, + app_descriptor_offset=offset, + byte_order=byte_order, + ) + + +class CRCComputer: + _MASK = 0xFFFFFFFFFFFFFFFF + # fmt: off + _TABLE = [ + 0x0000000000000000, 0x42F0E1EBA9EA3693, 0x85E1C3D753D46D26, 0xC711223CFA3E5BB5, 0x493366450E42ECDF, + 0x0BC387AEA7A8DA4C, 0xCCD2A5925D9681F9, 0x8E224479F47CB76A, 0x9266CC8A1C85D9BE, 0xD0962D61B56FEF2D, + 0x17870F5D4F51B498, 0x5577EEB6E6BB820B, 0xDB55AACF12C73561, 0x99A54B24BB2D03F2, 0x5EB4691841135847, + 0x1C4488F3E8F96ED4, 0x663D78FF90E185EF, 0x24CD9914390BB37C, 0xE3DCBB28C335E8C9, 0xA12C5AC36ADFDE5A, + 0x2F0E1EBA9EA36930, 0x6DFEFF5137495FA3, 0xAAEFDD6DCD770416, 0xE81F3C86649D3285, 0xF45BB4758C645C51, + 0xB6AB559E258E6AC2, 0x71BA77A2DFB03177, 0x334A9649765A07E4, 0xBD68D2308226B08E, 0xFF9833DB2BCC861D, + 0x388911E7D1F2DDA8, 0x7A79F00C7818EB3B, 0xCC7AF1FF21C30BDE, 0x8E8A101488293D4D, 0x499B3228721766F8, + 0x0B6BD3C3DBFD506B, 0x854997BA2F81E701, 0xC7B97651866BD192, 0x00A8546D7C558A27, 0x4258B586D5BFBCB4, + 0x5E1C3D753D46D260, 0x1CECDC9E94ACE4F3, 0xDBFDFEA26E92BF46, 0x990D1F49C77889D5, 0x172F5B3033043EBF, + 0x55DFBADB9AEE082C, 0x92CE98E760D05399, 0xD03E790CC93A650A, 0xAA478900B1228E31, 0xE8B768EB18C8B8A2, + 0x2FA64AD7E2F6E317, 0x6D56AB3C4B1CD584, 0xE374EF45BF6062EE, 0xA1840EAE168A547D, 0x66952C92ECB40FC8, + 0x2465CD79455E395B, 0x3821458AADA7578F, 0x7AD1A461044D611C, 0xBDC0865DFE733AA9, 0xFF3067B657990C3A, + 0x711223CFA3E5BB50, 0x33E2C2240A0F8DC3, 0xF4F3E018F031D676, 0xB60301F359DBE0E5, 0xDA050215EA6C212F, + 0x98F5E3FE438617BC, 0x5FE4C1C2B9B84C09, 0x1D14202910527A9A, 0x93366450E42ECDF0, 0xD1C685BB4DC4FB63, + 0x16D7A787B7FAA0D6, 0x5427466C1E109645, 0x4863CE9FF6E9F891, 0x0A932F745F03CE02, 0xCD820D48A53D95B7, + 0x8F72ECA30CD7A324, 0x0150A8DAF8AB144E, 0x43A04931514122DD, 0x84B16B0DAB7F7968, 0xC6418AE602954FFB, + 0xBC387AEA7A8DA4C0, 0xFEC89B01D3679253, 0x39D9B93D2959C9E6, 0x7B2958D680B3FF75, 0xF50B1CAF74CF481F, + 0xB7FBFD44DD257E8C, 0x70EADF78271B2539, 0x321A3E938EF113AA, 0x2E5EB66066087D7E, 0x6CAE578BCFE24BED, + 0xABBF75B735DC1058, 0xE94F945C9C3626CB, 0x676DD025684A91A1, 0x259D31CEC1A0A732, 0xE28C13F23B9EFC87, + 0xA07CF2199274CA14, 0x167FF3EACBAF2AF1, 0x548F120162451C62, 0x939E303D987B47D7, 0xD16ED1D631917144, + 0x5F4C95AFC5EDC62E, 0x1DBC74446C07F0BD, 0xDAAD56789639AB08, 0x985DB7933FD39D9B, 0x84193F60D72AF34F, + 0xC6E9DE8B7EC0C5DC, 0x01F8FCB784FE9E69, 0x43081D5C2D14A8FA, 0xCD2A5925D9681F90, 0x8FDAB8CE70822903, + 0x48CB9AF28ABC72B6, 0x0A3B7B1923564425, 0x70428B155B4EAF1E, 0x32B26AFEF2A4998D, 0xF5A348C2089AC238, + 0xB753A929A170F4AB, 0x3971ED50550C43C1, 0x7B810CBBFCE67552, 0xBC902E8706D82EE7, 0xFE60CF6CAF321874, + 0xE224479F47CB76A0, 0xA0D4A674EE214033, 0x67C58448141F1B86, 0x253565A3BDF52D15, 0xAB1721DA49899A7F, + 0xE9E7C031E063ACEC, 0x2EF6E20D1A5DF759, 0x6C0603E6B3B7C1CA, 0xF6FAE5C07D3274CD, 0xB40A042BD4D8425E, + 0x731B26172EE619EB, 0x31EBC7FC870C2F78, 0xBFC9838573709812, 0xFD39626EDA9AAE81, 0x3A28405220A4F534, + 0x78D8A1B9894EC3A7, 0x649C294A61B7AD73, 0x266CC8A1C85D9BE0, 0xE17DEA9D3263C055, 0xA38D0B769B89F6C6, + 0x2DAF4F0F6FF541AC, 0x6F5FAEE4C61F773F, 0xA84E8CD83C212C8A, 0xEABE6D3395CB1A19, 0x90C79D3FEDD3F122, + 0xD2377CD44439C7B1, 0x15265EE8BE079C04, 0x57D6BF0317EDAA97, 0xD9F4FB7AE3911DFD, 0x9B041A914A7B2B6E, + 0x5C1538ADB04570DB, 0x1EE5D94619AF4648, 0x02A151B5F156289C, 0x4051B05E58BC1E0F, 0x87409262A28245BA, + 0xC5B073890B687329, 0x4B9237F0FF14C443, 0x0962D61B56FEF2D0, 0xCE73F427ACC0A965, 0x8C8315CC052A9FF6, + 0x3A80143F5CF17F13, 0x7870F5D4F51B4980, 0xBF61D7E80F251235, 0xFD913603A6CF24A6, 0x73B3727A52B393CC, + 0x31439391FB59A55F, 0xF652B1AD0167FEEA, 0xB4A25046A88DC879, 0xA8E6D8B54074A6AD, 0xEA16395EE99E903E, + 0x2D071B6213A0CB8B, 0x6FF7FA89BA4AFD18, 0xE1D5BEF04E364A72, 0xA3255F1BE7DC7CE1, 0x64347D271DE22754, + 0x26C49CCCB40811C7, 0x5CBD6CC0CC10FAFC, 0x1E4D8D2B65FACC6F, 0xD95CAF179FC497DA, 0x9BAC4EFC362EA149, + 0x158E0A85C2521623, 0x577EEB6E6BB820B0, 0x906FC95291867B05, 0xD29F28B9386C4D96, 0xCEDBA04AD0952342, + 0x8C2B41A1797F15D1, 0x4B3A639D83414E64, 0x09CA82762AAB78F7, 0x87E8C60FDED7CF9D, 0xC51827E4773DF90E, + 0x020905D88D03A2BB, 0x40F9E43324E99428, 0x2CFFE7D5975E55E2, 0x6E0F063E3EB46371, 0xA91E2402C48A38C4, + 0xEBEEC5E96D600E57, 0x65CC8190991CB93D, 0x273C607B30F68FAE, 0xE02D4247CAC8D41B, 0xA2DDA3AC6322E288, + 0xBE992B5F8BDB8C5C, 0xFC69CAB42231BACF, 0x3B78E888D80FE17A, 0x7988096371E5D7E9, 0xF7AA4D1A85996083, + 0xB55AACF12C735610, 0x724B8ECDD64D0DA5, 0x30BB6F267FA73B36, 0x4AC29F2A07BFD00D, 0x08327EC1AE55E69E, + 0xCF235CFD546BBD2B, 0x8DD3BD16FD818BB8, 0x03F1F96F09FD3CD2, 0x41011884A0170A41, 0x86103AB85A2951F4, + 0xC4E0DB53F3C36767, 0xD8A453A01B3A09B3, 0x9A54B24BB2D03F20, 0x5D45907748EE6495, 0x1FB5719CE1045206, + 0x919735E51578E56C, 0xD367D40EBC92D3FF, 0x1476F63246AC884A, 0x568617D9EF46BED9, 0xE085162AB69D5E3C, + 0xA275F7C11F7768AF, 0x6564D5FDE549331A, 0x279434164CA30589, 0xA9B6706FB8DFB2E3, 0xEB46918411358470, + 0x2C57B3B8EB0BDFC5, 0x6EA7525342E1E956, 0x72E3DAA0AA188782, 0x30133B4B03F2B111, 0xF7021977F9CCEAA4, + 0xB5F2F89C5026DC37, 0x3BD0BCE5A45A6B5D, 0x79205D0E0DB05DCE, 0xBE317F32F78E067B, 0xFCC19ED95E6430E8, + 0x86B86ED5267CDBD3, 0xC4488F3E8F96ED40, 0x0359AD0275A8B6F5, 0x41A94CE9DC428066, 0xCF8B0890283E370C, + 0x8D7BE97B81D4019F, 0x4A6ACB477BEA5A2A, 0x089A2AACD2006CB9, 0x14DEA25F3AF9026D, 0x562E43B4931334FE, + 0x913F6188692D6F4B, 0xD3CF8063C0C759D8, 0x5DEDC41A34BBEEB2, 0x1F1D25F19D51D821, 0xD80C07CD676F8394, + 0x9AFCE626CE85B507, + ] + # fmt: on + + def __init__(self): + assert len(self._TABLE) == 256 + self._crc = 0 + + def add(self, data: typing.Iterable[int]) -> CRCComputer: + mask = self._MASK + val = self._crc ^ mask + table = self._TABLE + for b in data: + val = (table[b ^ (val >> 56)] ^ (val << 8)) & mask + self._crc = val ^ mask + assert 0 <= self._crc < 2 ** 64 + return self + + @property + def value(self) -> int: + return self._crc + + +def _parse_version(x: str) -> typing.Tuple[int, int]: + major, minor = (int(v) for v in x.replace(",", ".").split(".")) + return major, minor + + +def _get_output_file_name(input_file_name: str, descriptor: AppDescriptor) -> str: + base_stem = os.path.basename(input_file_name).rsplit(".", 1)[0] + return f"{base_stem}-{descriptor}.bin" + + +def _main() -> int: + parser = argparse.ArgumentParser(formatter_class=argparse.RawTextHelpFormatter, description=globals()["__doc__"]) + parser.add_argument( + "firmware_image", + metavar="PATH", + help="The name of the firmware image binary file where the descriptor should be written. " + 'Use the special value "self-test" to run the self-test and exit immediately afterwards.', + ) + # Field setter options. + parser.add_argument( + "--assign-version", + type=_parse_version, + metavar="MAJOR.MINOR", + help="Overwrite the version fields, major and minor. Values can be dot- or comma-separated.", + ) + parser.add_argument( + "--assign-flag-release", + type=int, + metavar="FLAG", + help="Overwrite the release build flag in the app descriptor.", + ) + parser.add_argument( + "--assign-flag-dirty", type=int, metavar="FLAG", help="Overwrite the dirty build flag in the app descriptor." + ) + parser.add_argument( + "--assign-timestamp", + type=lambda x: int(x, 0), + metavar="TIMESTAMP", + help="Overwrite the build timestamp. The value should be a UNIX timestamp in UTC.", + ) + parser.add_argument( + "--assign-vcs-revision-id", + type=lambda x: int(x, 0), + metavar="VCS_REVISION_ID", + help='Overwrite the VCS revision ID field. Hexadecimals accepted if prefixed with "0x".', + ) + # Other options. + parser.add_argument( + "--verbose", "-v", action="count", default=0, help="Enable verbose output. Twice for extra verbosity." + ) + parser.add_argument( + "--side-patch", + default=[], + action="append", + metavar="PATH", + help="File(s) where the same descriptor will be copied into (e.g., ELF executables). " + "Use multiple times to patch several files at once.", + ) + args = parser.parse_args() + + logging.basicConfig( + stream=sys.stderr, + format="%(message)s", + level={ + 0: logging.WARNING, + 1: logging.INFO, + }.get(args.verbose, logging.DEBUG), + ) + + if args.firmware_image == "self-test": + _test() + return 0 + + # Read the input file. All operations done in-memory. + with open(args.firmware_image, "rb") as in_file: + model = ImageModel.construct_from_image(in_file.read(), uninitialized_only=True) + if not model: + existing_model = ImageModel.construct_from_image(in_file.read()) + _logger.fatal( + f"An uninitialized app descriptor could not be found in {args.firmware_image!r}. " + f"Existing app descriptor: {existing_model.app_descriptor if existing_model else None!r}" + ) + return 1 + + # Populate the fields as requested. The size and CRC are managed automatically by the image model class. + desc = model.app_descriptor + _logger.debug(f"Original app descriptor: {desc!r}") + if args.assign_version is not None: + assert len(args.assign_version) == 2 + assert isinstance(args.assign_version[0], int) and isinstance(args.assign_version[1], int) + desc.version = args.assign_version + if args.assign_flag_release is not None: + desc.flags.release = bool(args.assign_flag_release) + if args.assign_flag_dirty is not None: + desc.flags.dirty = bool(args.assign_flag_dirty) + if args.assign_timestamp is not None: + desc.timestamp_utc = int(args.assign_timestamp) + if args.assign_vcs_revision_id is not None: + desc.vcs_revision_id = int(args.assign_vcs_revision_id) + + # Update the image and recompute the CRC with the new fields in the descriptor. + model.app_descriptor = desc + del desc + model.update() + assert len(model.image) == model.app_descriptor.image_size, "Internal logic error" + assert model.validate_app_descriptor(), "Internal logic error: output image validation failed" + _logger.info(f"Detected byte order: {model.byte_order}") + _logger.info(f"App descriptor offset: {model.app_descriptor_offset}") + _logger.info(f"Final app descriptor: {model.app_descriptor!r}") + + # Write the resulting image into the output file. + out_name = _get_output_file_name(args.firmware_image, model.app_descriptor) + with open(out_name, "wb") as out_file: + assert model.validate_app_descriptor(), "Internal logic error: output image validation failed" + out_file.write(model.image) + _logger.info(f"Output image written into {out_name!r}") + + # Perform the side-patching. + for path in args.side_patch: + with open(path, "rb") as f: + data = bytearray(f.read()) + offset = data.find(AppDescriptor.get_search_prefix(model.byte_order, uninitialized_only=True)) + if offset < 0: + _logger.fatal(f"Side-patching failure: an uninitialized app descriptor could not be found in {path!r}") + return 1 + _logger.info(f"Side-patching {path!r} at offset {offset} bytes") + data[offset : offset + AppDescriptor.SIZE] = model.app_descriptor.pack(model.byte_order) + with open(path, "wb") as f: + f.write(data) + + return 0 + + +def _test() -> None: + assert 0x62EC59E3F1A4F00A == CRCComputer().add(b"123456").add(b"").add(b"789").value + + assert (1, 4) == _parse_version("1,4") + assert (1, 4) == _parse_version("1.4") + assert (1, 4) == _parse_version(" 1 , 4 ") + try: + _parse_version("1,4,7") + except ValueError: + pass + else: + assert False + + assert Flags(release=False, dirty=False).pack() == 0 + assert Flags(release=True, dirty=False).pack() == 1 + assert Flags(release=False, dirty=True).pack() == 2 + assert Flags(release=True, dirty=True).pack() == 3 + assert Flags.unpack(0) == Flags(release=False, dirty=False) + assert Flags.unpack(1) == Flags(release=True, dirty=False) + assert Flags.unpack(2) == Flags(release=False, dirty=True) + assert Flags.unpack(3) == Flags(release=True, dirty=True) + + desc = AppDescriptor( + image_crc=0, + image_size=0, + version=(0, 0), + flags=Flags.unpack(0), + timestamp_utc=0, + vcs_revision_id=0, + ) + assert ( + AppDescriptor.MAGIC.to_bytes(8, "little") + + b"APDesc00" + + (0).to_bytes(8, "little") + + (0).to_bytes(4, "little") + + bytes(4) + + bytes([0, 0]) + + bytes([0]) + + bytes(1) + + (0).to_bytes(4, "little") + + (0).to_bytes(8, "little") + + bytes(16) + ) == desc.pack("little") + desc.image_crc = 0xDEADBEEF_0DDC0FFE + desc.image_size = 0xAABBCCDD + desc.version = 42, 95 + desc.flags.dirty = True + desc.timestamp_utc = 1234567890 + desc.vcs_revision_id = 0x1122334455667788 + assert ( + AppDescriptor.MAGIC.to_bytes(8, "little") + + b"APDesc00" + + 0xDEADBEEF_0DDC0FFE .to_bytes(8, "little") + + 0xAABBCCDD .to_bytes(4, "little") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "little") + + 0x1122334455667788 .to_bytes(8, "little") + + bytes(16) + ) == desc.pack("little") + assert ( + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + 0xDEADBEEF_0DDC0FFE .to_bytes(8, "big") + + 0xAABBCCDD .to_bytes(4, "big") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16) + ) == desc.pack("big") + + desc = AppDescriptor.unpack_from( + AppDescriptor.MAGIC.to_bytes(8, "little") + + b"APDesc00" + + 0xDEADBEEF_0DDC0FFE .to_bytes(8, "little") + + 0xAABBCCDD .to_bytes(4, "little") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "little") + + 0x1122334455667788 .to_bytes(8, "little") + + bytes(16), + "little", + ) + assert desc.image_crc == 0xDEADBEEF_0DDC0FFE + assert desc.image_size == 0xAABBCCDD + assert desc.version == (42, 95) + assert not desc.flags.release + assert desc.flags.dirty + assert desc.timestamp_utc == 1234567890 + assert desc.vcs_revision_id == 0x1122334455667788 + + desc = AppDescriptor.unpack_from( + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + 0xDEADBEEF_0DDC0FFE .to_bytes(8, "big") + + 0xAABBCCDD .to_bytes(4, "big") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16), + "big", + ) + assert desc.image_crc == 0xDEADBEEF_0DDC0FFE + assert desc.image_size == 0xAABBCCDD + assert desc.version == (42, 95) + assert not desc.flags.release + assert desc.flags.dirty + assert desc.timestamp_utc == 1234567890 + assert desc.vcs_revision_id == 0x1122334455667788 + + assert not AppDescriptor.unpack_from(AppDescriptor.MAGIC.to_bytes(8, "big"), "big") + assert not AppDescriptor.unpack_from(AppDescriptor.MAGIC.to_bytes(8, "little") + AppDescriptor.SIGNATURE, "little") + + assert str(desc) == "42.95.1122334455667788.deadbeef0ddc0ffe.app.dirty" + desc.flags.dirty = False + assert str(desc) == "42.95.1122334455667788.deadbeef0ddc0ffe.app" + desc.flags.dirty = True + desc.flags.release = True + assert str(desc) == "42.95.1122334455667788.deadbeef0ddc0ffe.app.release.dirty" + + im = ImageModel.construct_from_image( + b"BEFORE MODEL " + + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + (0).to_bytes(8, "big") + + (0).to_bytes(4, "big") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16) + + b"AFTER MODEL", + uninitialized_only=True, + ) + assert im + crc = ( + CRCComputer() + .add( + b"BEFORE MODEL " + + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + (0).to_bytes(8, "big") + + (96).to_bytes(4, "big") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16) + + b"AFTER MODEL" + + ImageModel.PADDING_BYTE * 5 + ) + .value + ) + assert im.byte_order == "big" + assert im.app_descriptor_offset == 16 + assert im.app_descriptor == AppDescriptor( + image_crc=0, + image_size=0, + version=(42, 95), + flags=Flags.unpack(2), + timestamp_utc=1234567890, + vcs_revision_id=0x1122334455667788, + ) + assert not im.validate_app_descriptor() + im.update() + assert im.validate_app_descriptor() + assert im.app_descriptor == AppDescriptor( + image_crc=crc, + image_size=96, + version=(42, 95), + flags=Flags.unpack(2), + timestamp_utc=1234567890, + vcs_revision_id=0x1122334455667788, + ) + assert ( + b"BEFORE MODEL " + + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + crc.to_bytes(8, "big") + + (96).to_bytes(4, "big") + + bytes(4) + + bytes([42, 95]) + + bytes([2]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16) + + b"AFTER MODEL" + + ImageModel.PADDING_BYTE * 5 + ) == im.image + desc = im.app_descriptor + desc.version = 1, 5 + desc.flags.release = True + im.app_descriptor = desc + assert not im.validate_app_descriptor() + im.update() + assert im.validate_app_descriptor() + crc = ( + CRCComputer() + .add( + b"BEFORE MODEL " + + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + (0).to_bytes(8, "big") + + (96).to_bytes(4, "big") + + bytes(4) + + bytes([1, 5]) + + bytes([3]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16) + + b"AFTER MODEL" + + ImageModel.PADDING_BYTE * 5 + ) + .value + ) + assert ( + b"BEFORE MODEL " + + AppDescriptor.MAGIC.to_bytes(8, "big") + + b"APDesc00" + + crc.to_bytes(8, "big") + + (96).to_bytes(4, "big") + + bytes(4) + + bytes([1, 5]) + + bytes([3]) + + bytes(1) + + (1234567890).to_bytes(4, "big") + + 0x1122334455667788 .to_bytes(8, "big") + + bytes(16) + + b"AFTER MODEL" + + ImageModel.PADDING_BYTE * 5 + ) == im.image + + assert ( + _get_output_file_name( + "firmware-1.bin", + desc, + ) + == f"firmware-1-1.5.1122334455667788.{desc.image_crc:016x}.app.release.dirty.bin" + ) + + +if __name__ == "__main__": + sys.exit(_main()) diff --git a/tools/pyproject.toml b/tools/pyproject.toml new file mode 100644 index 0000000..8465be3 --- /dev/null +++ b/tools/pyproject.toml @@ -0,0 +1,6 @@ +[tool.black] +line-length = 120 +target-version = ['py38'] +include = ''' +(.*\.pyi?$) +''' diff --git a/xmodem_ymodem_protocol_reference.txt b/xmodem_ymodem_protocol_reference.txt deleted file mode 100644 index 24a6578..0000000 --- a/xmodem_ymodem_protocol_reference.txt +++ /dev/null @@ -1,2112 +0,0 @@ - - - - - 1 - - - - - XMODEM/YMODEM PROTOCOL REFERENCE - A compendium of documents describing the - - XMODEM and YMODEM - - File Transfer Protocols - - - - - This document was formatted 10-14-88. - - - - - - - - Edited by Chuck Forsberg - - - - - - - - - - This file may be redistributed without restriction - provided the text is not altered. - - Please distribute as widely as possible. - - Questions to Chuck Forsberg - - - - - - Omen Technology Inc - The High Reliability Software - 17505-V Sauvie Island Road - Portland Oregon 97231 - VOICE: 503-621-3406 :VOICE - TeleGodzilla BBS: 503-621-3746 Speed 19200(Telebit PEP),2400,1200,300 - CompuServe: 70007,2304 - GEnie: CAF - UUCP: ...!tektronix!reed!omen!caf - - - - - - - - - - - - - - - - 2 - - - - - 1. TOWER OF BABEL - - A "YMODEM Tower of Babel" has descended on the microcomputing community - bringing with it confusion, frustration, bloated phone bills, and wasted - man hours. Sadly, I (Chuck Forsberg) am partly to blame for this mess. - - As author of the early 1980s batch and 1k XMODEM extensions, I assumed - readers of earlier versions of this document would implement as much of - the YMODEM protocol as their programming skills and computing environments - would permit. This proved a rather naive assumption as programmers - motivated by competitive pressure implemented as little of YMODEM as - possible. Some have taken whatever parts of YMODEM that appealed to them, - applied them to MODEM7 Batch, Telink, XMODEM or whatever, and called the - result YMODEM. - - Jeff Garbers (Crosstalk package development director) said it all: "With - protocols in the public domain, anyone who wants to dink around with them - can go ahead." [1] - - Documents containing altered examples derived from YMODEM.DOC have added - to the confusion. In one instance, some self styled rewriter of history - altered the heading in YMODEM.DOC's Figure 1 from "1024 byte Packets" to - "YMODEM/CRC File Transfer Protocol". None of the XMODEM and YMODEM - examples shown in that document were correct. - - To put an end to this confusion, we must make "perfectly clear" what - YMODEM stands for, as Ward Christensen defined it in his 1985 coining of - the term. - - To the majority of you who read, understood, and respected Ward's - definition of YMODEM, I apologize for the inconvenience. - - 1.1 Definitions - - ARC ARC is a program that compresses one or more files into an archive - and extracts files from such archives. - - XMODEM refers to the file transfer etiquette introduced by Ward - Christensen's 1977 MODEM.ASM program. The name XMODEM comes from - Keith Petersen's XMODEM.ASM program, an adaptation of MODEM.ASM - for Remote CP/M (RCPM) systems. It's also called the MODEM or - MODEM2 protocol. Some who are unaware of MODEM7's unusual batch - file mode call it MODEM7. Other aliases include "CP/M Users' - Group" and "TERM II FTP 3". The name XMODEM caught on partly - because it is distinctive and partly because of media interest in - - - __________ - - 1. Page C/12, PC-WEEK July 12, 1987 - - - - - Chapter 1 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 3 - - - - bulletin board and RCPM systems where it was accessed with an - "XMODEM" command. This protocol is supported by every serious - communications program because of its universality, simplicity, - and reasonable performance. - - XMODEM/CRC replaces XMODEM's 1 byte checksum with a two byte Cyclical - Redundancy Check (CRC-16), giving modern error detection - protection. - - XMODEM-1k Refers to the XMODEM/CRC protocol with 1024 byte data blocks. - - YMODEM Refers to the XMODEM/CRC (optional 1k blocks) protocol with batch - transmission as described below. In a nutshell, YMODEM means - BATCH. - - YMODEM-g Refers to the streaming YMODEM variation described below. - - True YMODEM(TM) In an attempt to sort out the YMODEM Tower of Babel, Omen - Technology has trademarked the term True YMODEM(TM) to represent - the complete YMODEM protocol described in this document, including - pathname, length, and modification date transmitted in block 0. - Please contact Omen Technology about certifying programs for True - YMODEM(TM) compliance. - - ZMODEM uses familiar XMODEM/CRC and YMODEM technology in a new protocol - that provides reliability, throughput, file management, and user - amenities appropriate to contemporary data communications. - - ZOO Like ARC, ZOO is a program that compresses one or more files into - a "zoo archive". ZOO supports many different operating systems - including Unix and VMS. - - - - - - - - - - - - - - - - - - - - - - - - Chapter 1 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 4 - - - - 2. YMODEM MINIMUM REQUIREMENTS - - All programs claiming to support YMODEM must meet the following minimum - requirements: - - + The sending program shall send the pathname (file name) in block 0. - - + The pathname shall be a null terminated ASCII string as described - below. - - For those who are too lazy to read the entire document: - - + Unless specifically requested, only the file name portion is - sent. - - + No drive letter is sent. - - + Systems that do not distinguish between upper and lower case - letters in filenames shall send the pathname in lower case only. - - - + The receiving program shall use this pathname for the received file - name, unless explicitly overridden. - - + When the receiving program receives this block and successfully - opened the output file, it shall acknowledge this block with an ACK - character and then proceed with a normal XMODEM file transfer - beginning with a "C" or NAK tranmsitted by the receiver. - - + The sending program shall use CRC-16 in response to a "C" pathname - nak, otherwise use 8 bit checksum. - - + The receiving program must accept any mixture of 128 and 1024 byte - blocks within each file it receives. Sending programs may - arbitrarily switch between 1024 and 128 byte blocks. - - + The sending program must not change the length of an unacknowledged - block. - - + At the end of each file, the sending program shall send EOT up to ten - times until it receives an ACK character. (This is part of the - XMODEM spec.) - - + The end of a transfer session shall be signified by a null (empty) - pathname, this pathname block shall be acknowledged the same as other - pathname blocks. - - Programs not meeting all of these requirements are not YMODEM compatible, - and shall not be described as supporting YMODEM. - - Meeting these MINIMUM requirements does not guarantee reliable file - - - - Chapter 2 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 5 - - - - transfers under stress. Particular attention is called to XMODEM's single - character supervisory messages that are easily corrupted by transmission - errors. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Chapter 2 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 6 - - - - 3. WHY YMODEM? - - Since its development half a decade ago, the Ward Christensen modem - protocol has enabled a wide variety of computer systems to interchange - data. There is hardly a communications program that doesn't at least - claim to support this protocol. - - Advances in computing, modems and networking have revealed a number of - weaknesses in the original protocol: - - + The short block length caused throughput to suffer when used with - timesharing systems, packet switched networks, satellite circuits, - and buffered (error correcting) modems. - - + The 8 bit arithmetic checksum and other aspects allowed line - impairments to interfere with dependable, accurate transfers. - - + Only one file could be sent per command. The file name had to be - given twice, first to the sending program and then again to the - receiving program. - - + The transmitted file could accumulate as many as 127 extraneous - bytes. - - + The modification date of the file was lost. - - A number of other protocols have been developed over the years, but none - have displaced XMODEM to date: - - + Lack of public domain documentation and example programs have kept - proprietary protocols such as Blast, Relay, and others tightly bound - to the fortunes of their suppliers. - - + Complexity discourages the widespread application of BISYNC, SDLC, - HDLC, X.25, and X.PC protocols. - - + Performance compromises and complexity have limited the popularity of - the Kermit protocol, which was developed to allow file transfers in - environments hostile to XMODEM. - - The XMODEM protocol extensions and YMODEM Batch address some of these - weaknesses while maintaining most of XMODEM's simplicity. - - YMODEM is supported by the public domain programs YAM (CP/M), - YAM(CP/M-86), YAM(CCPM-86), IMP (CP/M), KMD (CP/M), rz/sz (Unix, Xenix, - VMS, Berkeley Unix, Venix, Xenix, Coherent, IDRIS, Regulus). Commercial - implementations include MIRROR, and Professional-YAM.[1] Communications - - - - - - - - Chapter 3 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 7 - - - - programs supporting these extensions have been in use since 1981. - - The 1k block length (XMODEM-1k) described below may be used in conjunction - with YMODEM Batch Protocol, or with single file transfers identical to the - XMODEM/CRC protocol except for minimal changes to support 1k blocks. - - Another extension is the YMODEM-g protocol. YMODEM-g provides batch - transfers with maximum throughput when used with end to end error - correcting media, such as X.PC and error correcting modems, including 9600 - bps units by TeleBit, U.S.Robotics, Hayes, Electronic Vaults, Data Race, - and others. - - To complete this tome, edited versions of Ward Christensen's original - protocol document and John Byrns's CRC-16 document are included for - reference. - - References to the MODEM or MODEM7 protocol have been changed to XMODEM to - accommodate the vernacular. In Australia, it is properly called the - Christensen Protocol. - - - 3.1 Some Messages from the Pioneer - - #: 130940 S0/Communications 25-Apr-85 18:38:47 - Sb: my protocol - Fm: Ward Christensen 76703,302 [2] - To: all - - Be aware the article[3] DID quote me correctly in terms of the phrases - like "not robust", etc. - - It was a quick hack I threw together, very unplanned (like everything I - do), to satisfy a personal need to communicate with "some other" people. - - ONLY the fact that it was done in 8/77, and that I put it in the public - domain immediately, made it become the standard that it is. - - - - - - - - __________________________________________________________________________ - - 1. Available for IBM PC,XT,AT, Unix and Xenix - - 2. Edited for typesetting appearance - - 3. Infoworld April 29 p. 16 - - - - - Chapter 3 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 8 - - - - I think its time for me to - - (1) document it; (people call me and say "my product is going to include - it - what can I 'reference'", or "I'm writing a paper on it, what do I put - in the bibliography") and - - (2) propose an "incremental extension" to it, which might take "exactly" - the form of Chuck Forsberg's YAM protocol. He wrote YAM in C for CP/M and - put it in the public domain, and wrote a batch protocol for Unix[4] called - rb and sb (receive batch, send batch), which was basically XMODEM with - (a) a record 0 containing filename date time and size - (b) a 1K block size option - (c) CRC-16. - - He did some clever programming to detect false ACK or EOT, but basically - left them the same. - - People who suggest I make SIGNIFICANT changes to the protocol, such as - "full duplex", "multiple outstanding blocks", "multiple destinations", etc - etc don't understand that the incredible simplicity of the protocol is one - of the reasons it survived to this day in as many machines and programs as - it may be found in! - - Consider the PC-NET group back in '77 or so - documenting to beat the band - - THEY had a protocol, but it was "extremely complex", because it tried to - be "all things to all people" - i.e. send binary files on a 7-bit system, - etc. I was not that "benevolent". I (emphasize > I < ) had an 8-bit UART, - so "my protocol was an 8-bit protocol", and I would just say "sorry" to - people who were held back by 7-bit limitations. ... - - Block size: Chuck Forsberg created an extension of my protocol, called - YAM, which is also supported via his public domain programs for UNIX - called rb and sb - receive batch and send batch. They cleverly send a - "block 0" which contains the filename, date, time, and size. - Unfortunately, its UNIX style, and is a bit weird[5] - octal numbers, etc. - BUT, it is a nice way to overcome the kludgy "echo the chars of the name" - introduced with MODEM7. Further, chuck uses CRC-16 and optional 1K - blocks. Thus the record 0, 1K, and CRC, make it a "pretty slick new - protocol" which is not significantly different from my own. - - Also, there is a catchy name - YMODEM. That means to some that it is the - "next thing after XMODEM", and to others that it is the Y(am)MODEM - - - __________ - - 4. VAX/VMS versions of these programs are also available. - - 5. The file length, time, and file mode are optional. The pathname and - file length may be sent alone if desired. - - - - - Chapter 3 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 9 - - - - protocol. I don't want to emphasize that too much - out of fear that - other mfgrs might think it is a "competitive" protocol, rather than an - "unaffiliated" protocol. Chuck is currently selling a much-enhanced - version of his CP/M-80 C program YAM, calling it Professional Yam, and its - for the PC - I'm using it right now. VERY slick! 32K capture buffer, - script, scrolling, previously captured text search, plus built-in commands - for just about everything - directory (sorted every which way), XMODEM, - YMODEM, KERMIT, and ASCII file upload/download, etc. You can program it - to "behave" with most any system - for example when trying a number for - CIS it detects the "busy" string back from the modem and substitutes a - diff phone # into the dialing string and branches back to try it. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Chapter 3 - - - - - - - - X/YMODEM Protocol Reference June 18 1988 10 - - - - 4. XMODEM PROTOCOL ENHANCEMENTS - - This chapter discusses the protocol extensions to Ward Christensen's 1982 - XMODEM protocol description document. - - The original document recommends the user be asked whether to continue - trying or abort after 10 retries. Most programs no longer ask the - operator whether he wishes to keep retrying. Virtually all correctable - errors are corrected within the first few retransmissions. If the line is - so bad that ten attempts are insufficient, there is a significant danger - of undetected errors. If the connection is that bad, it's better to - redial for a better connection, or mail a floppy disk. - - - 4.1 Graceful Abort - - The YAM and Professional-YAM X/YMODEM routines recognize a sequence of two - consecutive CAN (Hex 18) characters without modem errors (overrun, - framing, etc.) as a transfer abort command. This sequence is recognized - when is waiting for the beginning of a block or for an acknowledgement to - a block that has been sent. The check for two consecutive CAN characters - reduces the number of transfers aborted by line hits. YAM sends eight CAN - characters when it aborts an XMODEM, YMODEM, or ZMODEM protocol file - transfer. Pro-YAM then sends eight backspaces to delete the CAN - characters from the remote's keyboard input buffer, in case the remote had - already aborted the transfer and was awaiting a keyboarded command. - - - 4.2 CRC-16 Option - - The XMODEM protocol uses an optional two character CRC-16 instead of the - one character arithmetic checksum used by the original protocol and by - most commercial implementations. CRC-16 guarantees detection of all - single and double bit errors, all errors with an odd number of error - bits, all burst errors of length 16 or less, 99.9969% of all 17-bit error - bursts, and 99.9984 per cent of all possible longer error bursts. By - contrast, a double bit error, or a burst error of 9 bits or more can sneak - past the XMODEM protocol arithmetic checksum. - - The XMODEM/CRC protocol is similar to the XMODEM protocol, except that the - receiver specifies CRC-16 by sending C (Hex 43) instead of NAK when - requesting the FIRST block. A two byte CRC is sent in place of the one - byte arithmetic checksum. - - YAM's c option to the r command enables CRC-16 in single file reception, - corresponding to the original implementation in the MODEM7 series - programs. This remains the default because many commercial communications - programs and bulletin board systems still do not support CRC-16, - especially those written in Basic or Pascal. - - XMODEM protocol with CRC is accurate provided both sender and receiver - - - - Chapter 4 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 11 - - - - both report a successful transmission. The protocol is robust in the - presence of characters lost by buffer overloading on timesharing systems. - - The single character ACK/NAK responses generated by the receiving program - adapt well to split speed modems, where the reverse channel is limited to - ten per cent or less of the main channel's speed. - - XMODEM and YMODEM are half duplex protocols which do not attempt to - transmit information and control signals in both directions at the same - time. This avoids buffer overrun problems that have been reported by - users attempting to exploit full duplex asynchronous file transfer - protocols such as Blast. - - Professional-YAM adds several proprietary logic enhancements to XMODEM's - error detection and recovery. These compatible enhancements eliminate - most of the bad file transfers other programs make when using the XMODEM - protocol under less than ideal conditions. - - - 4.3 XMODEM-1k 1024 Byte Block - - Disappointing throughput downloading from Unix with YMODEM[1] lead to the - development of 1024 byte blocks in 1982. 1024 byte blocks reduce the - effect of delays from timesharing systems, modems, and packet switched - networks on throughput by 87.5 per cent in addition to decreasing XMODEM's - 3 per cent overhead (block number, CRC, etc.). - - Some environments cannot accept 1024 byte bursts, including some networks - and minicomputer ports. The longer block length should be an option. - - The choice to use 1024 byte blocks is expressed to the sending program on - its command line or selection menu.[2] 1024 byte blocks improve throughput - in many applications. - - An STX (02) replaces the SOH (01) at the beginning of the transmitted - block to notify the receiver of the longer block length. The transmitted - block contains 1024 bytes of data. The receiver should be able to accept - any mixture of 128 and 1024 byte blocks. The block number (in the second - and third bytes of the block) is incremented by one for each block - regardless of the block length. - - The sender must not change between 128 and 1024 byte block lengths if it - has not received a valid ACK for the current block. Failure to observe - - - __________ - - 1. The name hadn't been coined yet, but the protocol was the same. - - 2. See "KMD/IMP Exceptions to YMODEM" below. - - - - - Chapter 4 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 12 - - - - this restriction allows transmission errors to pass undetected. - - If 1024 byte blocks are being used, it is possible for a file to "grow" up - to the next multiple of 1024 bytes. This does not waste disk space if the - allocation granularity is 1k or greater. With YMODEM batch transmission, - the optional file length transmitted in the file name block allows the - receiver to discard the padding, preserving the exact file length and - contents. - - 1024 byte blocks may be used with batch file transmission or with single - file transmission. CRC-16 should be used with the k option to preserve - data integrity over phone lines. If a program wishes to enforce this - recommendation, it should cancel the transfer, then issue an informative - diagnostic message if the receiver requests checksum instead of CRC-16. - - Under no circumstances may a sending program use CRC-16 unless the - receiver commands CRC-16. - - Figure 1. XMODEM-1k Blocks - - SENDER RECEIVER - "sx -k foo.bar" - "foo.bar open x.x minutes" - C - STX 01 FE Data[1024] CRC CRC - ACK - STX 02 FD Data[1024] CRC CRC - ACK - STX 03 FC Data[1000] CPMEOF[24] CRC CRC - ACK - EOT - ACK - - Figure 2. Mixed 1024 and 128 byte Blocks - - SENDER RECEIVER - "sx -k foo.bar" - "foo.bar open x.x minutes" - C - STX 01 FE Data[1024] CRC CRC - ACK - STX 02 FD Data[1024] CRC CRC - ACK - SOH 03 FC Data[128] CRC CRC - ACK - SOH 04 FB Data[100] CPMEOF[28] CRC CRC - ACK - EOT - ACK - - - - - - Chapter 4 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 13 - - - - 5. YMODEM Batch File Transmission - - The YMODEM Batch protocol is an extension to the XMODEM/CRC protocol that - allows 0 or more files to be transmitted with a single command. (Zero - files may be sent if none of the requested files is accessible.) The - design approach of the YMODEM Batch protocol is to use the normal routines - for sending and receiving XMODEM blocks in a layered fashion similar to - packet switching methods. - - Why was it necessary to design a new batch protocol when one already - existed in MODEM7?[1] The batch file mode used by MODEM7 is unsuitable - because it does not permit full pathnames, file length, file date, or - other attribute information to be transmitted. Such a restrictive design, - hastily implemented with only CP/M in mind, would not have permitted - extensions to current areas of personal computing such as Unix, DOS, and - object oriented systems. In addition, the MODEM7 batch file mode is - somewhat susceptible to transmission impairments. - - As in the case of single a file transfer, the receiver initiates batch - file transmission by sending a "C" character (for CRC-16). - - The sender opens the first file and sends block number 0 with the - following information.[2] - - Only the pathname (file name) part is required for batch transfers. - - To maintain upwards compatibility, all unused bytes in block 0 must be set - to null. - - Pathname The pathname (conventionally, the file name) is sent as a null - terminated ASCII string. This is the filename format used by the - handle oriented MSDOS(TM) functions and C library fopen functions. - An assembly language example follows: - DB 'foo.bar',0 - No spaces are included in the pathname. Normally only the file name - stem (no directory prefix) is transmitted unless the sender has - selected YAM's f option to send the full pathname. The source drive - (A:, B:, etc.) is not sent. - - Filename Considerations: - - - - __________ - - 1. The MODEM7 batch protocol transmitted CP/M FCB bytes f1...f8 and - t1...t3 one character at a time. The receiver echoed these bytes as - received, one at a time. - - 2. Only the data part of the block is described here. - - - - - Chapter 5 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 14 - - - - + File names are forced to lower case unless the sending system - supports upper/lower case file names. This is a convenience for - users of systems (such as Unix) which store filenames in upper - and lower case. - - + The receiver should accommodate file names in lower and upper - case. - - + When transmitting files between different operating systems, - file names must be acceptable to both the sender and receiving - operating systems. - - If directories are included, they are delimited by /; i.e., - "subdir/foo" is acceptable, "subdir\foo" is not. - - Length The file length and each of the succeeding fields are optional.[3] - The length field is stored in the block as a decimal string counting - the number of data bytes in the file. The file length does not - include any CPMEOF (^Z) or other garbage characters used to pad the - last block. - - If the file being transmitted is growing during transmission, the - length field should be set to at least the final expected file - length, or not sent. - - The receiver stores the specified number of characters, discarding - any padding added by the sender to fill up the last block. - - Modification Date The mod date is optional, and the filename and length - may be sent without requiring the mod date to be sent. - - Iff the modification date is sent, a single space separates the - modification date from the file length. - - The mod date is sent as an octal number giving the time the contents - of the file were last changed, measured in seconds from Jan 1 1970 - Universal Coordinated Time (GMT). A date of 0 implies the - modification date is unknown and should be left as the date the file - is received. - - This standard format was chosen to eliminate ambiguities arising from - transfers between different time zones. - - - - - - __________ - - 3. Fields may not be skipped. - - - - - Chapter 5 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 15 - - - - Mode Iff the file mode is sent, a single space separates the file mode - from the modification date. The file mode is stored as an octal - string. Unless the file originated from a Unix system, the file mode - is set to 0. rb(1) checks the file mode for the 0x8000 bit which - indicates a Unix type regular file. Files with the 0x8000 bit set - are assumed to have been sent from another Unix (or similar) system - which uses the same file conventions. Such files are not translated - in any way. - - - Serial Number Iff the serial number is sent, a single space separates the - serial number from the file mode. The serial number of the - transmitting program is stored as an octal string. Programs which do - not have a serial number should omit this field, or set it to 0. The - receiver's use of this field is optional. - - - Other Fields YMODEM was designed to allow additional header fields to be - added as above without creating compatibility problems with older - YMODEM programs. Please contact Omen Technology if other fields are - needed for special application requirements. - - The rest of the block is set to nulls. This is essential to preserve - upward compatibility.[4] - - If the filename block is received with a CRC or other error, a - retransmission is requested. After the filename block has been received, - it is ACK'ed if the write open is successful. If the file cannot be - opened for writing, the receiver cancels the transfer with CAN characters - as described above. - - The receiver then initiates transfer of the file contents with a "C" - character, according to the standard XMODEM/CRC protocol. - - After the file contents and XMODEM EOT have been transmitted and - acknowledged, the receiver again asks for the next pathname. - - Transmission of a null pathname terminates batch file transmission. - - Note that transmission of no files is not necessarily an error. This is - possible if none of the files requested of the sender could be opened for - reading. - - - - __________ - - 4. If, perchance, this information extends beyond 128 bytes (possible - with Unix 4.2 BSD extended file names), the block should be sent as a - 1k block as described above. - - - - - Chapter 5 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 16 - - - - Most YMODEM receivers request CRC-16 by default. - - The Unix programs sz(1) and rz(1) included in the source code file - RZSZ.ZOO should answer other questions about YMODEM batch protocol. - - Figure 3. YMODEM Batch Transmission Session (1 file) - - SENDER RECEIVER - "sb foo.*" - "sending in batch mode etc." - C (command:rb) - SOH 00 FF foo.c NUL[123] CRC CRC - ACK - C - SOH 01 FE Data[128] CRC CRC - ACK - SOH 02 FC Data[128] CRC CRC - ACK - SOH 03 FB Data[100] CPMEOF[28] CRC CRC - ACK - EOT - NAK - EOT - ACK - C - SOH 00 FF NUL[128] CRC CRC - ACK - - Figure 7. YMODEM Header Information and Features - - _____________________________________________________________ - | Program | Length | Date | Mode | S/N | 1k-Blk | YMODEM-g | - |___________|________|______|______|_____|________|__________| - |Unix rz/sz | yes | yes | yes | no | yes | sb only | - |___________|________|______|______|_____|________|__________| - |VMS rb/sb | yes | no | no | no | yes | no | - |___________|________|______|______|_____|________|__________| - |Pro-YAM | yes | yes | no | yes | yes | yes | - |___________|________|______|______|_____|________|__________| - |CP/M YAM | no | no | no | no | yes | no | - |___________|________|______|______|_____|________|__________| - |KMD/IMP | ? | no | no | no | yes | no | - |___________|________|______|______|_____|________|__________| - - 5.1 KMD/IMP Exceptions to YMODEM - - KMD and IMP use a "CK" character sequence emitted by the receiver to - trigger the use of 1024 byte blocks as an alternative to specifying this - option to the sending program. This two character sequence generally - works well on single process micros in direct communication, provided the - programs rigorously adhere to all the XMODEM recommendations included - - - - Chapter 5 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 17 - - - - Figure 4. YMODEM Batch Transmission Session (2 files) - - SENDER RECEIVER - "sb foo.c baz.c" - "sending in batch mode etc." - C (command:rb) - SOH 00 FF foo.c NUL[123] CRC CRC - ACK - C - SOH 01 FE Data[128] CRC CRC - ACK - SOH 02 FC Data[128] CRC CRC - ACK - SOH 03 FB Data[100] CPMEOF[28] CRC CRC - ACK - EOT - NAK - EOT - ACK - C - SOH 00 FF baz.c NUL[123] CRC CRC - ACK - C - SOH 01 FB Data[100] CPMEOF[28] CRC CRC - ACK - EOT - NAK - EOT - ACK - C - SOH 00 FF NUL[128] CRC CRC - ACK - - Figure 5. YMODEM Batch Transmission Session-1k Blocks - - SENDER RECEIVER - "sb -k foo.*" - "sending in batch mode etc." - C (command:rb) - SOH 00 FF foo.c NUL[123] CRC CRC - ACK - C - STX 01 FD Data[1024] CRC CRC - ACK - SOH 02 FC Data[128] CRC CRC - ACK - SOH 03 FB Data[100] CPMEOF[28] CRC CRC - ACK - EOT - NAK - EOT - - - - Chapter 5 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 18 - - - - ACK - C - SOH 00 FF NUL[128] CRC CRC - ACK - - Figure 6. YMODEM Filename block transmitted by sz - - -rw-r--r-- 6347 Jun 17 1984 20:34 bbcsched.txt - - 00 0100FF62 62637363 6865642E 74787400 |...bbcsched.txt.| - 10 36333437 20333331 34373432 35313320 |6347 3314742513 | - 20 31303036 34340000 00000000 00000000 |100644..........| - 30 00000000 00000000 00000000 00000000 - 40 00000000 00000000 00000000 00000000 - 50 00000000 00000000 00000000 00000000 - 60 00000000 00000000 00000000 00000000 - 70 00000000 00000000 00000000 00000000 - 80 000000CA 56 - - herein. Programs with marginal XMODEM implementations do not fare so - well. Timesharing systems and packet switched networks can separate the - successive characters, rendering this method unreliable. - - Sending programs may detect the CK sequence if the operating enviornment - does not preclude reliable implementation. - - Instead of the standard YMODEM file length in decimal, KMD and IMP - transmit the CP/M record count in the last two bytes of the header block. - - - 6. YMODEM-g File Transmission - - Developing technology is providing phone line data transmission at ever - higher speeds using very specialized techniques. These high speed modems, - as well as session protocols such as X.PC, provide high speed, nearly - error free communications at the expense of considerably increased delay - time. - - This delay time is moderate compared to human interactions, but it - cripples the throughput of most error correcting protocols. - - The g option to YMODEM has proven effective under these circumstances. - The g option is driven by the receiver, which initiates the batch transfer - by transmitting a G instead of C. When the sender recognizes the G, it - bypasses the usual wait for an ACK to each transmitted block, sending - succeeding blocks at full speed, subject to XOFF/XON or other flow control - exerted by the medium. - - The sender expects an inital G to initiate the transmission of a - particular file, and also expects an ACK on the EOT sent at the end of - each file. This synchronization allows the receiver time to open and - - - - Chapter 6 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 19 - - - - close files as necessary. - - If an error is detected in a YMODEM-g transfer, the receiver aborts the - transfer with the multiple CAN abort sequence. The ZMODEM protocol should - be used in applications that require both streaming throughput and error - recovery. - - Figure 8. YMODEM-g Transmission Session - - SENDER RECEIVER - "sb foo.*" - "sending in batch mode etc..." - G (command:rb -g) - SOH 00 FF foo.c NUL[123] CRC CRC - G - SOH 01 FE Data[128] CRC CRC - STX 02 FD Data[1024] CRC CRC - SOH 03 FC Data[128] CRC CRC - SOH 04 FB Data[100] CPMEOF[28] CRC CRC - EOT - ACK - G - SOH 00 FF NUL[128] CRC CRC - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Chapter 6 XMODEM Protocol Enhancements - - - - - - - - X/YMODEM Protocol Reference June 18 1988 20 - - - - 7. XMODEM PROTOCOL OVERVIEW - - 8/9/82 by Ward Christensen. - - I will maintain a master copy of this. Please pass on changes or - suggestions via CBBS/Chicago at (312) 545-8086, CBBS/CPMUG (312) 849-1132 - or by voice at (312) 849-6279. - - 7.1 Definitions - - 01H - 04H - 06H - 15H - 18H - 43H - - - 7.2 Transmission Medium Level Protocol - - Asynchronous, 8 data bits, no parity, one stop bit. - - The protocol imposes no restrictions on the contents of the data being - transmitted. No control characters are looked for in the 128-byte data - messages. Absolutely any kind of data may be sent - binary, ASCII, etc. - The protocol has not formally been adopted to a 7-bit environment for the - transmission of ASCII-only (or unpacked-hex) data , although it could be - simply by having both ends agree to AND the protocol-dependent data with - 7F hex before validating it. I specifically am referring to the checksum, - and the block numbers and their ones- complement. - - Those wishing to maintain compatibility of the CP/M file structure, i.e. - to allow modemming ASCII files to or from CP/M systems should follow this - data format: - - + ASCII tabs used (09H); tabs set every 8. - - + Lines terminated by CR/LF (0DH 0AH) - - + End-of-file indicated by ^Z, 1AH. (one or more) - - + Data is variable length, i.e. should be considered a continuous - stream of data bytes, broken into 128-byte chunks purely for the - purpose of transmission. - - + A CP/M "peculiarity": If the data ends exactly on a 128-byte - boundary, i.e. CR in 127, and LF in 128, a subsequent sector - containing the ^Z EOF character(s) is optional, but is preferred. - Some utilities or user programs still do not handle EOF without ^Zs. - - - - - - Chapter 7 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 21 - - - - + The last block sent is no different from others, i.e. there is no - "short block". - Figure 9. XMODEM Message Block Level Protocol - - Each block of the transfer looks like: - <255-blk #><--128 data bytes--> - in which: - = 01 hex - = binary number, starts at 01 increments by 1, and - wraps 0FFH to 00H (not to 01) - <255-blk #> = blk # after going thru 8080 "CMA" instr, i.e. - each bit complemented in the 8-bit block number. - Formally, this is the "ones complement". - = the sum of the data bytes only. Toss any carry. - - 7.3 File Level Protocol - - 7.3.1 Common_to_Both_Sender_and_Receiver - All errors are retried 10 times. For versions running with an operator - (i.e. NOT with XMODEM), a message is typed after 10 errors asking the - operator whether to "retry or quit". - - Some versions of the protocol use , ASCII ^X, to cancel transmission. - This was never adopted as a standard, as having a single "abort" character - makes the transmission susceptible to false termination due to an - or being corrupted into a and aborting transmission. - - The protocol may be considered "receiver driven", that is, the sender need - not automatically re-transmit, although it does in the current - implementations. - - - 7.3.2 Receive_Program_Considerations - The receiver has a 10-second timeout. It sends a every time it - times out. The receiver's first timeout, which sends a , signals the - transmitter to start. Optionally, the receiver could send a - immediately, in case the sender was ready. This would save the initial 10 - second timeout. However, the receiver MUST continue to timeout every 10 - seconds in case the sender wasn't ready. - - Once into a receiving a block, the receiver goes into a one-second timeout - for each character and the checksum. If the receiver wishes to a - block for any reason (invalid header, timeout receiving data), it must - wait for the line to clear. See "programming tips" for ideas - - Synchronizing: If a valid block number is received, it will be: 1) the - expected one, in which case everything is fine; or 2) a repeat of the - previously received block. This should be considered OK, and only - indicates that the receivers got glitched, and the sender re- - transmitted; 3) any other block number indicates a fatal loss of - synchronization, such as the rare case of the sender getting a line-glitch - - - - Chapter 7 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 22 - - - - that looked like an . Abort the transmission, sending a - - - 7.3.3 Sending_program_considerations - While waiting for transmission to begin, the sender has only a single very - long timeout, say one minute. In the current protocol, the sender has a - 10 second timeout before retrying. I suggest NOT doing this, and letting - the protocol be completely receiver-driven. This will be compatible with - existing programs. - - When the sender has no more data, it sends an , and awaits an , - resending the if it doesn't get one. Again, the protocol could be - receiver-driven, with the sender only having the high-level 1-minute - timeout to abort. - - - Here is a sample of the data flow, sending a 3-block message. It includes - the two most common line hits - a garbaged block, and an reply - getting garbaged. represents the checksum byte. - - Figure 10. Data flow including Error Recovery - - SENDER RECEIVER - times out after 10 seconds, - <--- - 01 FE -data- ---> - <--- - 02 FD -data- xx ---> (data gets line hit) - <--- - 02 FD -data- xx ---> - <--- - 03 FC -data- xx ---> - (ack gets garbaged) <--- - 03 FC -data- xx ---> - ---> - <--- - ---> - <--- - (finished) - - 7.4 Programming Tips - - + The character-receive subroutine should be called with a parameter - specifying the number of seconds to wait. The receiver should first - call it with a time of 10, then and try again, 10 times. - - After receiving the , the receiver should call the character - receive subroutine with a 1-second timeout, for the remainder of the - message and the . Since they are sent as a continuous stream, - timing out of this implies a serious like glitch that caused, say, - 127 characters to be seen instead of 128. - - - - Chapter 7 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 23 - - - - + When the receiver wishes to , it should call a "PURGE" - subroutine, to wait for the line to clear. Recall the sender tosses - any characters in its UART buffer immediately upon completing sending - a block, to ensure no glitches were mis- interpreted. - - The most common technique is for "PURGE" to call the character - receive subroutine, specifying a 1-second timeout,[1] and looping - back to PURGE until a timeout occurs. The is then sent, - ensuring the other end will see it. - - + You may wish to add code recommended by John Mahr to your character - receive routine - to set an error flag if the UART shows framing - error, or overrun. This will help catch a few more glitches - the - most common of which is a hit in the high bits of the byte in two - consecutive bytes. The comes out OK since counting in 1-byte - produces the same result of adding 80H + 80H as with adding 00H + - 00H. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - __________ - - 1. These times should be adjusted for use with timesharing systems. - - - - - Chapter 7 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 24 - - - - 8. XMODEM/CRC Overview - - Original 1/13/85 by John Byrns -- CRC option. - - Please pass on any reports of errors in this document or suggestions for - improvement to me via Ward's/CBBS at (312) 849-1132, or by voice at (312) - 885-1105. - - The CRC used in the Modem Protocol is an alternate form of block check - which provides more robust error detection than the original checksum. - Andrew S. Tanenbaum says in his book, Computer Networks, that the CRC- - CCITT used by the Modem Protocol will detect all single and double bit - errors, all errors with an odd number of bits, all burst errors of length - 16 or less, 99.997% of 17-bit error bursts, and 99.998% of 18-bit and - longer bursts.[1] - - The changes to the Modem Protocol to replace the checksum with the CRC are - straight forward. If that were all that we did we would not be able to - communicate between a program using the old checksum protocol and one - using the new CRC protocol. An initial handshake was added to solve this - problem. The handshake allows a receiving program with CRC capability to - determine whether the sending program supports the CRC option, and to - switch it to CRC mode if it does. This handshake is designed so that it - will work properly with programs which implement only the original - protocol. A description of this handshake is presented in section 10. - - Figure 11. Message Block Level Protocol, CRC mode - - Each block of the transfer in CRC mode looks like: - <255-blk #><--128 data bytes--> - in which: - = 01 hex - = binary number, starts at 01 increments by 1, and - wraps 0FFH to 00H (not to 01) - <255-blk #> = ones complement of blk #. - = byte containing the 8 hi order coefficients of the CRC. - = byte containing the 8 lo order coefficients of the CRC. - - 8.1 CRC Calculation - - 8.1.1 Formal_Definition - To calculate the 16 bit CRC the message bits are considered to be the - coefficients of a polynomial. This message polynomial is first multiplied - by X^16 and then divided by the generator polynomial (X^16 + X^12 + X^5 + - - - __________ - - 1. This reliability figure is misleading because XMODEM's critical - supervisory functions are not protected by this CRC. - - - - - Chapter 8 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 25 - - - - 1) using modulo two arithmetic. The remainder left after the division is - the desired CRC. Since a message block in the Modem Protocol is 128 bytes - or 1024 bits, the message polynomial will be of order X^1023. The hi order - bit of the first byte of the message block is the coefficient of X^1023 in - the message polynomial. The lo order bit of the last byte of the message - block is the coefficient of X^0 in the message polynomial. - - Figure 12. Example of CRC Calculation written in C - - The following XMODEM crc routine is taken from "rbsb.c". Please refer to - the source code for these programs (contained in RZSZ.ZOO) for usage. A - fast table driven version is also included in this file. - - /* update CRC */ - unsigned short - updcrc(c, crc) - register c; - register unsigned crc; - { - register count; - - for (count=8; --count>=0;) { - if (crc & 0x8000) { - crc <<= 1; - crc += (((c<<=1) & 0400) != 0); - crc ^= 0x1021; - } - else { - crc <<= 1; - crc += (((c<<=1) & 0400) != 0); - } - } - return crc; - } - - 8.2 CRC File Level Protocol Changes - - 8.2.1 Common_to_Both_Sender_and_Receiver - The only change to the File Level Protocol for the CRC option is the - initial handshake which is used to determine if both the sending and the - receiving programs support the CRC mode. All Modem Programs should support - the checksum mode for compatibility with older versions. A receiving - program that wishes to receive in CRC mode implements the mode setting - handshake by sending a in place of the initial . If the sending - program supports CRC mode it will recognize the and will set itself - into CRC mode, and respond by sending the first block as if a had - been received. If the sending program does not support CRC mode it will - not respond to the at all. After the receiver has sent the it will - wait up to 3 seconds for the that starts the first block. If it - receives a within 3 seconds it will assume the sender supports CRC - mode and will proceed with the file exchange in CRC mode. If no is - - - - Chapter 8 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 26 - - - - received within 3 seconds the receiver will switch to checksum mode, send - a , and proceed in checksum mode. If the receiver wishes to use - checksum mode it should send an initial and the sending program - should respond to the as defined in the original Modem Protocol. - After the mode has been set by the initial or the protocol - follows the original Modem Protocol and is identical whether the checksum - or CRC is being used. - - - 8.2.2 Receive_Program_Considerations - There are at least 4 things that can go wrong with the mode setting - handshake. - - 1. the initial can be garbled or lost. - - 2. the initial can be garbled. - - 3. the initial can be changed to a . - - 4. the initial from a receiver which wants to receive in checksum - can be changed to a . - - The first problem can be solved if the receiver sends a second after - it times out the first time. This process can be repeated several times. - It must not be repeated too many times before sending a and - switching to checksum mode or a sending program without CRC support may - time out and abort. Repeating the will also fix the second problem if - the sending program cooperates by responding as if a were received - instead of ignoring the extra . - - It is possible to fix problems 3 and 4 but probably not worth the trouble - since they will occur very infrequently. They could be fixed by switching - modes in either the sending or the receiving program after a large number - of successive s. This solution would risk other problems however. - - - 8.2.3 Sending_Program_Considerations - The sending program should start in the checksum mode. This will insure - compatibility with checksum only receiving programs. Anytime a is - received before the first or the sending program should set - itself into CRC mode and respond as if a were received. The sender - should respond to additional s as if they were s until the first - is received. This will assist the receiving program in determining - the correct mode when the is lost or garbled. After the first - is received the sending program should ignore s. - - - - - - - - - - Chapter 8 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 27 - - - - 8.3 Data Flow Examples with CRC Option - - Here is a data flow example for the case where the receiver requests - transmission in the CRC mode but the sender does not support the CRC - option. This example also includes various transmission errors. - represents the checksum byte. - - Figure 13. Data Flow: Receiver has CRC Option, Sender Doesn't - - SENDER RECEIVER - <--- - times out after 3 seconds, - <--- - times out after 3 seconds, - <--- - times out after 3 seconds, - <--- - times out after 3 seconds, - <--- - 01 FE -data- ---> - <--- - 02 FD -data- ---> (data gets line hit) - <--- - 02 FD -data- ---> - <--- - 03 FC -data- ---> - (ack gets garbaged) <--- - times out after 10 seconds, - <--- - 03 FC -data- ---> - <--- - ---> - <--- - - Here is a data flow example for the case where the receiver requests - transmission in the CRC mode and the sender supports the CRC option. This - example also includes various transmission errors. represents the - 2 CRC bytes. - - - - - - - - - - - - - - - - - Chapter 8 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 28 - - - - Figure 14. Receiver and Sender Both have CRC Option - - SENDER RECEIVER - <--- - 01 FE -data- ---> - <--- - 02 FD -data- ---> (data gets line hit) - <--- - 02 FD -data- ---> - <--- - 03 FC -data- ---> - (ack gets garbaged) <--- - times out after 10 seconds, - <--- - 03 FC -data- ---> - <--- - ---> - <--- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Chapter 8 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 29 - - - - 9. MORE INFORMATION - - Please contact Omen Technology for troff source files and typeset copies - of this document. - - - 9.1 TeleGodzilla Bulletin Board - - More information may be obtained by calling TeleGodzilla at 503-621-3746. - Speed detection is automatic for 1200, 2400 and 19200(Telebit PEP) bps. - TrailBlazer modem users may issue the TeleGodzilla trailblazer command to - swith to 19200 bps once they have logged in. - - Interesting files include RZSZ.ZOO (C source code), YZMODEM.ZOO (Official - XMODEM, YMODEM, and ZMODEM protocol descriptions), ZCOMMEXE.ARC, - ZCOMMDOC.ARC, and ZCOMMHLP.ARC (PC-DOS shareware comm program with XMODEM, - True YMODEM(TM), ZMODEM, Kermit Sliding Windows, Telink, MODEM7 Batch, - script language, etc.). - - - 9.2 Unix UUCP Access - - UUCP sites can obtain the current version of this file with - uucp omen!/u/caf/public/ymodem.doc /tmp - A continually updated list of available files is stored in - /usr/spool/uucppublic/FILES. When retrieving these files with uucp, - remember that the destination directory on your system must be writeable - by anyone, or the UUCP transfer will fail. - - The following L.sys line calls TeleGodzilla (Pro-YAM in host operation). - TeleGodzilla determines the incoming speed automatically. - - In response to "Name Please:" uucico gives the Pro-YAM "link" command as a - user name. The password (Giznoid) controls access to the Xenix system - connected to the IBM PC's other serial port. Communications between - Pro-YAM and Xenix use 9600 bps; YAM converts this to the caller's speed. - - Finally, the calling uucico logs in as uucp. - - omen Any ACU 2400 1-503-621-3746 se:--se: link ord: Giznoid in:--in: uucp - - - - 10. REVISIONS - - 6-18-88 Further revised for clarity. Corrected block numbering in two - examples. - 10-27-87 Optional fields added for number of files remaining to be sent - and total number of bytes remaining to be sent. - 10-18-87 Flow control discussion added to 1024 byte block descritpion, - minor revisions for clarity per user comments. - - - - Chapter 10 Xmodem Protocol Overview - - - - - - - - X/YMODEM Protocol Reference June 18 1988 30 - - - - 8-03-87 Revised for clarity. - 5-31-1987 emphasizes minimum requirements for YMODEM, and updates - information on accessing files. - 9-11-1986 clarifies nomenclature and some minor points. - The April 15 1986 edition clarifies some points concerning CRC - calculations and spaces in the header. - - - 11. YMODEM Programs - - ZCOMM, A shareware little brother to Professional-YAM, is available as - ZCOMMEXE.ARC on TeleGodzilla and other bulletin board systems. ZCOMM may - be used to test YMODEM amd ZMODEM implementations. - - Unix programs supporting YMODEM are available on TeleGodzilla in RZSZ.ZOO. - This ZOO archive includes a ZCOMM/Pro-YAM/PowerCom script ZUPL.T to upload - a bootstrap program MINIRB.C, compile it, and then upload the rest of the - files using the compiled MINIRB. Most Unix like systems are supported, - including V7, Xenix, Sys III, 4.2 BSD, SYS V, Idris, Coherent, and - Regulus. - - A version for VAX-VMS is available in VRBSB.SHQ. - - Irv Hoff has added 1k blocks and basic YMODEM batch transfers to the KMD - and IMP series programs, which replace the XMODEM and MODEM7/MDM7xx series - respectively. Overlays are available for a wide variety of CP/M systems. - - Questions about Professional-YAM communications software may be directed - to: - Chuck Forsberg - Omen Technology Inc - 17505-V Sauvie Island Road - Portland Oregon 97231 - VOICE: 503-621-3406 :VOICE - Modem: 503-621-3746 Speed: 19200(Telebit PEP),2400,1200,300 - Usenet: ...!tektronix!reed!omen!caf - CompuServe: 70007,2304 - GEnie: CAF - - Unlike ZMODEM and Kermit, XMODEM and YMODEM place obstacles in the path of - a reliable high performance implementation, evidenced by poor reliability - under stress of the industry leaders' XMODEM and YMODEM programs. Omen - Technology provides consulting and other services to those wishing to - implement XMODEM, YMODEM, and ZMODEM with state of the art features and - reliability. - - - - - - - - - - Chapter 11 Xmodem Protocol Overview - - - - - - - - - - - - CONTENTS - - - 1. TOWER OF BABEL................................................... 2 - 1.1 Definitions................................................. 2 - - 2. YMODEM MINIMUM REQUIREMENTS...................................... 4 - - 3. WHY YMODEM?...................................................... 6 - 3.1 Some Messages from the Pioneer.............................. 7 - - 4. XMODEM PROTOCOL ENHANCEMENTS..................................... 10 - 4.1 Graceful Abort.............................................. 10 - 4.2 CRC-16 Option............................................... 10 - 4.3 XMODEM-1k 1024 Byte Block................................... 11 - - 5. YMODEM Batch File Transmission................................... 13 - 5.1 KMD/IMP Exceptions to YMODEM................................ 16 - - 6. YMODEM-g File Transmission....................................... 18 - - 7. XMODEM PROTOCOL OVERVIEW......................................... 20 - 7.1 Definitions................................................. 20 - 7.2 Transmission Medium Level Protocol.......................... 20 - 7.3 File Level Protocol......................................... 21 - 7.4 Programming Tips............................................ 22 - - 8. XMODEM/CRC Overview.............................................. 24 - 8.1 CRC Calculation............................................. 24 - 8.2 CRC File Level Protocol Changes............................. 25 - 8.3 Data Flow Examples with CRC Option.......................... 27 - - 9. MORE INFORMATION................................................. 29 - 9.1 TeleGodzilla Bulletin Board................................. 29 - 9.2 Unix UUCP Access............................................ 29 - - 10. REVISIONS........................................................ 29 - - 11. YMODEM Programs.................................................. 30 - - - - - - - - - - - - - - - - - i - - - - - - - - - - - - - - - - LIST OF FIGURES - - - Figure 1. XMODEM-1k Blocks.......................................... 12 - - Figure 2. Mixed 1024 and 128 byte Blocks............................ 12 - - Figure 3. YMODEM Batch Transmission Session (1 file)................ 16 - - Figure 4. YMODEM Batch Transmission Session (2 files)............... 16 - - Figure 5. YMODEM Batch Transmission Session-1k Blocks............... 16 - - Figure 6. YMODEM Filename block transmitted by sz................... 16 - - Figure 7. YMODEM Header Information and Features.................... 16 - - Figure 8. YMODEM-g Transmission Session............................. 19 - - Figure 9. XMODEM Message Block Level Protocol....................... 21 - - Figure 10. Data flow including Error Recovery........................ 22 - - Figure 11. Message Block Level Protocol, CRC mode.................... 24 - - Figure 12. Example of CRC Calculation written in C................... 25 - - Figure 13. Data Flow: Receiver has CRC Option, Sender Doesn't........ 27 - - Figure 14. Receiver and Sender Both have CRC Option.................. 28 - - - - - - - - - - - - - - - - - - - - - - - ii - - - - -