Руслан Урядинский
/
libuavcan
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: UAVCAN UAVCAN_Subscriber
Revision 0:dfe6edabb8ec, committed 2018-04-14
- Comitter:
- RuslanUrya
- Date:
- Sat Apr 14 10:25:32 2018 +0000
- Commit message:
- Initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CMakeLists.txt Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,87 @@
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+cmake_minimum_required(VERSION 2.8)
+
+project(uavcan C CXX)
+
+#
+# Build options
+#
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+ set(DEFAULT_UAVCAN_PLATFORM "linux")
+endif()
+
+# options are listed in a table format below
+set(opts
+ # name: type: default value: string options list : description
+ "CMAKE_BUILD_TYPE:STRING:RelWithDebInfo:Debug Release RelWithDebInfo MinSizeRel:Build type."
+ "CMAKE_CXX_FLAGS:STRING:::C++ flags."
+ "CMAKE_C_FLAGS:STRING:::C flags."
+ "UAVCAN_USE_CPP03:BOOL:OFF::Use C++03 standard."
+ "UAVCAN_PLATFORM:STRING:generic:generic linux stm32:Platform."
+ "CONTINUOUS_INTEGRATION_BUILD:BOOL:OFF::Disable error redirection and timing tests"
+ "UAVCAN_CMAKE_VERBOSE:BOOL:OFF::Verbose CMake configure output"
+ )
+foreach(_opt ${opts})
+ # arguments are : delimited
+ string(REPLACE ":" ";" _opt ${_opt})
+ list(GET _opt 0 _name)
+ list(GET _opt 1 _type)
+ list(GET _opt 2 _default)
+ list(GET _opt 3 _options)
+ list(GET _opt 4 _descr)
+ # options are space delimited
+ string(REPLACE " " ";" _options "${_options}")
+ # if a default has not already been defined, use default from table
+ if(NOT DEFINED DEFAULT_${_name})
+ set(DEFAULT_${_name} ${_default})
+ endif()
+ # option has not been set already or it is empty, set it with the default
+ if(NOT DEFINED ${_name} OR ${_name} STREQUAL "")
+ set(${_name} ${DEFAULT_${_name}})
+ endif()
+ # create a cache from the variable and force it to set
+if(UAVCAN_CMAKE_VERBOSE)
+ message(STATUS "${_name}\t: ${${_name}} : ${_descr}")
+endif()
+ set("${_name}" "${${_name}}" CACHE "${_type}" "${_descr}" FORCE)
+ # if an options list is provided for the cache, set it
+ if("${_type}" STREQUAL "STRING" AND NOT "${_options}" STREQUAL "")
+ set_property(CACHE ${_name} PROPERTY STRINGS ${_options})
+ endif()
+endforeach()
+
+#
+# Set flags
+#
+include_directories(
+ ./libuavcan/include/
+ ./libuavcan/include/dsdlc_generated
+ )
+
+#
+# Install
+#
+# DSDL definitions
+install(DIRECTORY dsdl DESTINATION share/uavcan)
+
+#
+# Subdirectories
+#
+# library
+add_subdirectory(libuavcan)
+
+# drivers
+if (${UAVCAN_PLATFORM} STREQUAL "linux")
+ message(STATUS "Adding UAVCAN Linux platform driver")
+ add_subdirectory(libuavcan_drivers/posix)
+ add_subdirectory(libuavcan_drivers/linux)
+elseif(${UAVCAN_PLATFORM} STREQUAL "stm32")
+ message(STATUS "Adding UAVCAN STM32 platform driver")
+ add_subdirectory(libuavcan_drivers/posix)
+ add_subdirectory(libuavcan_drivers/stm32/driver)
+endif()
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LICENSE Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,20 @@ +The MIT License (MIT) + +Copyright (c) 2014 Pavel Kirienko + +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.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/README.md Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,143 @@ +UAVCAN stack in C++ +=================== + +[](https://scan.coverity.com/projects/1513) +[](https://travis-ci.org/UAVCAN/libuavcan) +[](https://gitter.im/UAVCAN/general) + +Portable reference implementation of the [UAVCAN protocol stack](http://uavcan.org) in C++ for embedded systems +and Linux. + +UAVCAN is a lightweight protocol designed for reliable communication in aerospace and robotic applications via CAN bus. + +## Documentation + +* [UAVCAN website](http://uavcan.org) +* [UAVCAN discussion group](https://groups.google.com/forum/#!forum/uavcan) +* [Libuavcan overview](http://uavcan.org/Implementations/Libuavcan/) +* [List of platforms officially supported by libuavcan](http://uavcan.org/Implementations/Libuavcan/Platforms/) +* [Libuavcan tutorials](http://uavcan.org/Implementations/Libuavcan/Tutorials/) + +## Library usage + +### Dependencies + +* Python 2.7 or 3.3 or newer + +Note that this reporitory includes [Pyuavcan](http://uavcan.org/Implementations/Pyuavcan) as a submodule. +Such inclusion enables the library to be built even if pyuavcan is not installed in the system. + +### Cloning the repository + +```bash +git clone https://github.com/UAVCAN/libuavcan +cd libuavcan +git submodule update --init +``` + +If this repository is used as a git submodule in your project, make sure to use `--recursive` when updating it. + +### Using in a Linux application + +Libuavcan can be built as a static library and installed on the system globally as shown below. + +```bash +mkdir build +cd build +cmake .. # Default build type is RelWithDebInfo, which can be overriden if needed. +make -j8 +sudo make install +``` + +The following components will be installed: + +* Libuavcan headers and the static library +* Generated DSDL headers +* Libuavcan DSDL compiler (a Python script named `libuavcan_dsdlc`) +* Libuavcan DSDL compiler's support library (a Python package named `libuavcan_dsdl_compiler`) + +Note that Pyuavcan (an implementation of UAVCAN in Python) will not be installed. +You will need to install it separately if you intend to use the Libuavcan's DSDL compiler in your applications. + +It is also possible to use the library as a submodule rather than installing it system-wide. +Please refer to the example applications supplied with the Linux platform driver for more information. + +### Using with an embedded system + +For ARM targets, it is recommended to use [GCC ARM Embedded](https://launchpad.net/gcc-arm-embedded); +however, any other standard-compliant C++ compiler should also work. + +#### With Make + +Please refer to the [documentation at the UAVCAN website](http://uavcan.org/Implementations/Libuavcan). + +#### With CMake + +In order to cross-compile the library with CMake, please follow the below instructions. +You will need to provide a CMake toolchain file, `Toolchain-stm32-cortex-m4.cmake` in this example. +If you're not sure what a toolchain file is or how to prepare one, these instructions are probably not for your +use case; please refer to the section about Make instead. + +```bash +mkdir build +cd build +cmake .. -DCMAKE_TOOLCHAIN_FILE=../cmake/Toolchain-stm32-cortex-m4.cmake +make -j8 +``` + +## Library development + +Despite the fact that the library itself can be used on virtually any platform that has a standard-compliant +C++03 or C++11 compiler, the library development process assumes that the host OS is Linux. + +Prerequisites: + +* Google test library for C++ - gtest (see [how to install on Debian/Ubuntu](http://stackoverflow.com/questions/13513905/how-to-properly-setup-googletest-on-linux)) +* C++03 *and* C++11 capable compiler with GCC-like interface (e.g. GCC, Clang) +* CMake 2.8+ +* Optional: static analysis tool for C++ - cppcheck (on Debian/Ubuntu use package `cppcheck`) + +Building the debug version and running the unit tests: +```bash +mkdir build +cd build +cmake .. -DCMAKE_BUILD_TYPE=Debug +make +``` + +Test outputs can be found in the build directory under `libuavcan`. +Note that unit tests must be executed in real time, otherwise they may produce false warnings; +this implies that they will likely fail if ran on a virtual machine or on a highly loaded system. + +Contributors, please follow the [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah). + +### Developing with Eclipse + +An Eclipse project can be generated like that: + +```bash +cmake ../../libuavcan -G"Eclipse CDT4 - Unix Makefiles" \ + -DCMAKE_ECLIPSE_VERSION=4.3 \ + -DCMAKE_BUILD_TYPE=Debug \ + -DCMAKE_CXX_COMPILER_ARG1=-std=c++11 +``` + +Path `../../libuavcan` in the command above points at the directory where the top-level `CMakeLists.txt` is located; +you may need to adjust this per your environment. +Note that the directory where Eclipse project is generated must not be a descendant of the source directory. + +### Submitting a Coverity Scan build + +First, [get the Coverity build tool](https://scan.coverity.com/download?tab=cxx). Then build the library with it: + +```bash +export PATH=$PATH:<coverity-build-tool-directory>/bin/ +mkdir build && cd build +cmake <uavcan-source-directory> -DCMAKE_BUILD_TYPE=Debug +cov-build --dir cov-int make -j8 +tar czvf uavcan.tgz cov-int +``` + +Then upload the resulting archive to Coverity. + +Automatic check can be triggered by pushing to the branch `coverity_scan`.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/CMakeLists.txt Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,162 @@
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+cmake_minimum_required(VERSION 2.8)
+
+if(DEFINED CMAKE_BUILD_TYPE)
+ set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "Debug Release RelWithDebInfo MinSizeRel")
+else()
+ set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Debug Release RelWithDebInfo MinSizeRel")
+endif()
+
+# Detecting whether we need to add debug targets
+string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_lower)
+if (build_type_lower STREQUAL "debug")
+ set(DEBUG_BUILD 1)
+ message(STATUS "Debug build")
+else ()
+ set(DEBUG_BUILD 0)
+endif ()
+
+project(libuavcan)
+
+find_program(PYTHON python)
+
+if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+ set(COMPILER_IS_GCC_COMPATIBLE 1)
+else ()
+ set(COMPILER_IS_GCC_COMPATIBLE 0)
+endif ()
+
+#
+# DSDL compiler invocation
+# Probably output files should be saved into CMake output dir?
+#
+execute_process(COMMAND ${PYTHON} setup.py build WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler OUTPUT_QUIET)
+set(DSDLC_INPUTS "test/dsdl_test/root_ns_a" "test/dsdl_test/root_ns_b" "${CMAKE_CURRENT_SOURCE_DIR}/../dsdl/uavcan")
+set(DSDLC_OUTPUT "include/dsdlc_generated")
+
+set(DSDLC_INPUT_FILES "")
+foreach(DSDLC_INPUT ${DSDLC_INPUTS})
+ file(GLOB_RECURSE DSDLC_NEW_INPUT_FILES ${CMAKE_CURRENT_SOURCE_DIR} "${DSDLC_INPUT}/*.uavcan")
+ set(DSDLC_INPUT_FILES ${DSDLC_INPUT_FILES} ${DSDLC_NEW_INPUT_FILES})
+endforeach(DSDLC_INPUT)
+add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
+ COMMAND ${PYTHON} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
+ COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
+ DEPENDS ${DSDLC_INPUT_FILES}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+ COMMENT "Running dsdl compiler")
+add_custom_target(libuavcan_dsdlc DEPENDS ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp)
+include_directories(${DSDLC_OUTPUT})
+
+#
+# Compiler flags
+#
+if (COMPILER_IS_GCC_COMPATIBLE)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wundef")
+ if (UAVCAN_USE_CPP03)
+ message(STATUS "Using C++03")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++03 -Wno-variadic-macros -Wno-long-long")
+ else ()
+ message(STATUS "Using C++11 (pass UAVCAN_USE_CPP03=1 to override)")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ endif ()
+endif ()
+
+if (DEBUG_BUILD)
+ add_definitions(-DUAVCAN_DEBUG=1)
+endif ()
+
+include_directories(include)
+
+#
+# libuavcan
+#
+file(GLOB_RECURSE LIBUAVCAN_CXX_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "src/*.cpp")
+add_library(uavcan STATIC ${LIBUAVCAN_CXX_FILES})
+add_dependencies(uavcan libuavcan_dsdlc)
+
+install(TARGETS uavcan DESTINATION lib)
+install(DIRECTORY include/uavcan DESTINATION include)
+install(DIRECTORY include/dsdlc_generated/uavcan DESTINATION include) # Generated and lib's .hpp
+install(CODE "execute_process(COMMAND ${PYTHON} setup.py install --record installed_files.log
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler)")
+
+#
+# Tests and static analysis - only for debug builds
+#
+function(add_libuavcan_test name library flags) # Adds GTest executable and creates target to execute it every build
+ find_package(Threads REQUIRED)
+ include_directories(${GTEST_INCLUDE_DIRS})
+
+ file(GLOB_RECURSE TEST_CXX_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "test/*.cpp")
+ add_executable(${name} ${TEST_CXX_FILES})
+ add_dependencies(${name} ${library})
+
+ if (flags)
+ set_target_properties(${name} PROPERTIES COMPILE_FLAGS ${flags})
+ endif ()
+
+ target_link_libraries(${name} ${GTEST_BOTH_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
+ target_link_libraries(${name} ${library})
+ target_link_libraries(${name} rt)
+
+ # Tests run automatically upon successful build
+ # If failing tests need to be investigated with debugger, use 'make --ignore-errors'
+ if (CONTINUOUS_INTEGRATION_BUILD)
+ # Don't redirect test output, and don't run tests suffixed with "RealTime"
+ add_custom_command(TARGET ${name} POST_BUILD
+ COMMAND ./${name} --gtest_filter=-*RealTime
+ WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
+ else ()
+ add_custom_command(TARGET ${name} POST_BUILD
+ COMMAND ./${name} 1>"${name}.log" 2>&1
+ WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
+ endif()
+endfunction()
+
+if (DEBUG_BUILD)
+ message(STATUS "Debug build (note: requires gtest)")
+
+ if (COMPILER_IS_GCC_COMPATIBLE)
+ # No such thing as too many warnings
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -Wfloat-equal -Wconversion")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations -Wlogical-op")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wtype-limits -Wno-error=array-bounds")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wzero-as-null-pointer-constant -Wnon-virtual-dtor")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error=deprecated-declarations")
+ #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Weffc++ -Wno-error=effc++") # Produces heaps of useless warnings
+ set(cpp03_flags "-std=c++03 -Wno-variadic-macros -Wno-long-long -Wno-zero-as-null-pointer-constant")
+ set(optim_flags "-O3 -DNDEBUG -g0")
+ else ()
+ message(STATUS "Compiler ID: ${CMAKE_CXX_COMPILER_ID}")
+ message(FATAL_ERROR "This compiler cannot be used to build tests; use release build instead.")
+ endif ()
+
+ # Additional flavours of the library
+ add_library(uavcan_cpp03 STATIC ${LIBUAVCAN_CXX_FILES})
+ set_target_properties(uavcan_cpp03 PROPERTIES COMPILE_FLAGS ${cpp03_flags})
+ add_dependencies(uavcan_cpp03 libuavcan_dsdlc)
+
+ add_library(uavcan_optim STATIC ${LIBUAVCAN_CXX_FILES})
+ set_target_properties(uavcan_optim PROPERTIES COMPILE_FLAGS ${optim_flags})
+ add_dependencies(uavcan_optim libuavcan_dsdlc)
+
+ # GTest executables
+ find_package(GTest)
+ if (GTEST_FOUND)
+ message(STATUS "GTest found, tests will be built and run [${GTEST_INCLUDE_DIRS}] [${GTEST_BOTH_LIBRARIES}]")
+ add_libuavcan_test(libuavcan_test uavcan "") # Default
+ add_libuavcan_test(libuavcan_test_cpp03 uavcan_cpp03 "${cpp03_flags}") # C++03
+ add_libuavcan_test(libuavcan_test_optim uavcan_optim "${optim_flags}") # Max optimization
+ else (GTEST_FOUND)
+ message(STATUS "GTest was not found, tests will not be built")
+ endif (GTEST_FOUND)
+else ()
+ message(STATUS "Release build type: " ${CMAKE_BUILD_TYPE})
+endif ()
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/cppcheck.sh Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,18 @@ +#!/bin/sh +# +# cppcheck static analysis +# For Debian based: apt-get install cppcheck +# + +num_cores=$(grep -c ^processor /proc/cpuinfo) +if [ -z "$num_cores" ]; then + echo "Hey, it looks like we're not on Linux. Please fix this script to add support for this OS." + num_cores=4 +fi +echo "Number of threads for cppcheck: $num_cores" + +# TODO: with future versions of cppcheck, add --library=glibc +cppcheck . --error-exitcode=1 --quiet --enable=all --platform=unix64 --std=c99 --std=c++11 \ + --inline-suppr --force --template=gcc -j$num_cores \ + -U__BIGGEST_ALIGNMENT__ -UUAVCAN_MEM_POOL_BLOCK_SIZE -UBIG_ENDIAN -UBYTE_ORDER \ + -Iinclude $@
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,309 @@
+#
+# UAVCAN DSDL compiler for libuavcan
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+'''
+This module implements the core functionality of the UAVCAN DSDL compiler for libuavcan.
+Supported Python versions: 3.2+, 2.7.
+It accepts a list of root namespaces and produces the set of C++ header files for libuavcan.
+It is based on the DSDL parsing package from pyuavcan.
+'''
+
+from __future__ import division, absolute_import, print_function, unicode_literals
+import sys, os, logging, errno, re
+from .pyratemp import Template
+from uavcan import dsdl
+
+# Python 2.7 compatibility
+try:
+ str = unicode
+except NameError:
+ pass
+
+OUTPUT_FILE_EXTENSION = 'hpp'
+OUTPUT_FILE_PERMISSIONS = 0o444 # Read only for all
+TEMPLATE_FILENAME = os.path.join(os.path.dirname(__file__), 'data_type_template.tmpl')
+
+__all__ = ['run', 'logger', 'DsdlCompilerException']
+
+class DsdlCompilerException(Exception):
+ pass
+
+logger = logging.getLogger(__name__)
+
+def run(source_dirs, include_dirs, output_dir):
+ '''
+ This function takes a list of root namespace directories (containing DSDL definition files to parse), a
+ possibly empty list of search directories (containing DSDL definition files that can be referenced from the types
+ that are going to be parsed), and the output directory path (possibly nonexistent) where the generated C++
+ header files will be stored.
+
+ Note that this module features lazy write, i.e. if an output file does already exist and its content is not going
+ to change, it will not be overwritten. This feature allows to avoid unnecessary recompilation of dependent object
+ files.
+
+ Args:
+ source_dirs List of root namespace directories to parse.
+ include_dirs List of root namespace directories with referenced types (possibly empty). This list is
+ automaitcally extended with source_dirs.
+ output_dir Output directory path. Will be created if doesn't exist.
+ '''
+ assert isinstance(source_dirs, list)
+ assert isinstance(include_dirs, list)
+ output_dir = str(output_dir)
+
+ types = run_parser(source_dirs, include_dirs + source_dirs)
+ if not types:
+ die('No type definitions were found')
+
+ logger.info('%d types total', len(types))
+ run_generator(types, output_dir)
+
+# -----------------
+
+def pretty_filename(filename):
+ try:
+ a = os.path.abspath(filename)
+ r = os.path.relpath(filename)
+ return a if '..' in r else r
+ except ValueError:
+ return filename
+
+def type_output_filename(t):
+ assert t.category == t.CATEGORY_COMPOUND
+ return t.full_name.replace('.', os.path.sep) + '.' + OUTPUT_FILE_EXTENSION
+
+def makedirs(path):
+ try:
+ try:
+ os.makedirs(path, exist_ok=True) # May throw "File exists" when executed as root, which is wrong
+ except TypeError:
+ os.makedirs(path) # Python 2.7 compatibility
+ except OSError as ex:
+ if ex.errno != errno.EEXIST: # http://stackoverflow.com/questions/12468022
+ raise
+
+def die(text):
+ raise DsdlCompilerException(str(text))
+
+def run_parser(source_dirs, search_dirs):
+ try:
+ types = dsdl.parse_namespaces(source_dirs, search_dirs)
+ except dsdl.DsdlException as ex:
+ logger.info('Parser failure', exc_info=True)
+ die(ex)
+ return types
+
+def run_generator(types, dest_dir):
+ try:
+ template_expander = make_template_expander(TEMPLATE_FILENAME)
+ dest_dir = os.path.abspath(dest_dir) # Removing '..'
+ makedirs(dest_dir)
+ for t in types:
+ logger.info('Generating type %s', t.full_name)
+ filename = os.path.join(dest_dir, type_output_filename(t))
+ text = generate_one_type(template_expander, t)
+ write_generated_data(filename, text)
+ except Exception as ex:
+ logger.info('Generator failure', exc_info=True)
+ die(ex)
+
+def write_generated_data(filename, data):
+ dirname = os.path.dirname(filename)
+ makedirs(dirname)
+
+ # Lazy update - file will not be rewritten if its content is not going to change
+ if os.path.exists(filename):
+ with open(filename) as f:
+ existing_data = f.read()
+ if data == existing_data:
+ logger.info('Up to date [%s]', pretty_filename(filename))
+ return
+ logger.info('Rewriting [%s]', pretty_filename(filename))
+ os.remove(filename)
+ else:
+ logger.info('Creating [%s]', pretty_filename(filename))
+
+ # Full rewrite
+ with open(filename, 'w') as f:
+ f.write(data)
+ try:
+ os.chmod(filename, OUTPUT_FILE_PERMISSIONS)
+ except (OSError, IOError) as ex:
+ logger.warning('Failed to set permissions for %s: %s', pretty_filename(filename), ex)
+
+def type_to_cpp_type(t):
+ if t.category == t.CATEGORY_PRIMITIVE:
+ cast_mode = {
+ t.CAST_MODE_SATURATED: '::uavcan::CastModeSaturate',
+ t.CAST_MODE_TRUNCATED: '::uavcan::CastModeTruncate',
+ }[t.cast_mode]
+ if t.kind == t.KIND_FLOAT:
+ return '::uavcan::FloatSpec< %d, %s >' % (t.bitlen, cast_mode)
+ else:
+ signedness = {
+ t.KIND_BOOLEAN: '::uavcan::SignednessUnsigned',
+ t.KIND_UNSIGNED_INT: '::uavcan::SignednessUnsigned',
+ t.KIND_SIGNED_INT: '::uavcan::SignednessSigned',
+ }[t.kind]
+ return '::uavcan::IntegerSpec< %d, %s, %s >' % (t.bitlen, signedness, cast_mode)
+ elif t.category == t.CATEGORY_ARRAY:
+ value_type = type_to_cpp_type(t.value_type)
+ mode = {
+ t.MODE_STATIC: '::uavcan::ArrayModeStatic',
+ t.MODE_DYNAMIC: '::uavcan::ArrayModeDynamic',
+ }[t.mode]
+ return '::uavcan::Array< %s, %s, %d >' % (value_type, mode, t.max_size)
+ elif t.category == t.CATEGORY_COMPOUND:
+ return '::' + t.full_name.replace('.', '::')
+ elif t.category == t.CATEGORY_VOID:
+ return '::uavcan::IntegerSpec< %d, ::uavcan::SignednessUnsigned, ::uavcan::CastModeSaturate >' % t.bitlen
+ else:
+ raise DsdlCompilerException('Unknown type category: %s' % t.category)
+
+def generate_one_type(template_expander, t):
+ t.short_name = t.full_name.split('.')[-1]
+ t.cpp_type_name = t.short_name + '_'
+ t.cpp_full_type_name = '::' + t.full_name.replace('.', '::')
+ t.include_guard = t.full_name.replace('.', '_').upper() + '_HPP_INCLUDED'
+
+ # Dependencies (no duplicates)
+ def fields_includes(fields):
+ def detect_include(t):
+ if t.category == t.CATEGORY_COMPOUND:
+ return type_output_filename(t)
+ if t.category == t.CATEGORY_ARRAY:
+ return detect_include(t.value_type)
+ return list(sorted(set(filter(None, [detect_include(x.type) for x in fields]))))
+
+ if t.kind == t.KIND_MESSAGE:
+ t.cpp_includes = fields_includes(t.fields)
+ else:
+ t.cpp_includes = fields_includes(t.request_fields + t.response_fields)
+
+ t.cpp_namespace_components = t.full_name.split('.')[:-1]
+ t.has_default_dtid = t.default_dtid is not None
+
+ # Attribute types
+ def inject_cpp_types(attributes):
+ void_index = 0
+ for a in attributes:
+ a.cpp_type = type_to_cpp_type(a.type)
+ a.void = a.type.category == a.type.CATEGORY_VOID
+ if a.void:
+ assert not a.name
+ a.name = '_void_%d' % void_index
+ void_index += 1
+
+ if t.kind == t.KIND_MESSAGE:
+ inject_cpp_types(t.fields)
+ inject_cpp_types(t.constants)
+ t.all_attributes = t.fields + t.constants
+ t.union = t.union and len(t.fields)
+ else:
+ inject_cpp_types(t.request_fields)
+ inject_cpp_types(t.request_constants)
+ inject_cpp_types(t.response_fields)
+ inject_cpp_types(t.response_constants)
+ t.all_attributes = t.request_fields + t.request_constants + t.response_fields + t.response_constants
+ t.request_union = t.request_union and len(t.request_fields)
+ t.response_union = t.response_union and len(t.response_fields)
+
+ # Constant properties
+ def inject_constant_info(constants):
+ for c in constants:
+ if c.type.kind == c.type.KIND_FLOAT:
+ float(c.string_value) # Making sure that this is a valid float literal
+ c.cpp_value = c.string_value
+ else:
+ int(c.string_value) # Making sure that this is a valid integer literal
+ c.cpp_value = c.string_value
+ if c.type.kind == c.type.KIND_UNSIGNED_INT:
+ c.cpp_value += 'U'
+
+ if t.kind == t.KIND_MESSAGE:
+ inject_constant_info(t.constants)
+ else:
+ inject_constant_info(t.request_constants)
+ inject_constant_info(t.response_constants)
+
+ # Data type kind
+ t.cpp_kind = {
+ t.KIND_MESSAGE: '::uavcan::DataTypeKindMessage',
+ t.KIND_SERVICE: '::uavcan::DataTypeKindService',
+ }[t.kind]
+
+ # Generation
+ text = template_expander(t=t) # t for Type
+ text = '\n'.join(x.rstrip() for x in text.splitlines())
+ text = text.replace('\n\n\n\n\n', '\n\n').replace('\n\n\n\n', '\n\n').replace('\n\n\n', '\n\n')
+ text = text.replace('{\n\n ', '{\n ')
+ return text
+
+def make_template_expander(filename):
+ '''
+ Templating is based on pyratemp (http://www.simple-is-better.org/template/pyratemp.html).
+ The pyratemp's syntax is rather verbose and not so human friendly, so we define some
+ custom extensions to make it easier to read and write.
+ The resulting syntax somewhat resembles Mako (which was used earlier instead of pyratemp):
+ Substitution:
+ ${expression}
+ Line joining through backslash (replaced with a single space):
+ ${foo(bar(very_long_arument=42, \
+ second_line=72))}
+ Blocks:
+ % for a in range(10):
+ % if a == 5:
+ ${foo()}
+ % endif
+ % endfor
+ The extended syntax is converted into pyratemp's through regexp substitution.
+ '''
+ with open(filename) as f:
+ template_text = f.read()
+
+ # Backslash-newline elimination
+ template_text = re.sub(r'\\\r{0,1}\n\ *', r' ', template_text)
+
+ # Substitution syntax transformation: ${foo} ==> $!foo!$
+ template_text = re.sub(r'([^\$]{0,1})\$\{([^\}]+)\}', r'\1$!\2!$', template_text)
+
+ # Flow control expression transformation: % foo: ==> <!--(foo)-->
+ template_text = re.sub(r'(?m)^(\ *)\%\ *(.+?):{0,1}$', r'\1<!--(\2)-->', template_text)
+
+ # Block termination transformation: <!--(endfoo)--> ==> <!--(end)-->
+ template_text = re.sub(r'\<\!--\(end[a-z]+\)--\>', r'<!--(end)-->', template_text)
+
+ # Pyratemp workaround.
+ # The problem is that if there's no empty line after a macro declaration, first line will be doubly indented.
+ # Workaround:
+ # 1. Remove trailing comments
+ # 2. Add a newline after each macro declaration
+ template_text = re.sub(r'\ *\#\!.*', '', template_text)
+ template_text = re.sub(r'(\<\!--\(macro\ [a-zA-Z0-9_]+\)--\>.*?)', r'\1\n', template_text)
+
+ # Preprocessed text output for debugging
+# with open(filename + '.d', 'w') as f:
+# f.write(template_text)
+
+ template = Template(template_text)
+
+ def expand(**args):
+ # This function adds one indentation level (4 spaces); it will be used from the template
+ args['indent'] = lambda text, idnt = ' ': idnt + text.replace('\n', '\n' + idnt)
+ # This function works like enumerate(), telling you whether the current item is the last one
+ def enum_last_value(iterable, start=0):
+ it = iter(iterable)
+ count = start
+ last = next(it)
+ for val in it:
+ yield count, False, last
+ last = val
+ count += 1
+ yield count, True, last
+ args['enum_last_value'] = enum_last_value
+ return template(**args)
+
+ return expand
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,558 @@
+/*
+ * UAVCAN data structure definition for libuavcan.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: ${t.source_file}
+ */
+
+#ifndef ${t.include_guard}
+#define ${t.include_guard}
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/global_data_type_registry.hpp>
+#include <uavcan/marshal/types.hpp>
+
+% for inc in t.cpp_includes:
+#include <${inc}>
+% endfor
+
+/******************************* Source text **********************************
+% for line in t.source_text.strip().splitlines():
+${line}
+% endfor
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+% for line in t.get_dsdl_signature_source_definition().splitlines():
+${line}
+% endfor
+******************************************************************************/
+
+% for a in t.all_attributes:
+#undef ${a.name}
+% endfor
+
+% for nsc in t.cpp_namespace_components:
+namespace ${nsc}
+{
+% endfor
+
+% if t.kind != t.KIND_SERVICE:
+template <int _tmpl>
+% endif
+struct UAVCAN_EXPORT ${t.cpp_type_name}
+{
+<!--(macro generate_primary_body)--> #! type_name, max_bitlen, fields, constants, union
+ typedef const ${type_name}<_tmpl>& ParameterType;
+ typedef ${type_name}<_tmpl>& ReferenceType;
+
+ <!--(macro expand_attr_types)--> #! group_name, attrs
+ struct ${group_name}
+ {
+ % for a in attrs:
+ typedef ${a.cpp_type} ${a.name};
+ % endfor
+ };
+ <!--(end)-->
+ ${expand_attr_types(group_name='ConstantTypes', attrs=constants)}
+ ${expand_attr_types(group_name='FieldTypes', attrs=fields)}
+
+ % if union:
+
+ struct Tag
+ {
+ enum Type
+ {
+ % for idx,last,a in enum_last_value(fields):
+ ${a.name}${',' if not last else ''}
+ % endfor
+ };
+ };
+
+ typedef ::uavcan::IntegerSpec< ::uavcan::IntegerBitLen< ${len(fields)} >::Result,
+ ::uavcan::SignednessUnsigned, ::uavcan::CastModeTruncate > TagType;
+
+ <!--(macro expand_enum_per_field)--> #! enum_name, enum_comparator
+ enum
+ {
+ ${enum_name} = TagType::BitLen +
+ % for idx,last,a in enum_last_value(fields):
+ % if not last:
+ ::uavcan::${enum_comparator}<FieldTypes::${a.name}::${enum_name},
+ % else:
+ FieldTypes::${a.name}::${enum_name} ${'>::Result' * (len(fields) - 1)}
+ % endif
+ % endfor
+ };
+ <!--(end)-->
+
+ ${expand_enum_per_field(enum_name='MinBitLen', enum_comparator='EnumMin')}
+ ${expand_enum_per_field(enum_name='MaxBitLen', enum_comparator='EnumMax')}
+
+ % else:
+
+ <!--(macro expand_enum_per_field)--> #! enum_name
+ enum
+ {
+ ${enum_name}
+ % for idx,a in enumerate(fields):
+ ${'=' if idx == 0 else '+'} FieldTypes::${a.name}::${enum_name}
+ % endfor
+ };
+ <!--(end)-->
+
+ ${expand_enum_per_field(enum_name='MinBitLen')}
+ ${expand_enum_per_field(enum_name='MaxBitLen')}
+
+ % endif
+
+ // Constants
+ % for a in constants:
+ static const typename ::uavcan::StorageType< typename ConstantTypes::${a.name} >::Type ${a.name}; // ${a.init_expression}
+ % endfor
+
+ // Fields
+ % for a in [x for x in fields if not x.void]:
+ typename ::uavcan::StorageType< typename FieldTypes::${a.name} >::Type ${a.name};
+ % endfor
+
+ % if union:
+private:
+ typename ::uavcan::StorageType< TagType >::Type _tag_; // The name is mangled to avoid clashing with fields
+
+ template <typename Tag::Type T>
+ struct TagToType;
+
+public:
+ % endif
+
+ ${type_name}()
+ % for idx,a in enumerate([x for x in fields if not x.void]):
+ ${':' if idx == 0 else ','} ${a.name}()
+ % endfor
+ % if union:
+ , _tag_()
+ % endif
+ {
+ ::uavcan::StaticAssert<_tmpl == 0>::check(); // Usage check
+
+#if UAVCAN_DEBUG
+ /*
+ * Cross-checking MaxBitLen provided by the DSDL compiler.
+ * This check shall never be performed in user code because MaxBitLen value
+ * actually depends on the nested types, thus it is not invariant.
+ */
+ ::uavcan::StaticAssert<${max_bitlen} == MaxBitLen>::check();
+#endif
+ }
+
+ bool operator==(ParameterType rhs) const;
+ bool operator!=(ParameterType rhs) const { return !operator==(rhs); }
+
+ /**
+ * This comparison is based on @ref uavcan::areClose(), which ensures proper comparison of
+ * floating point fields at any depth.
+ */
+ bool isClose(ParameterType rhs) const;
+
+ static int encode(ParameterType self, ::uavcan::ScalarCodec& codec,
+ ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled);
+
+ static int decode(ReferenceType self, ::uavcan::ScalarCodec& codec,
+ ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled);
+
+ % if union:
+ /**
+ * Explicit access to the tag.
+ * It is safer to use is()/as()/to() instead.
+ */
+ typename Tag::Type getTag() const { return typename Tag::Type(_tag_); }
+ void setTag(typename Tag::Type x) { _tag_ = typename ::uavcan::StorageType< TagType >::Type(x); }
+
+ /**
+ * Whether the union is set to the given type.
+ * Access by tag; this will work even if there are non-unique types within the union.
+ */
+ bool is(typename Tag::Type x) const { return typename Tag::Type(_tag_) == x; }
+
+ /**
+ * If the union is currently set to the type T, returns pointer to the appropriate field.
+ * If the union is set to another type, returns null pointer.
+ */
+ template <typename Tag::Type T>
+ inline const typename TagToType<T>::StorageType* as() const;
+
+ /**
+ * Switches the union to the given type and returns a mutable reference to the appropriate field.
+ * If the previous type was different, a default constructor will be called first.
+ */
+ template <typename Tag::Type T>
+ inline typename TagToType<T>::StorageType& to();
+ % endif
+<!--(end)-->
+
+% if t.kind == t.KIND_SERVICE:
+ template <int _tmpl>
+ struct Request_
+ {
+ ${indent(generate_primary_body(type_name='Request_', max_bitlen=t.get_max_bitlen_request(), \
+ fields=t.request_fields, constants=t.request_constants, \
+ union=t.request_union))}
+ };
+
+ template <int _tmpl>
+ struct Response_
+ {
+ ${indent(generate_primary_body(type_name='Response_', max_bitlen=t.get_max_bitlen_response(), \
+ fields=t.response_fields, constants=t.response_constants, \
+ union=t.response_union))}
+ };
+
+ typedef Request_<0> Request;
+ typedef Response_<0> Response;
+% else:
+ ${generate_primary_body(type_name=t.cpp_type_name, max_bitlen=t.get_max_bitlen(), \
+ fields=t.fields, constants=t.constants, union=t.union)}
+% endif
+
+ /*
+ * Static type info
+ */
+ enum { DataTypeKind = ${t.cpp_kind} };
+% if t.has_default_dtid:
+ enum { DefaultDataTypeID = ${t.default_dtid} };
+% else:
+ // This type has no default data type ID
+% endif
+
+ static const char* getDataTypeFullName()
+ {
+ return "${t.full_name}";
+ }
+
+ static void extendDataTypeSignature(::uavcan::DataTypeSignature& signature)
+ {
+ signature.extend(getDataTypeSignature());
+ }
+
+ static ::uavcan::DataTypeSignature getDataTypeSignature();
+
+% if t.kind == t.KIND_SERVICE:
+private:
+ ${t.cpp_type_name}(); // Don't create objects of this type. Use Request/Response instead.
+% endif
+};
+
+/*
+ * Out of line struct method definitions
+ */
+<!--(macro define_out_of_line_struct_methods)--> #! scope_prefix, fields, union
+
+template <int _tmpl>
+bool ${scope_prefix}<_tmpl>::operator==(ParameterType rhs) const
+{
+ % if union:
+ if (_tag_ != rhs._tag_)
+ {
+ return false;
+ }
+ % for idx,a in enumerate(fields):
+ if (_tag_ == ${idx})
+ {
+ return ${a.name} == rhs.${a.name};
+ }
+ % endfor
+ UAVCAN_ASSERT(0); // Invalid tag
+ return false;
+ % else:
+ % if fields:
+ return
+ % for idx,last,a in enum_last_value([x for x in fields if not x.void]):
+ ${a.name} == rhs.${a.name}${' &&' if not last else ';'}
+ % endfor
+ % else:
+ (void)rhs;
+ return true;
+ % endif
+ % endif
+}
+
+template <int _tmpl>
+bool ${scope_prefix}<_tmpl>::isClose(ParameterType rhs) const
+{
+ % if union:
+ if (_tag_ != rhs._tag_)
+ {
+ return false;
+ }
+ % for idx,a in enumerate(fields):
+ if (_tag_ == ${idx})
+ {
+ return ::uavcan::areClose(${a.name}, rhs.${a.name});
+ }
+ % endfor
+ UAVCAN_ASSERT(0); // Invalid tag
+ return false;
+ % else:
+ % if fields:
+ return
+ % for idx,last,a in enum_last_value([x for x in fields if not x.void]):
+ ::uavcan::areClose(${a.name}, rhs.${a.name})${' &&' if not last else ';'}
+ % endfor
+ % else:
+ (void)rhs;
+ return true;
+ % endif
+ % endif
+}
+
+ <!--(macro generate_codec_calls_per_field)--> #! call_name, self_parameter_type
+template <int _tmpl>
+int ${scope_prefix}<_tmpl>::${call_name}(${self_parameter_type} self, ::uavcan::ScalarCodec& codec,
+ ::uavcan::TailArrayOptimizationMode tao_mode)
+{
+ (void)self;
+ (void)codec;
+ (void)tao_mode;
+ % if union:
+ const int res = TagType::${call_name}(self._tag_, codec, ::uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ % for idx,a in enumerate(fields):
+ if (self._tag_ == ${idx})
+ {
+ return FieldTypes::${a.name}::${call_name}(self.${a.name}, codec, tao_mode);
+ }
+ % endfor
+ return -1; // Invalid tag value
+ % else:
+ % for a in [x for x in fields if x.void]:
+ typename ::uavcan::StorageType< typename FieldTypes::${a.name} >::Type ${a.name} = 0;
+ % endfor
+ int res = 1;
+ % for idx,last,a in enum_last_value(fields):
+ res = FieldTypes::${a.name}::${call_name}(${'self.' * (not a.void)}${a.name}, codec, \
+${'::uavcan::TailArrayOptDisabled' if not last else 'tao_mode'});
+ % if not last:
+ if (res <= 0)
+ {
+ return res;
+ }
+ % endif
+ % endfor
+ return res;
+ % endif
+}
+ <!--(end)-->
+${generate_codec_calls_per_field(call_name='encode', self_parameter_type='ParameterType')}
+${generate_codec_calls_per_field(call_name='decode', self_parameter_type='ReferenceType')}
+
+ % if union:
+ % for idx,a in enumerate(fields):
+template <>
+template <>
+struct ${scope_prefix}<0>::TagToType<${scope_prefix}<0>::Tag::${a.name}>
+{
+ typedef typename ${scope_prefix}<0>::FieldTypes::${a.name} Type;
+ typedef typename ::uavcan::StorageType<Type>::Type StorageType;
+};
+
+template <>
+template <>
+inline const typename ${scope_prefix}<0>::TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType*
+${scope_prefix}<0>::as< ${scope_prefix}<0>::Tag::${a.name} >() const
+{
+ return is(${scope_prefix}<0>::Tag::${a.name}) ? &${a.name} : UAVCAN_NULLPTR;
+}
+
+template <>
+template <>
+inline typename ${scope_prefix}<0>::TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType&
+${scope_prefix}<0>::to< ${scope_prefix}<0>::Tag::${a.name} >()
+{
+ if (_tag_ != ${idx})
+ {
+ _tag_ = ${idx};
+ ${a.name} = typename TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType();
+ }
+ return ${a.name};
+}
+
+ % endfor
+ % endif
+<!--(end)-->
+
+% if t.kind == t.KIND_SERVICE:
+${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name + '::Request_', fields=t.request_fields, \
+ union=t.request_union)}
+${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name + '::Response_', fields=t.response_fields, \
+ union=t.response_union)}
+% else:
+${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name, fields=t.fields, union=t.union)}
+% endif
+
+/*
+ * Out of line type method definitions
+ */
+% if t.kind == t.KIND_SERVICE:
+inline ::uavcan::DataTypeSignature ${t.cpp_type_name}::getDataTypeSignature()
+% else:
+template <int _tmpl>
+::uavcan::DataTypeSignature ${t.cpp_type_name}<_tmpl>::getDataTypeSignature()
+% endif
+{
+ ::uavcan::DataTypeSignature signature(${'0x%08X' % t.get_dsdl_signature()}ULL);
+<!--(macro extend_signature_per_field)--> #! scope_prefix, fields
+ % for a in fields:
+ ${scope_prefix}FieldTypes::${a.name}::extendDataTypeSignature(signature);
+ % endfor
+<!--(end)-->
+% if t.kind == t.KIND_SERVICE:
+${extend_signature_per_field(scope_prefix='Request::', fields=t.request_fields)}
+${extend_signature_per_field(scope_prefix='Response::', fields=t.response_fields)}
+% else:
+${extend_signature_per_field(scope_prefix='', fields=t.fields)}
+% endif
+ return signature;
+}
+
+/*
+ * Out of line constant definitions
+ */
+<!--(macro define_out_of_line_constants)--> #! scope_prefix, constants
+ % for a in constants:
+template <int _tmpl>
+const typename ::uavcan::StorageType< typename ${scope_prefix}<_tmpl>::ConstantTypes::${a.name} >::Type
+ ${scope_prefix}<_tmpl>::${a.name} = ${a.cpp_value}; // ${a.init_expression}
+
+ % endfor
+<!--(end)-->
+% if t.kind == t.KIND_SERVICE:
+${define_out_of_line_constants(scope_prefix=t.cpp_type_name + '::Request_', constants=t.request_constants)}
+${define_out_of_line_constants(scope_prefix=t.cpp_type_name + '::Response_', constants=t.response_constants)}
+% else:
+${define_out_of_line_constants(scope_prefix=t.cpp_type_name, constants=t.constants)}
+% endif
+
+/*
+ * Final typedef
+ */
+% if t.kind == t.KIND_SERVICE:
+typedef ${t.cpp_type_name} ${t.short_name};
+% else:
+typedef ${t.cpp_type_name}<0> ${t.short_name};
+% endif
+
+% if t.has_default_dtid:
+namespace
+{
+
+const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_full_type_name} > _uavcan_gdtr_registrator_${t.short_name};
+
+}
+% else:
+// No default registration
+% endif
+
+% for nsc in t.cpp_namespace_components[::-1]:
+} // Namespace ${nsc}
+% endfor
+
+/*
+ * YAML streamer specialization
+ */
+namespace uavcan
+{
+
+<!--(macro define_yaml_streamer)--> #! type_name, fields, union
+template <>
+class UAVCAN_EXPORT YamlStreamer< ${type_name} >
+{
+public:
+ template <typename Stream>
+ static void stream(Stream& s, ${type_name}::ParameterType obj, const int level);
+};
+
+template <typename Stream>
+void YamlStreamer< ${type_name} >::stream(Stream& s, ${type_name}::ParameterType obj, const int level)
+{
+ (void)s;
+ (void)obj;
+ (void)level;
+ % if union:
+ if (level > 0)
+ {
+ s << '\n';
+ for (int pos = 0; pos < level; pos++)
+ {
+ s << " ";
+ }
+ }
+ % for idx,a in enumerate(fields):
+ if (static_cast<int>(obj.getTag()) == ${idx})
+ {
+ s << "${a.name}: ";
+ YamlStreamer< ${type_name}::FieldTypes::${a.name} >::stream(s, obj.${a.name}, level + 1);
+ }
+ % endfor
+ % else:
+ % for idx,a in enumerate([x for x in fields if not x.void]):
+ % if idx == 0:
+ if (level > 0)
+ {
+ s << '\n';
+ for (int pos = 0; pos < level; pos++)
+ {
+ s << " ";
+ }
+ }
+ % else:
+ s << '\n';
+ for (int pos = 0; pos < level; pos++)
+ {
+ s << " ";
+ }
+ % endif
+ s << "${a.name}: ";
+ YamlStreamer< ${type_name}::FieldTypes::${a.name} >::stream(s, obj.${a.name}, level + 1);
+ % endfor
+ % endif
+}
+<!--(end)-->
+% if t.kind == t.KIND_SERVICE:
+${define_yaml_streamer(type_name=t.cpp_full_type_name + '::Request', fields=t.request_fields, union=t.request_union)}
+${define_yaml_streamer(type_name=t.cpp_full_type_name + '::Response', fields=t.response_fields, union=t.response_union)}
+% else:
+${define_yaml_streamer(type_name=t.cpp_full_type_name, fields=t.fields, union=t.union)}
+% endif
+
+}
+
+% for nsc in t.cpp_namespace_components:
+namespace ${nsc}
+{
+% endfor
+
+<!--(macro define_streaming_operator)--> #! type_name
+template <typename Stream>
+inline Stream& operator<<(Stream& s, ${type_name}::ParameterType obj)
+{
+ ::uavcan::YamlStreamer< ${type_name} >::stream(s, obj, 0);
+ return s;
+}
+<!--(end)-->
+% if t.kind == t.KIND_SERVICE:
+${define_streaming_operator(type_name=t.cpp_full_type_name + '::Request')}
+${define_streaming_operator(type_name=t.cpp_full_type_name + '::Response')}
+% else:
+${define_streaming_operator(type_name=t.cpp_full_type_name)}
+% endif
+
+% for nsc in t.cpp_namespace_components[::-1]:
+} // Namespace ${nsc}
+% endfor
+
+#endif // ${t.include_guard}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,1226 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+Small, simple and powerful template-engine for Python.
+
+A template-engine for Python, which is very simple, easy to use, small,
+fast, powerful, modular, extensible, well documented and pythonic.
+
+See documentation for a list of features, template-syntax etc.
+
+:Version: 0.3.0
+:Requires: Python >=2.6 / 3.x
+
+:Usage:
+ see class ``Template`` and examples below.
+
+:Example:
+
+ Note that the examples are in Python 2; they also work in
+ Python 3 if you replace u"..." by "...", unicode() by str()
+ and partly "..." by b"...".
+
+ quickstart::
+ >>> t = Template("hello @!name!@")
+ >>> print(t(name="marvin"))
+ hello marvin
+
+ quickstart with a template-file::
+ # >>> t = Template(filename="mytemplate.tmpl")
+ # >>> print(t(name="marvin"))
+ # hello marvin
+
+ generic usage::
+ >>> t = Template(u"output is in Unicode \\xe4\\xf6\\xfc\\u20ac")
+ >>> t #doctest: +ELLIPSIS
+ <...Template instance at 0x...>
+ >>> t()
+ u'output is in Unicode \\xe4\\xf6\\xfc\\u20ac'
+ >>> unicode(t)
+ u'output is in Unicode \\xe4\\xf6\\xfc\\u20ac'
+
+ with data::
+ >>> t = Template("hello @!name!@", data={"name":"world"})
+ >>> t()
+ u'hello world'
+ >>> t(name="worlds")
+ u'hello worlds'
+
+ # >>> t(note="data must be Unicode or ASCII", name=u"\\xe4")
+ # u'hello \\xe4'
+
+ escaping::
+ >>> t = Template("hello escaped: @!name!@, unescaped: $!name!$")
+ >>> t(name='''<>&'"''')
+ u'hello escaped: <>&'", unescaped: <>&\\'"'
+
+ result-encoding::
+ # encode the unicode-object to your encoding with encode()
+ >>> t = Template(u"hello \\xe4\\xf6\\xfc\\u20ac")
+ >>> result = t()
+ >>> result
+ u'hello \\xe4\\xf6\\xfc\\u20ac'
+ >>> result.encode("utf-8")
+ 'hello \\xc3\\xa4\\xc3\\xb6\\xc3\\xbc\\xe2\\x82\\xac'
+ >>> result.encode("ascii")
+ Traceback (most recent call last):
+ ...
+ UnicodeEncodeError: 'ascii' codec can't encode characters in position 6-9: ordinal not in range(128)
+ >>> result.encode("ascii", 'xmlcharrefreplace')
+ 'hello äöü€'
+
+ Python-expressions::
+ >>> Template('formatted: @! "%8.5f" % value !@')(value=3.141592653)
+ u'formatted: 3.14159'
+ >>> Template("hello --@!name.upper().center(20)!@--")(name="world")
+ u'hello -- WORLD --'
+ >>> Template("calculate @!var*5+7!@")(var=7)
+ u'calculate 42'
+
+ blocks (if/for/macros/...)::
+ >>> t = Template("<!--(if foo == 1)-->bar<!--(elif foo == 2)-->baz<!--(else)-->unknown(@!foo!@)<!--(end)-->")
+ >>> t(foo=2)
+ u'baz'
+ >>> t(foo=5)
+ u'unknown(5)'
+
+ >>> t = Template("<!--(for i in mylist)-->@!i!@ <!--(else)-->(empty)<!--(end)-->")
+ >>> t(mylist=[])
+ u'(empty)'
+ >>> t(mylist=[1,2,3])
+ u'1 2 3 '
+
+ >>> t = Template("<!--(for i,elem in enumerate(mylist))--> - @!i!@: @!elem!@<!--(end)-->")
+ >>> t(mylist=["a","b","c"])
+ u' - 0: a - 1: b - 2: c'
+
+ >>> t = Template('<!--(macro greetings)-->hello <strong>@!name!@</strong><!--(end)--> @!greetings(name=user)!@')
+ >>> t(user="monty")
+ u' hello <strong>monty</strong>'
+
+ exists::
+ >>> t = Template('<!--(if exists("foo"))-->YES<!--(else)-->NO<!--(end)-->')
+ >>> t()
+ u'NO'
+ >>> t(foo=1)
+ u'YES'
+ >>> t(foo=None) # note this difference to 'default()'
+ u'YES'
+
+ default-values::
+ # non-existing variables raise an error
+ >>> Template('hi @!optional!@')()
+ Traceback (most recent call last):
+ ...
+ TemplateRenderError: Cannot eval expression 'optional'. (NameError: name 'optional' is not defined)
+
+ >>> t = Template('hi @!default("optional","anyone")!@')
+ >>> t()
+ u'hi anyone'
+ >>> t(optional=None)
+ u'hi anyone'
+ >>> t(optional="there")
+ u'hi there'
+
+ # the 1st parameter can be any eval-expression
+ >>> t = Template('@!default("5*var1+var2","missing variable")!@')
+ >>> t(var1=10)
+ u'missing variable'
+ >>> t(var1=10, var2=2)
+ u'52'
+
+ # also in blocks
+ >>> t = Template('<!--(if default("opt1+opt2",0) > 0)-->yes<!--(else)-->no<!--(end)-->')
+ >>> t()
+ u'no'
+ >>> t(opt1=23, opt2=42)
+ u'yes'
+
+ >>> t = Template('<!--(for i in default("optional_list",[]))-->@!i!@<!--(end)-->')
+ >>> t()
+ u''
+ >>> t(optional_list=[1,2,3])
+ u'123'
+
+
+ # but make sure to put the expression in quotation marks, otherwise:
+ >>> Template('@!default(optional,"fallback")!@')()
+ Traceback (most recent call last):
+ ...
+ TemplateRenderError: Cannot eval expression 'default(optional,"fallback")'. (NameError: name 'optional' is not defined)
+
+ setvar::
+ >>> t = Template('$!setvar("i", "i+1")!$@!i!@')
+ >>> t(i=6)
+ u'7'
+
+ >>> t = Template('''<!--(if isinstance(s, (list,tuple)))-->$!setvar("s", '"\\\\\\\\n".join(s)')!$<!--(end)-->@!s!@''')
+ >>> t(isinstance=isinstance, s="123")
+ u'123'
+ >>> t(isinstance=isinstance, s=["123", "456"])
+ u'123\\n456'
+
+:Author: Roland Koebler (rk at simple-is-better dot org)
+:Copyright: Roland Koebler
+:License: MIT/X11-like, see __license__
+
+:RCS: $Id: pyratemp.py,v 1.12 2013/04/02 20:26:06 rk Exp $
+"""
+from __future__ import unicode_literals
+
+__version__ = "0.3.0"
+__author__ = "Roland Koebler <rk at simple-is-better dot org>"
+__license__ = """Copyright (c) Roland Koebler, 2007-2013
+
+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."""
+
+#=========================================
+
+import os, re, sys
+if sys.version_info[0] >= 3:
+ import builtins
+ unicode = str
+ long = int
+else:
+ import __builtin__ as builtins
+ from codecs import open
+
+#=========================================
+# some useful functions
+
+#----------------------
+# string-position: i <-> row,col
+
+def srow(string, i):
+ """Get line numer of ``string[i]`` in `string`.
+
+ :Returns: row, starting at 1
+ :Note: This works for text-strings with ``\\n`` or ``\\r\\n``.
+ """
+ return string.count('\n', 0, max(0, i)) + 1
+
+def scol(string, i):
+ """Get column number of ``string[i]`` in `string`.
+
+ :Returns: column, starting at 1 (but may be <1 if i<0)
+ :Note: This works for text-strings with ``\\n`` or ``\\r\\n``.
+ """
+ return i - string.rfind('\n', 0, max(0, i))
+
+def sindex(string, row, col):
+ """Get index of the character at `row`/`col` in `string`.
+
+ :Parameters:
+ - `row`: row number, starting at 1.
+ - `col`: column number, starting at 1.
+ :Returns: ``i``, starting at 0 (but may be <1 if row/col<0)
+ :Note: This works for text-strings with '\\n' or '\\r\\n'.
+ """
+ n = 0
+ for _ in range(row-1):
+ n = string.find('\n', n) + 1
+ return n+col-1
+
+#----------------------
+
+def dictkeyclean(d):
+ """Convert all keys of the dict `d` to strings.
+ """
+ new_d = {}
+ for k, v in d.items():
+ new_d[str(k)] = v
+ return new_d
+
+#----------------------
+
+def dummy(*_, **__):
+ """Dummy function, doing nothing.
+ """
+ pass
+
+def dummy_raise(exception, value):
+ """Create an exception-raising dummy function.
+
+ :Returns: dummy function, raising ``exception(value)``
+ """
+ def mydummy(*_, **__):
+ raise exception(value)
+ return mydummy
+
+#=========================================
+# escaping
+
+(NONE, HTML, LATEX, MAIL_HEADER) = range(0, 4)
+ESCAPE_SUPPORTED = {"NONE":None, "HTML":HTML, "LATEX":LATEX, "MAIL_HEADER":MAIL_HEADER}
+
+def escape(s, format=HTML):
+ """Replace special characters by their escape sequence.
+
+ :Parameters:
+ - `s`: unicode-string to escape
+ - `format`:
+
+ - `NONE`: nothing is replaced
+ - `HTML`: replace &<>'" by &...;
+ - `LATEX`: replace \#$%&_{}~^
+ - `MAIL_HEADER`: escape non-ASCII mail-header-contents
+ :Returns:
+ the escaped string in unicode
+ :Exceptions:
+ - `ValueError`: if `format` is invalid.
+
+ :Uses:
+ MAIL_HEADER uses module email
+ """
+ #Note: If you have to make sure that every character gets replaced
+ # only once (and if you cannot achieve this with the following code),
+ # use something like "".join([replacedict.get(c,c) for c in s])
+ # which is about 2-3 times slower (but maybe needs less memory).
+ #Note: This is one of the most time-consuming parts of the template.
+ if format is None or format == NONE:
+ pass
+ elif format == HTML:
+ s = s.replace("&", "&") # must be done first!
+ s = s.replace("<", "<")
+ s = s.replace(">", ">")
+ s = s.replace('"', """)
+ s = s.replace("'", "'")
+ elif format == LATEX:
+ s = s.replace("\\", "\\x") #must be done first!
+ s = s.replace("#", "\\#")
+ s = s.replace("$", "\\$")
+ s = s.replace("%", "\\%")
+ s = s.replace("&", "\\&")
+ s = s.replace("_", "\\_")
+ s = s.replace("{", "\\{")
+ s = s.replace("}", "\\}")
+ s = s.replace("\\x","\\textbackslash{}")
+ s = s.replace("~", "\\textasciitilde{}")
+ s = s.replace("^", "\\textasciicircum{}")
+ elif format == MAIL_HEADER:
+ import email.header
+ try:
+ s.encode("ascii")
+ return s
+ except UnicodeEncodeError:
+ return email.header.make_header([(s, "utf-8")]).encode()
+ else:
+ raise ValueError('Invalid format (only None, HTML, LATEX and MAIL_HEADER are supported).')
+ return s
+
+#=========================================
+
+#-----------------------------------------
+# Exceptions
+
+class TemplateException(Exception):
+ """Base class for template-exceptions."""
+ pass
+
+class TemplateParseError(TemplateException):
+ """Template parsing failed."""
+ def __init__(self, err, errpos):
+ """
+ :Parameters:
+ - `err`: error-message or exception to wrap
+ - `errpos`: ``(filename,row,col)`` where the error occured.
+ """
+ self.err = err
+ self.filename, self.row, self.col = errpos
+ TemplateException.__init__(self)
+ def __str__(self):
+ if not self.filename:
+ return "line %d, col %d: %s" % (self.row, self.col, str(self.err))
+ else:
+ return "file %s, line %d, col %d: %s" % (self.filename, self.row, self.col, str(self.err))
+
+class TemplateSyntaxError(TemplateParseError, SyntaxError):
+ """Template syntax-error."""
+ pass
+
+class TemplateIncludeError(TemplateParseError):
+ """Template 'include' failed."""
+ pass
+
+class TemplateRenderError(TemplateException):
+ """Template rendering failed."""
+ pass
+
+#-----------------------------------------
+# Loader
+
+class LoaderString:
+ """Load template from a string/unicode.
+
+ Note that 'include' is not possible in such templates.
+ """
+ def __init__(self, encoding='utf-8'):
+ self.encoding = encoding
+
+ def load(self, s):
+ """Return template-string as unicode.
+ """
+ if isinstance(s, unicode):
+ u = s
+ else:
+ u = s.decode(self.encoding)
+ return u
+
+class LoaderFile:
+ """Load template from a file.
+
+ When loading a template from a file, it's possible to including other
+ templates (by using 'include' in the template). But for simplicity
+ and security, all included templates have to be in the same directory!
+ (see ``allowed_path``)
+ """
+ def __init__(self, allowed_path=None, encoding='utf-8'):
+ """Init the loader.
+
+ :Parameters:
+ - `allowed_path`: path of the template-files
+ - `encoding`: encoding of the template-files
+ :Exceptions:
+ - `ValueError`: if `allowed_path` is not a directory
+ """
+ if allowed_path and not os.path.isdir(allowed_path):
+ raise ValueError("'allowed_path' has to be a directory.")
+ self.path = allowed_path
+ self.encoding = encoding
+
+ def load(self, filename):
+ """Load a template from a file.
+
+ Check if filename is allowed and return its contens in unicode.
+
+ :Parameters:
+ - `filename`: filename of the template without path
+ :Returns:
+ the contents of the template-file in unicode
+ :Exceptions:
+ - `ValueError`: if `filename` contains a path
+ """
+ if filename != os.path.basename(filename):
+ raise ValueError("No path allowed in filename. (%s)" %(filename))
+ filename = os.path.join(self.path, filename)
+
+ f = open(filename, 'r', encoding=self.encoding)
+ u = f.read()
+ f.close()
+
+ return u
+
+#-----------------------------------------
+# Parser
+
+class Parser(object):
+ """Parse a template into a parse-tree.
+
+ Includes a syntax-check, an optional expression-check and verbose
+ error-messages.
+
+ See documentation for a description of the parse-tree.
+ """
+ # template-syntax
+ _comment_start = "#!"
+ _comment_end = "!#"
+ _sub_start = "$!"
+ _sub_end = "!$"
+ _subesc_start = "@!"
+ _subesc_end = "!@"
+ _block_start = "<!--("
+ _block_end = ")-->"
+
+ # build regexps
+ # comment
+ # single-line, until end-tag or end-of-line.
+ _strComment = r"""%s(?P<content>.*?)(?P<end>%s|\n|$)""" \
+ % (re.escape(_comment_start), re.escape(_comment_end))
+ _reComment = re.compile(_strComment, re.M)
+
+ # escaped or unescaped substitution
+ # single-line ("|$" is needed to be able to generate good error-messges)
+ _strSubstitution = r"""
+ (
+ %s\s*(?P<sub>.*?)\s*(?P<end>%s|$) #substitution
+ |
+ %s\s*(?P<escsub>.*?)\s*(?P<escend>%s|$) #escaped substitution
+ )
+ """ % (re.escape(_sub_start), re.escape(_sub_end),
+ re.escape(_subesc_start), re.escape(_subesc_end))
+ _reSubstitution = re.compile(_strSubstitution, re.X|re.M)
+
+ # block
+ # - single-line, no nesting.
+ # or
+ # - multi-line, nested by whitespace indentation:
+ # * start- and end-tag of a block must have exactly the same indentation.
+ # * start- and end-tags of *nested* blocks should have a greater indentation.
+ # NOTE: A single-line block must not start at beginning of the line with
+ # the same indentation as the enclosing multi-line blocks!
+ # Note that " " and "\t" are different, although they may
+ # look the same in an editor!
+ _s = re.escape(_block_start)
+ _e = re.escape(_block_end)
+ _strBlock = r"""
+ ^(?P<mEnd>[ \t]*)%send%s(?P<meIgnored>.*)\r?\n? # multi-line end (^ <!--(end)-->IGNORED_TEXT\n)
+ |
+ (?P<sEnd>)%send%s # single-line end (<!--(end)-->)
+ |
+ (?P<sSpace>[ \t]*) # single-line tag (no nesting)
+ %s(?P<sKeyw>\w+)[ \t]*(?P<sParam>.*?)%s
+ (?P<sContent>.*?)
+ (?=(?:%s.*?%s.*?)??%send%s) # (match until end or i.e. <!--(elif/else...)-->)
+ |
+ # multi-line tag, nested by whitespace indentation
+ ^(?P<indent>[ \t]*) # save indentation of start tag
+ %s(?P<mKeyw>\w+)\s*(?P<mParam>.*?)%s(?P<mIgnored>.*)\r?\n
+ (?P<mContent>(?:.*\n)*?)
+ (?=(?P=indent)%s(?:.|\s)*?%s) # match indentation
+ """ % (_s, _e,
+ _s, _e,
+ _s, _e, _s, _e, _s, _e,
+ _s, _e, _s, _e)
+ _reBlock = re.compile(_strBlock, re.X|re.M)
+
+ # "for"-block parameters: "var(,var)* in ..."
+ _strForParam = r"""^(?P<names>\w+(?:\s*,\s*\w+)*)\s+in\s+(?P<iter>.+)$"""
+ _reForParam = re.compile(_strForParam)
+
+ # allowed macro-names
+ _reMacroParam = re.compile(r"""^\w+$""")
+
+
+ def __init__(self, loadfunc=None, testexpr=None, escape=HTML):
+ """Init the parser.
+
+ :Parameters:
+ - `loadfunc`: function to load included templates
+ (i.e. ``LoaderFile(...).load``)
+ - `testexpr`: function to test if a template-expressions is valid
+ (i.e. ``EvalPseudoSandbox().compile``)
+ - `escape`: default-escaping (may be modified by the template)
+ :Exceptions:
+ - `ValueError`: if `testexpr` or `escape` is invalid.
+ """
+ if loadfunc is None:
+ self._load = dummy_raise(NotImplementedError, "'include' not supported, since no 'loadfunc' was given.")
+ else:
+ self._load = loadfunc
+
+ if testexpr is None:
+ self._testexprfunc = dummy
+ else:
+ try: # test if testexpr() works
+ testexpr("i==1")
+ except Exception as err:
+ raise ValueError("Invalid 'testexpr'. (%s)" %(err))
+ self._testexprfunc = testexpr
+
+ if escape not in ESCAPE_SUPPORTED.values():
+ raise ValueError("Unsupported 'escape'. (%s)" %(escape))
+ self.escape = escape
+ self._includestack = []
+
+ def parse(self, template):
+ """Parse a template.
+
+ :Parameters:
+ - `template`: template-unicode-string
+ :Returns: the resulting parse-tree
+ :Exceptions:
+ - `TemplateSyntaxError`: for template-syntax-errors
+ - `TemplateIncludeError`: if template-inclusion failed
+ - `TemplateException`
+ """
+ self._includestack = [(None, template)] # for error-messages (_errpos)
+ return self._parse(template)
+
+ def _errpos(self, fpos):
+ """Convert `fpos` to ``(filename,row,column)`` for error-messages."""
+ filename, string = self._includestack[-1]
+ return filename, srow(string, fpos), scol(string, fpos)
+
+ def _testexpr(self, expr, fpos=0):
+ """Test a template-expression to detect errors."""
+ try:
+ self._testexprfunc(expr)
+ except SyntaxError as err:
+ raise TemplateSyntaxError(err, self._errpos(fpos))
+
+ def _parse_sub(self, parsetree, text, fpos=0):
+ """Parse substitutions, and append them to the parse-tree.
+
+ Additionally, remove comments.
+ """
+ curr = 0
+ for match in self._reSubstitution.finditer(text):
+ start = match.start()
+ if start > curr:
+ parsetree.append(("str", self._reComment.sub('', text[curr:start])))
+
+ if match.group("sub") is not None:
+ if not match.group("end"):
+ raise TemplateSyntaxError("Missing closing tag '%s' for '%s'."
+ % (self._sub_end, match.group()), self._errpos(fpos+start))
+ if len(match.group("sub")) > 0:
+ self._testexpr(match.group("sub"), fpos+start)
+ parsetree.append(("sub", match.group("sub")))
+ else:
+ assert(match.group("escsub") is not None)
+ if not match.group("escend"):
+ raise TemplateSyntaxError("Missing closing tag '%s' for '%s'."
+ % (self._subesc_end, match.group()), self._errpos(fpos+start))
+ if len(match.group("escsub")) > 0:
+ self._testexpr(match.group("escsub"), fpos+start)
+ parsetree.append(("esc", self.escape, match.group("escsub")))
+
+ curr = match.end()
+
+ if len(text) > curr:
+ parsetree.append(("str", self._reComment.sub('', text[curr:])))
+
+ def _parse(self, template, fpos=0):
+ """Recursive part of `parse()`.
+
+ :Parameters:
+ - template
+ - fpos: position of ``template`` in the complete template (for error-messages)
+ """
+ # blank out comments
+ # (So that its content does not collide with other syntax, and
+ # because removing them completely would falsify the character-
+ # position ("match.start()") of error-messages)
+ template = self._reComment.sub(lambda match: self._comment_start+" "*len(match.group(1))+match.group(2), template)
+
+ # init parser
+ parsetree = []
+ curr = 0 # current position (= end of previous block)
+ block_type = None # block type: if,for,macro,raw,...
+ block_indent = None # None: single-line, >=0: multi-line
+
+ # find blocks
+ for match in self._reBlock.finditer(template):
+ start = match.start()
+ # process template-part before this block
+ if start > curr:
+ self._parse_sub(parsetree, template[curr:start], fpos)
+
+ # analyze block syntax (incl. error-checking and -messages)
+ keyword = None
+ block = match.groupdict()
+ pos__ = fpos + start # shortcut
+ if block["sKeyw"] is not None: # single-line block tag
+ block_indent = None
+ keyword = block["sKeyw"]
+ param = block["sParam"]
+ content = block["sContent"]
+ if block["sSpace"]: # restore spaces before start-tag
+ if len(parsetree) > 0 and parsetree[-1][0] == "str":
+ parsetree[-1] = ("str", parsetree[-1][1] + block["sSpace"])
+ else:
+ parsetree.append(("str", block["sSpace"]))
+ pos_p = fpos + match.start("sParam") # shortcuts
+ pos_c = fpos + match.start("sContent")
+ elif block["mKeyw"] is not None: # multi-line block tag
+ block_indent = len(block["indent"])
+ keyword = block["mKeyw"]
+ param = block["mParam"]
+ content = block["mContent"]
+ pos_p = fpos + match.start("mParam")
+ pos_c = fpos + match.start("mContent")
+ ignored = block["mIgnored"].strip()
+ if ignored and ignored != self._comment_start:
+ raise TemplateSyntaxError("No code allowed after block-tag.", self._errpos(fpos+match.start("mIgnored")))
+ elif block["mEnd"] is not None: # multi-line block end
+ if block_type is None:
+ raise TemplateSyntaxError("No block to end here/invalid indent.", self._errpos(pos__) )
+ if block_indent != len(block["mEnd"]):
+ raise TemplateSyntaxError("Invalid indent for end-tag.", self._errpos(pos__) )
+ ignored = block["meIgnored"].strip()
+ if ignored and ignored != self._comment_start:
+ raise TemplateSyntaxError("No code allowed after end-tag.", self._errpos(fpos+match.start("meIgnored")))
+ block_type = None
+ elif block["sEnd"] is not None: # single-line block end
+ if block_type is None:
+ raise TemplateSyntaxError("No block to end here/invalid indent.", self._errpos(pos__))
+ if block_indent is not None:
+ raise TemplateSyntaxError("Invalid indent for end-tag.", self._errpos(pos__))
+ block_type = None
+ else:
+ raise TemplateException("FATAL: Block regexp error. Please contact the author. (%s)" % match.group())
+
+ # analyze block content (mainly error-checking and -messages)
+ if keyword:
+ keyword = keyword.lower()
+ if 'for' == keyword:
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing block-end-tag before new block at '%s'." %(match.group()), self._errpos(pos__))
+ block_type = 'for'
+ cond = self._reForParam.match(param)
+ if cond is None:
+ raise TemplateSyntaxError("Invalid 'for ...' at '%s'." %(param), self._errpos(pos_p))
+ names = tuple(n.strip() for n in cond.group("names").split(","))
+ self._testexpr(cond.group("iter"), pos_p+cond.start("iter"))
+ parsetree.append(("for", names, cond.group("iter"), self._parse(content, pos_c)))
+ elif 'if' == keyword:
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing block-end-tag before new block at '%s'." %(match.group()), self._errpos(pos__))
+ if not param:
+ raise TemplateSyntaxError("Missing condition for 'if' at '%s'." %(match.group()), self._errpos(pos__))
+ block_type = 'if'
+ self._testexpr(param, pos_p)
+ parsetree.append(("if", param, self._parse(content, pos_c)))
+ elif 'elif' == keyword:
+ if block_type != 'if':
+ raise TemplateSyntaxError("'elif' may only appear after 'if' at '%s'." %(match.group()), self._errpos(pos__))
+ if not param:
+ raise TemplateSyntaxError("Missing condition for 'elif' at '%s'." %(match.group()), self._errpos(pos__))
+ self._testexpr(param, pos_p)
+ parsetree.append(("elif", param, self._parse(content, pos_c)))
+ elif 'else' == keyword:
+ if block_type not in ('if', 'for'):
+ raise TemplateSyntaxError("'else' may only appear after 'if' or 'for' at '%s'." %(match.group()), self._errpos(pos__))
+ if param:
+ raise TemplateSyntaxError("'else' may not have parameters at '%s'." %(match.group()), self._errpos(pos__))
+ parsetree.append(("else", self._parse(content, pos_c)))
+ elif 'macro' == keyword:
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__))
+ block_type = 'macro'
+ # make sure param is "\w+" (instead of ".+")
+ if not param:
+ raise TemplateSyntaxError("Missing name for 'macro' at '%s'." %(match.group()), self._errpos(pos__))
+ if not self._reMacroParam.match(param):
+ raise TemplateSyntaxError("Invalid name for 'macro' at '%s'." %(match.group()), self._errpos(pos__))
+ #remove last newline
+ if len(content) > 0 and content[-1] == '\n':
+ content = content[:-1]
+ if len(content) > 0 and content[-1] == '\r':
+ content = content[:-1]
+ parsetree.append(("macro", param, self._parse(content, pos_c)))
+
+ # parser-commands
+ elif 'raw' == keyword:
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__))
+ if param:
+ raise TemplateSyntaxError("'raw' may not have parameters at '%s'." %(match.group()), self._errpos(pos__))
+ block_type = 'raw'
+ parsetree.append(("str", content))
+ elif 'include' == keyword:
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__))
+ if param:
+ raise TemplateSyntaxError("'include' may not have parameters at '%s'." %(match.group()), self._errpos(pos__))
+ block_type = 'include'
+ try:
+ u = self._load(content.strip())
+ except Exception as err:
+ raise TemplateIncludeError(err, self._errpos(pos__))
+ self._includestack.append((content.strip(), u)) # current filename/template for error-msg.
+ p = self._parse(u)
+ self._includestack.pop()
+ parsetree.extend(p)
+ elif 'set_escape' == keyword:
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__))
+ if param:
+ raise TemplateSyntaxError("'set_escape' may not have parameters at '%s'." %(match.group()), self._errpos(pos__))
+ block_type = 'set_escape'
+ esc = content.strip().upper()
+ if esc not in ESCAPE_SUPPORTED:
+ raise TemplateSyntaxError("Unsupported escape '%s'." %(esc), self._errpos(pos__))
+ self.escape = ESCAPE_SUPPORTED[esc]
+ else:
+ raise TemplateSyntaxError("Invalid keyword '%s'." %(keyword), self._errpos(pos__))
+ curr = match.end()
+
+ if block_type is not None:
+ raise TemplateSyntaxError("Missing end-tag.", self._errpos(pos__))
+
+ if len(template) > curr: # process template-part after last block
+ self._parse_sub(parsetree, template[curr:], fpos+curr)
+
+ return parsetree
+
+#-----------------------------------------
+# Evaluation
+
+# some checks
+assert len(eval("dir()", {'__builtins__':{'dir':dir}})) == 1, \
+ "FATAL: 'eval' does not work as expected (%s)."
+assert compile("0 .__class__", "<string>", "eval").co_names == ('__class__',), \
+ "FATAL: 'compile' does not work as expected."
+
+class EvalPseudoSandbox:
+ """An eval-pseudo-sandbox.
+
+ The pseudo-sandbox restricts the available functions/objects, so the
+ code can only access:
+
+ - some of the builtin Python-functions, which are considered "safe"
+ (see safe_builtins)
+ - some additional functions (exists(), default(), setvar(), escape())
+ - the passed objects incl. their methods.
+
+ Additionally, names beginning with "_" are forbidden.
+ This is to prevent things like '0 .__class__', with which you could
+ easily break out of a "sandbox".
+
+ Be careful to only pass "safe" objects/functions to the template,
+ because any unsafe function/method could break the sandbox!
+ For maximum security, restrict the access to as few objects/functions
+ as possible!
+
+ :Warning:
+ Note that this is no real sandbox! (And although I don't know any
+ way to break out of the sandbox without passing-in an unsafe object,
+ I cannot guarantee that there is no such way. So use with care.)
+
+ Take care if you want to use it for untrusted code!!
+ """
+
+ safe_builtins = {
+ "True" : True,
+ "False" : False,
+ "None" : None,
+
+ "abs" : builtins.abs,
+ "chr" : builtins.chr,
+ "divmod" : builtins.divmod,
+ "hash" : builtins.hash,
+ "hex" : builtins.hex,
+ "len" : builtins.len,
+ "max" : builtins.max,
+ "min" : builtins.min,
+ "oct" : builtins.oct,
+ "ord" : builtins.ord,
+ "pow" : builtins.pow,
+ "range" : builtins.range,
+ "round" : builtins.round,
+ "sorted" : builtins.sorted,
+ "sum" : builtins.sum,
+ "unichr" : builtins.chr,
+ "zip" : builtins.zip,
+
+ "bool" : builtins.bool,
+ "bytes" : builtins.bytes,
+ "complex" : builtins.complex,
+ "dict" : builtins.dict,
+ "enumerate" : builtins.enumerate,
+ "float" : builtins.float,
+ "int" : builtins.int,
+ "list" : builtins.list,
+ "long" : long,
+ "reversed" : builtins.reversed,
+ "str" : builtins.str,
+ "tuple" : builtins.tuple,
+ "unicode" : unicode,
+ }
+ if sys.version_info[0] < 3:
+ safe_builtins["unichr"] = builtins.unichr
+
+ def __init__(self):
+ self._compile_cache = {}
+ self.locals_ptr = None
+ self.eval_allowed_globals = self.safe_builtins.copy()
+ self.register("__import__", self.f_import)
+ self.register("exists", self.f_exists)
+ self.register("default", self.f_default)
+ self.register("setvar", self.f_setvar)
+ self.register("escape", self.f_escape)
+
+ def register(self, name, obj):
+ """Add an object to the "allowed eval-globals".
+
+ Mainly useful to add user-defined functions to the pseudo-sandbox.
+ """
+ self.eval_allowed_globals[name] = obj
+
+ def compile(self, expr):
+ """Compile a Python-eval-expression.
+
+ - Use a compile-cache.
+ - Raise a `NameError` if `expr` contains a name beginning with ``_``.
+
+ :Returns: the compiled `expr`
+ :Exceptions:
+ - `SyntaxError`: for compile-errors
+ - `NameError`: if expr contains a name beginning with ``_``
+ """
+ if expr not in self._compile_cache:
+ c = compile(expr, "", "eval")
+ for i in c.co_names: #prevent breakout via new-style-classes
+ if i[0] == '_':
+ raise NameError("Name '%s' is not allowed." % i)
+ self._compile_cache[expr] = c
+ return self._compile_cache[expr]
+
+ def eval(self, expr, locals):
+ """Eval a Python-eval-expression.
+
+ Sets ``self.locals_ptr`` to ``locales`` and compiles the code
+ before evaluating.
+ """
+ sav = self.locals_ptr
+ self.locals_ptr = locals
+ x = eval(self.compile(expr), {"__builtins__":self.eval_allowed_globals}, locals)
+ self.locals_ptr = sav
+ return x
+
+ def f_import(self, name, *_, **__):
+ """``import``/``__import__()`` for the sandboxed code.
+
+ Since "import" is insecure, the PseudoSandbox does not allow to
+ import other modules. But since some functions need to import
+ other modules (e.g. "datetime.datetime.strftime" imports "time"),
+ this function replaces the builtin "import" and allows to use
+ modules which are already accessible by the sandboxed code.
+
+ :Note:
+ - This probably only works for rather simple imports.
+ - For security, it may be better to avoid such (complex) modules
+ which import other modules. (e.g. use time.localtime and
+ time.strftime instead of datetime.datetime.strftime,
+ or write a small wrapper.)
+
+ :Example:
+
+ >>> from datetime import datetime
+ >>> import pyratemp
+ >>> t = pyratemp.Template('@!mytime.strftime("%H:%M:%S")!@')
+
+ # >>> print(t(mytime=datetime.now()))
+ # Traceback (most recent call last):
+ # ...
+ # ImportError: import not allowed in pseudo-sandbox; try to import 'time' yourself and pass it to the sandbox/template
+
+ >>> import time
+ >>> print(t(mytime=datetime.strptime("13:40:54", "%H:%M:%S"), time=time))
+ 13:40:54
+
+ # >>> print(t(mytime=datetime.now(), time=time))
+ # 13:40:54
+ """
+ import types
+ if self.locals_ptr is not None and name in self.locals_ptr and isinstance(self.locals_ptr[name], types.ModuleType):
+ return self.locals_ptr[name]
+ else:
+ raise ImportError("import not allowed in pseudo-sandbox; try to import '%s' yourself (and maybe pass it to the sandbox/template)" % name)
+
+ def f_exists(self, varname):
+ """``exists()`` for the sandboxed code.
+
+ Test if the variable `varname` exists in the current locals-namespace.
+
+ This only works for single variable names. If you want to test
+ complicated expressions, use i.e. `default`.
+ (i.e. `default("expr",False)`)
+
+ :Note: the variable-name has to be quoted! (like in eval)
+ :Example: see module-docstring
+ """
+ return (varname in self.locals_ptr)
+
+ def f_default(self, expr, default=None):
+ """``default()`` for the sandboxed code.
+
+ Try to evaluate an expression and return the result or a
+ fallback-/default-value; the `default`-value is used
+ if `expr` does not exist/is invalid/results in None.
+
+ This is very useful for optional data.
+
+ :Parameter:
+ - expr: eval-expression
+ - default: fallback-falue if eval(expr) fails or is None.
+ :Returns:
+ the eval-result or the "fallback"-value.
+
+ :Note: the eval-expression has to be quoted! (like in eval)
+ :Example: see module-docstring
+ """
+ try:
+ r = self.eval(expr, self.locals_ptr)
+ if r is None:
+ return default
+ return r
+ #TODO: which exceptions should be catched here?
+ except (NameError, LookupError, TypeError):
+ return default
+
+ def f_setvar(self, name, expr):
+ """``setvar()`` for the sandboxed code.
+
+ Set a variable.
+
+ :Example: see module-docstring
+ """
+ self.locals_ptr[name] = self.eval(expr, self.locals_ptr)
+ return ""
+
+ def f_escape(self, s, format="HTML"):
+ """``escape()`` for the sandboxed code.
+ """
+ if isinstance(format, (str, unicode)):
+ format = ESCAPE_SUPPORTED[format.upper()]
+ return escape(unicode(s), format)
+
+#-----------------------------------------
+# basic template / subtemplate
+
+class TemplateBase:
+ """Basic template-class.
+
+ Used both for the template itself and for 'macro's ("subtemplates") in
+ the template.
+ """
+
+ def __init__(self, parsetree, renderfunc, data=None):
+ """Create the Template/Subtemplate/Macro.
+
+ :Parameters:
+ - `parsetree`: parse-tree of the template/subtemplate/macro
+ - `renderfunc`: render-function
+ - `data`: data to fill into the template by default (dictionary).
+ This data may later be overridden when rendering the template.
+ :Exceptions:
+ - `TypeError`: if `data` is not a dictionary
+ """
+ #TODO: parameter-checking?
+ self.parsetree = parsetree
+ if isinstance(data, dict):
+ self.data = data
+ elif data is None:
+ self.data = {}
+ else:
+ raise TypeError('"data" must be a dict (or None).')
+ self.current_data = data
+ self._render = renderfunc
+
+ def __call__(self, **override):
+ """Fill out/render the template.
+
+ :Parameters:
+ - `override`: objects to add to the data-namespace, overriding
+ the "default"-data.
+ :Returns: the filled template (in unicode)
+ :Note: This is also called when invoking macros
+ (i.e. ``$!mymacro()!$``).
+ """
+ self.current_data = self.data.copy()
+ self.current_data.update(override)
+ u = "".join(self._render(self.parsetree, self.current_data))
+ self.current_data = self.data # restore current_data
+ return _dontescape(u) # (see class _dontescape)
+
+ def __unicode__(self):
+ """Alias for __call__()."""
+ return self.__call__()
+ def __str__(self):
+ """Alias for __call__()."""
+ return self.__call__()
+
+#-----------------------------------------
+# Renderer
+
+class _dontescape(unicode):
+ """Unicode-string which should not be escaped.
+
+ If ``isinstance(object,_dontescape)``, then don't escape the object in
+ ``@!...!@``. It's useful for not double-escaping macros, and it's
+ automatically used for macros/subtemplates.
+
+ :Note: This only works if the object is used on its own in ``@!...!@``.
+ It i.e. does not work in ``@!object*2!@`` or ``@!object + "hi"!@``.
+ """
+ __slots__ = []
+
+
+class Renderer(object):
+ """Render a template-parse-tree.
+
+ :Uses: `TemplateBase` for macros
+ """
+
+ def __init__(self, evalfunc, escapefunc):
+ """Init the renderer.
+
+ :Parameters:
+ - `evalfunc`: function for template-expression-evaluation
+ (i.e. ``EvalPseudoSandbox().eval``)
+ - `escapefunc`: function for escaping special characters
+ (i.e. `escape`)
+ """
+ #TODO: test evalfunc
+ self.evalfunc = evalfunc
+ self.escapefunc = escapefunc
+
+ def _eval(self, expr, data):
+ """evalfunc with error-messages"""
+ try:
+ return self.evalfunc(expr, data)
+ #TODO: any other errors to catch here?
+ except (TypeError,NameError,LookupError,AttributeError, SyntaxError) as err:
+ raise TemplateRenderError("Cannot eval expression '%s'. (%s: %s)" %(expr, err.__class__.__name__, err))
+
+ def render(self, parsetree, data):
+ """Render a parse-tree of a template.
+
+ :Parameters:
+ - `parsetree`: the parse-tree
+ - `data`: the data to fill into the template (dictionary)
+ :Returns: the rendered output-unicode-string
+ :Exceptions:
+ - `TemplateRenderError`
+ """
+ _eval = self._eval # shortcut
+ output = []
+ do_else = False # use else/elif-branch?
+
+ if parsetree is None:
+ return ""
+ for elem in parsetree:
+ if "str" == elem[0]:
+ output.append(elem[1])
+ elif "sub" == elem[0]:
+ output.append(unicode(_eval(elem[1], data)))
+ elif "esc" == elem[0]:
+ obj = _eval(elem[2], data)
+ #prevent double-escape
+ if isinstance(obj, _dontescape) or isinstance(obj, TemplateBase):
+ output.append(unicode(obj))
+ else:
+ output.append(self.escapefunc(unicode(obj), elem[1]))
+ elif "for" == elem[0]:
+ do_else = True
+ (names, iterable) = elem[1:3]
+ try:
+ loop_iter = iter(_eval(iterable, data))
+ except TypeError:
+ raise TemplateRenderError("Cannot loop over '%s'." % iterable)
+ for i in loop_iter:
+ do_else = False
+ if len(names) == 1:
+ data[names[0]] = i
+ else:
+ data.update(zip(names, i)) #"for a,b,.. in list"
+ output.extend(self.render(elem[3], data))
+ elif "if" == elem[0]:
+ do_else = True
+ if _eval(elem[1], data):
+ do_else = False
+ output.extend(self.render(elem[2], data))
+ elif "elif" == elem[0]:
+ if do_else and _eval(elem[1], data):
+ do_else = False
+ output.extend(self.render(elem[2], data))
+ elif "else" == elem[0]:
+ if do_else:
+ do_else = False
+ output.extend(self.render(elem[1], data))
+ elif "macro" == elem[0]:
+ data[elem[1]] = TemplateBase(elem[2], self.render, data)
+ else:
+ raise TemplateRenderError("Invalid parse-tree (%s)." %(elem))
+
+ return output
+
+#-----------------------------------------
+# template user-interface (putting it all together)
+
+class Template(TemplateBase):
+ """Template-User-Interface.
+
+ :Usage:
+ ::
+ t = Template(...) (<- see __init__)
+ output = t(...) (<- see TemplateBase.__call__)
+
+ :Example:
+ see module-docstring
+ """
+
+ def __init__(self, string=None,filename=None,parsetree=None, encoding='utf-8', data=None, escape=HTML,
+ loader_class=LoaderFile,
+ parser_class=Parser,
+ renderer_class=Renderer,
+ eval_class=EvalPseudoSandbox,
+ escape_func=escape):
+ """Load (+parse) a template.
+
+ :Parameters:
+ - `string,filename,parsetree`: a template-string,
+ filename of a template to load,
+ or a template-parsetree.
+ (only one of these 3 is allowed)
+ - `encoding`: encoding of the template-files (only used for "filename")
+ - `data`: data to fill into the template by default (dictionary).
+ This data may later be overridden when rendering the template.
+ - `escape`: default-escaping for the template, may be overwritten by the template!
+ - `loader_class`
+ - `parser_class`
+ - `renderer_class`
+ - `eval_class`
+ - `escapefunc`
+ """
+ if [string, filename, parsetree].count(None) != 2:
+ raise ValueError('Exactly 1 of string,filename,parsetree is necessary.')
+
+ tmpl = None
+ # load template
+ if filename is not None:
+ incl_load = loader_class(os.path.dirname(filename), encoding).load
+ tmpl = incl_load(os.path.basename(filename))
+ if string is not None:
+ incl_load = dummy_raise(NotImplementedError, "'include' not supported for template-strings.")
+ tmpl = LoaderString(encoding).load(string)
+
+ # eval (incl. compile-cache)
+ templateeval = eval_class()
+
+ # parse
+ if tmpl is not None:
+ p = parser_class(loadfunc=incl_load, testexpr=templateeval.compile, escape=escape)
+ parsetree = p.parse(tmpl)
+ del p
+
+ # renderer
+ renderfunc = renderer_class(templateeval.eval, escape_func).render
+
+ #create template
+ TemplateBase.__init__(self, parsetree, renderfunc, data)
+
+
+#=========================================
+#doctest
+
+def _doctest():
+ """doctest this module."""
+ import doctest
+ doctest.testmod()
+
+#----------------------
+if __name__ == '__main__':
+ if sys.version_info[0] <= 2:
+ _doctest()
+
+#=========================================
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/dsdl_compiler/libuavcan_dsdlc Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+#
+# UAVCAN DSDL compiler for libuavcan
+# Supported Python versions: 3.2+, 2.7.
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+from __future__ import division, absolute_import, print_function, unicode_literals
+import os, sys, logging, argparse
+
+# This trickery allows to use the compiler even if pyuavcan is not installed in the system.
+# This is extremely important, as it makes the compiler (and therefore libuavcan in general)
+# totally dependency-free, except for the Python interpreter itself.
+SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+LOCAL_PYUAVCAN_DIR = os.path.join(SCRIPT_DIR, 'pyuavcan')
+RUNNING_FROM_SRC_DIR = os.path.isdir(LOCAL_PYUAVCAN_DIR)
+if RUNNING_FROM_SRC_DIR:
+ #print('Running from the source directory')
+ sys.path.insert(0, SCRIPT_DIR)
+ sys.path.insert(0, LOCAL_PYUAVCAN_DIR)
+
+def configure_logging(verbosity):
+ fmt = '%(message)s'
+ level = { 0: logging.WARNING, 1: logging.INFO, 2: logging.DEBUG }.get(verbosity or 0, logging.DEBUG)
+ logging.basicConfig(stream=sys.stderr, level=level, format=fmt)
+
+def die(text):
+ print(text, file=sys.stderr)
+ exit(1)
+
+DEFAULT_OUTDIR = 'dsdlc_generated'
+
+DESCRIPTION = '''UAVCAN DSDL compiler for libuavcan.
+Takes an input directory that contains an hierarchy of DSDL
+definitions and converts it into compatible hierarchy of C++ types for libuavcan.
+This script can be used directly from the source directory, no installation required!
+Supported Python versions: 3.2+, 2.7.
+'''
+
+argparser = argparse.ArgumentParser(description=DESCRIPTION)
+argparser.add_argument('source_dir', nargs='+', help='source directory with DSDL definitions')
+argparser.add_argument('--verbose', '-v', action='count', help='verbosity level (-v, -vv)')
+argparser.add_argument('--outdir', '-O', default=DEFAULT_OUTDIR, help='output directory, default %s' % DEFAULT_OUTDIR)
+argparser.add_argument('--incdir', '-I', default=[], action='append', help=
+'''nested type namespaces, one path per argument. Can be also specified through the environment variable
+UAVCAN_DSDL_INCLUDE_PATH, where the path entries are separated by colons ":"''')
+args = argparser.parse_args()
+
+configure_logging(args.verbose)
+
+try:
+ extra_incdir = os.environ['UAVCAN_DSDL_INCLUDE_PATH'].split(':')
+ logging.info('Additional include directories: %s', extra_incdir)
+ args.incdir += extra_incdir
+except KeyError:
+ pass
+
+from libuavcan_dsdl_compiler import run as dsdlc_run
+try:
+ dsdlc_run(args.source_dir, args.incdir, args.outdir)
+except Exception as ex:
+ logging.error('Compiler failure', exc_info=True)
+ die(str(ex))
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/dsdl_compiler/setup.py Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+
+args = dict(
+ name='libuavcan_dsdl_compiler',
+ version='0.1',
+ description='UAVCAN DSDL compiler for libuavcan',
+ packages=['libuavcan_dsdl_compiler'],
+ package_data={'libuavcan_dsdl_compiler': ['data_type_template.tmpl']},
+ scripts=['libuavcan_dsdlc'],
+ requires=['uavcan'],
+ author='Pavel Kirienko',
+ author_email='pavel.kirienko@gmail.com',
+ url='http://uavcan.org',
+ license='MIT'
+)
+
+setup(**args)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/include.mk Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,24 @@ +# +# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> +# + +LIBUAVCAN_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +UAVCAN_DIR := $(LIBUAVCAN_DIR)../ + +# +# Library sources +# +LIBUAVCAN_SRC := $(shell find $(LIBUAVCAN_DIR)src -type f -name '*.cpp') + +LIBUAVCAN_INC := $(LIBUAVCAN_DIR)include + +# +# DSDL compiler executable +# +LIBUAVCAN_DSDLC := $(LIBUAVCAN_DIR)dsdl_compiler/libuavcan_dsdlc + +# +# Standard DSDL definitions +# +UAVCAN_DSDL_DIR := $(UAVCAN_DIR)dsdl/uavcan
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/build_config.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,276 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_BUILD_CONFIG_HPP_INCLUDED
+#define UAVCAN_BUILD_CONFIG_HPP_INCLUDED
+
+/**
+ * UAVCAN version definition
+ */
+#define UAVCAN_VERSION_MAJOR 1
+#define UAVCAN_VERSION_MINOR 0
+
+/**
+ * UAVCAN_CPP_VERSION - version of the C++ standard used during compilation.
+ * This definition contains the integer year number after which the standard was named:
+ * - 2003 for C++03
+ * - 2011 for C++11
+ *
+ * This config automatically sets according to the actual C++ standard used by the compiler.
+ *
+ * In C++03 mode the library has almost zero dependency on the C++ standard library, which allows
+ * to use it on platforms with a very limited C++ support. On the other hand, C++11 mode requires
+ * many parts of the standard library (e.g. <functional>), thus the user might want to force older
+ * standard than used by the compiler, in which case this symbol can be overridden manually via
+ * compiler flags.
+ */
+#define UAVCAN_CPP11 2011
+#define UAVCAN_CPP03 2003
+
+#ifndef UAVCAN_CPP_VERSION
+# if __cplusplus > 201200
+# error Unsupported C++ standard. You can explicitly set UAVCAN_CPP_VERSION=UAVCAN_CPP11 to silence this error.
+# elif (__cplusplus > 201100) || defined(__GXX_EXPERIMENTAL_CXX0X__)
+# define UAVCAN_CPP_VERSION UAVCAN_CPP11
+# else
+# define UAVCAN_CPP_VERSION UAVCAN_CPP03
+# endif
+#endif
+
+/**
+ * The library uses UAVCAN_NULLPTR instead of UAVCAN_NULLPTR and nullptr in order to allow the use of
+ * -Wzero-as-null-pointer-constant.
+ */
+#ifndef UAVCAN_NULLPTR
+# if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# define UAVCAN_NULLPTR nullptr
+# else
+# define UAVCAN_NULLPTR NULL
+# endif
+#endif
+
+/**
+ * By default, libuavcan enables all features if it detects that it is being built for a general-purpose
+ * target like Linux. Value of this macro influences other configuration options located below in this file.
+ * This macro can be overriden if needed.
+ */
+#ifndef UAVCAN_GENERAL_PURPOSE_PLATFORM
+# if (defined(__linux__) || defined(__linux) || defined(__APPLE__) ||\
+ defined(_WIN64) || defined(_WIN32) || defined(__ANDROID__) ||\
+ defined(_SYSTYPE_BSD) || defined(__FreeBSD__))
+# define UAVCAN_GENERAL_PURPOSE_PLATFORM 1
+# else
+# define UAVCAN_GENERAL_PURPOSE_PLATFORM 0
+# endif
+#endif
+
+/**
+ * This macro enables built-in runtime checks and debug output via printf().
+ * Should be used only for library development.
+ */
+#ifndef UAVCAN_DEBUG
+# define UAVCAN_DEBUG 0
+#endif
+
+/**
+ * This option allows to select whether libuavcan should throw exceptions on fatal errors, or try to handle
+ * errors differently. By default, exceptions will be enabled only if the library is built for a general-purpose
+ * OS like Linux. Set UAVCAN_EXCEPTIONS explicitly to override.
+ */
+#ifndef UAVCAN_EXCEPTIONS
+# define UAVCAN_EXCEPTIONS UAVCAN_GENERAL_PURPOSE_PLATFORM
+#endif
+
+/**
+ * This specification is used by some error reporting functions like in the Logger class.
+ * The default can be overriden by defining the macro UAVCAN_NOEXCEPT explicitly, e.g. via compiler options.
+ */
+#ifndef UAVCAN_NOEXCEPT
+# if UAVCAN_EXCEPTIONS
+# if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# define UAVCAN_NOEXCEPT noexcept
+# else
+# define UAVCAN_NOEXCEPT throw()
+# endif
+# else
+# define UAVCAN_NOEXCEPT
+# endif
+#endif
+
+/**
+ * Declaration visibility
+ * http://gcc.gnu.org/wiki/Visibility
+ */
+#ifndef UAVCAN_EXPORT
+# define UAVCAN_EXPORT
+#endif
+
+/**
+ * Trade-off between ROM/RAM usage and functionality/determinism.
+ * Note that this feature is not well tested and should be avoided.
+ * Use code search for UAVCAN_TINY to find what functionality will be disabled.
+ * This is particularly useful for embedded systems with less than 40kB of ROM.
+ */
+#ifndef UAVCAN_TINY
+# define UAVCAN_TINY 0
+#endif
+
+/**
+ * Disable the global data type registry, which can save some space on embedded systems.
+ */
+#ifndef UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY
+# define UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY 0
+#endif
+
+/**
+ * toString() methods will be disabled by default, unless the library is built for a general-purpose target like Linux.
+ * It is not recommended to enable toString() on embedded targets as code size will explode.
+ */
+#ifndef UAVCAN_TOSTRING
+# if UAVCAN_EXCEPTIONS
+# define UAVCAN_TOSTRING UAVCAN_GENERAL_PURPOSE_PLATFORM
+# else
+# define UAVCAN_TOSTRING 0
+# endif
+#endif
+
+#if UAVCAN_TOSTRING
+# if !UAVCAN_EXCEPTIONS
+# error UAVCAN_TOSTRING requires UAVCAN_EXCEPTIONS
+# endif
+# include <string>
+#endif
+
+/**
+ * Some C++ implementations are half-broken because they don't implement the placement new operator.
+ * If UAVCAN_IMPLEMENT_PLACEMENT_NEW is defined, libuavcan will implement its own operator new (std::size_t, void*)
+ * and its delete() counterpart, instead of relying on the standard header <new>.
+ */
+#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW
+# define UAVCAN_IMPLEMENT_PLACEMENT_NEW 0
+#endif
+
+/**
+ * Allows the user's application to provide custom implementation of uavcan::snprintf(),
+ * which is often useful on deeply embedded systems.
+ */
+#ifndef UAVCAN_USE_EXTERNAL_SNPRINTF
+# define UAVCAN_USE_EXTERNAL_SNPRINTF 0
+#endif
+
+/**
+ * Allows the user's application to provide a custom implementation of IEEE754Converter::nativeIeeeToHalf and
+ * IEEE754Converter::halfToNativeIeee.
+ */
+#ifndef UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
+# define UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 0
+#endif
+
+/**
+ * Run time checks.
+ * Resolves to the standard assert() by default.
+ * Disabled completely if UAVCAN_NO_ASSERTIONS is defined.
+ */
+#ifndef UAVCAN_ASSERT
+# ifndef UAVCAN_NO_ASSERTIONS
+# define UAVCAN_NO_ASSERTIONS 0
+# endif
+# if UAVCAN_NO_ASSERTIONS
+# define UAVCAN_ASSERT(x)
+# else
+# define UAVCAN_ASSERT(x) assert(x)
+# endif
+#endif
+
+#ifndef UAVCAN_LIKELY
+# if __GNUC__
+# define UAVCAN_LIKELY(x) __builtin_expect(!!(x), true)
+# else
+# define UAVCAN_LIKELY(x) (x)
+# endif
+#endif
+
+#ifndef UAVCAN_UNLIKELY
+# if __GNUC__
+# define UAVCAN_UNLIKELY(x) __builtin_expect(!!(x), false)
+# else
+# define UAVCAN_UNLIKELY(x) (x)
+# endif
+#endif
+
+namespace uavcan
+{
+/**
+ * Memory pool block size.
+ *
+ * The default of 64 bytes should be OK for any target arch up to AMD64 and any compiler.
+ *
+ * The library leverages compile-time checks to ensure that all types that are subject to dynamic allocation
+ * fit this size, otherwise compilation fails.
+ *
+ * For platforms featuring small pointer width (16..32 bits), UAVCAN_MEM_POOL_BLOCK_SIZE can often be safely
+ * reduced to 56 or even 48 bytes, which leads to lower memory footprint.
+ *
+ * Note that the pool block size shall be aligned at biggest alignment of the target platform (detected and
+ * checked automatically at compile time).
+ */
+#ifdef UAVCAN_MEM_POOL_BLOCK_SIZE
+/// Explicitly specified by the user.
+static const unsigned MemPoolBlockSize = UAVCAN_MEM_POOL_BLOCK_SIZE;
+#elif defined(__BIGGEST_ALIGNMENT__) && (__BIGGEST_ALIGNMENT__ <= 8)
+/// Convenient default for GCC-like compilers - if alignment allows, pool block size can be safely reduced.
+static const unsigned MemPoolBlockSize = 56;
+#else
+/// Safe default that should be OK for any platform.
+static const unsigned MemPoolBlockSize = 64;
+#endif
+
+#ifdef __BIGGEST_ALIGNMENT__
+static const unsigned MemPoolAlignment = __BIGGEST_ALIGNMENT__;
+#else
+static const unsigned MemPoolAlignment = 16;
+#endif
+
+typedef char _alignment_check_for_MEM_POOL_BLOCK_SIZE[((MemPoolBlockSize & (MemPoolAlignment - 1)) == 0) ? 1 : -1];
+
+/**
+ * This class that allows to check at compile time whether type T can be allocated using the memory pool.
+ * If the check fails, compilation fails.
+ */
+template <typename T>
+struct UAVCAN_EXPORT IsDynamicallyAllocatable
+{
+ static void check()
+ {
+ char dummy[(sizeof(T) <= MemPoolBlockSize) ? 1 : -1] = { '0' };
+ (void)dummy;
+ }
+};
+
+/**
+ * Float comparison precision.
+ * For details refer to:
+ * http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ * https://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h
+ */
+#ifdef UAVCAN_FLOAT_COMPARISON_EPSILON_MULT
+static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSILON_MULT;
+#else
+static const unsigned FloatComparisonEpsilonMult = 10;
+#endif
+
+/**
+ * Maximum number of CAN acceptance filters available on the platform
+ */
+#ifdef UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS
+/// Explicitly specified by the user.
+static const unsigned MaxCanAcceptanceFilters = UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS;
+#else
+/// Default that should be OK for any platform.
+static const unsigned MaxCanAcceptanceFilters = 32;
+#endif
+
+}
+
+#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/data_type.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_DATA_TYPE_HPP_INCLUDED
+#define UAVCAN_DATA_TYPE_HPP_INCLUDED
+
+#include <cassert>
+#include <cstring>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/transport/transfer.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT TransferCRC;
+
+enum DataTypeKind
+{
+ DataTypeKindService,
+ DataTypeKindMessage
+};
+
+static const uint8_t NumDataTypeKinds = 2;
+
+
+static inline DataTypeKind getDataTypeKindForTransferType(const TransferType tt)
+{
+ if (tt == TransferTypeServiceResponse ||
+ tt == TransferTypeServiceRequest)
+ {
+ return DataTypeKindService;
+ }
+ else if (tt == TransferTypeMessageBroadcast)
+ {
+ return DataTypeKindMessage;
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ return DataTypeKind(0);
+ }
+}
+
+
+class UAVCAN_EXPORT DataTypeID
+{
+ uint32_t value_;
+
+public:
+ static const uint16_t MaxServiceDataTypeIDValue = 255;
+ static const uint16_t MaxMessageDataTypeIDValue = 65535;
+ static const uint16_t MaxPossibleDataTypeIDValue = MaxMessageDataTypeIDValue;
+
+ DataTypeID() : value_(0xFFFFFFFFUL) { }
+
+ DataTypeID(uint16_t id) // Implicit
+ : value_(id)
+ { }
+
+ static DataTypeID getMaxValueForDataTypeKind(const DataTypeKind dtkind);
+
+ bool isValidForDataTypeKind(DataTypeKind dtkind) const
+ {
+ return value_ <= getMaxValueForDataTypeKind(dtkind).get();
+ }
+
+ uint16_t get() const { return static_cast<uint16_t>(value_); }
+
+ bool operator==(DataTypeID rhs) const { return value_ == rhs.value_; }
+ bool operator!=(DataTypeID rhs) const { return value_ != rhs.value_; }
+
+ bool operator<(DataTypeID rhs) const { return value_ < rhs.value_; }
+ bool operator>(DataTypeID rhs) const { return value_ > rhs.value_; }
+ bool operator<=(DataTypeID rhs) const { return value_ <= rhs.value_; }
+ bool operator>=(DataTypeID rhs) const { return value_ >= rhs.value_; }
+};
+
+
+/**
+ * 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 UAVCAN_EXPORT DataTypeSignatureCRC
+{
+ uint64_t crc_;
+
+public:
+ static DataTypeSignatureCRC extend(uint64_t crc);
+
+ DataTypeSignatureCRC() : crc_(0xFFFFFFFFFFFFFFFFULL) { }
+
+ void add(uint8_t byte);
+
+ void add(const uint8_t* bytes, unsigned len);
+
+ uint64_t get() const { return crc_ ^ 0xFFFFFFFFFFFFFFFFULL; }
+};
+
+
+class UAVCAN_EXPORT DataTypeSignature
+{
+ uint64_t value_;
+
+ void mixin64(uint64_t x);
+
+public:
+ DataTypeSignature() : value_(0) { }
+ explicit DataTypeSignature(uint64_t value) : value_(value) { }
+
+ void extend(DataTypeSignature dts);
+
+ TransferCRC toTransferCRC() const;
+
+ uint64_t get() const { return value_; }
+
+ bool operator==(DataTypeSignature rhs) const { return value_ == rhs.value_; }
+ bool operator!=(DataTypeSignature rhs) const { return !operator==(rhs); }
+};
+
+/**
+ * This class contains complete description of a data type.
+ */
+class UAVCAN_EXPORT DataTypeDescriptor
+{
+ DataTypeSignature signature_;
+ const char* full_name_;
+ DataTypeKind kind_;
+ DataTypeID id_;
+
+public:
+ static const unsigned MaxFullNameLen = 80;
+
+ DataTypeDescriptor() :
+ full_name_(""),
+ kind_(DataTypeKind(0))
+ { }
+
+ DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) :
+ signature_(signature),
+ full_name_(name),
+ kind_(kind),
+ id_(id)
+ {
+ UAVCAN_ASSERT((kind == DataTypeKindMessage) || (kind == DataTypeKindService));
+ UAVCAN_ASSERT(name);
+ UAVCAN_ASSERT(std::strlen(name) <= MaxFullNameLen);
+ }
+
+ bool isValid() const;
+
+ DataTypeKind getKind() const { return kind_; }
+ DataTypeID getID() const { return id_; }
+ const DataTypeSignature& getSignature() const { return signature_; }
+ const char* getFullName() const { return full_name_; }
+
+ bool match(DataTypeKind kind, const char* name) const;
+ bool match(DataTypeKind kind, DataTypeID id) const;
+
+ bool operator!=(const DataTypeDescriptor& rhs) const { return !operator==(rhs); }
+ bool operator==(const DataTypeDescriptor& rhs) const;
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+};
+
+}
+
+#endif // UAVCAN_DATA_TYPE_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/debug.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,35 @@
+/*
+ * Debug stuff, should only be used for library development.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_DEBUG_HPP_INCLUDED
+#define UAVCAN_DEBUG_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+
+#if UAVCAN_DEBUG
+
+# include <cstdio>
+# include <cstdarg>
+
+# if __GNUC__
+__attribute__ ((format(printf, 2, 3)))
+# endif
+static void UAVCAN_TRACE(const char* src, const char* fmt, ...)
+{
+ va_list args;
+ (void)std::printf("UAVCAN: %s: ", src);
+ va_start(args, fmt);
+ (void)std::vprintf(fmt, args);
+ va_end(args);
+ (void)std::puts("");
+}
+
+#else
+
+# define UAVCAN_TRACE(...) ((void)0)
+
+#endif
+
+#endif // UAVCAN_DEBUG_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/driver/can.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,249 @@
+/*
+ * CAN bus driver interface.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_DRIVER_CAN_HPP_INCLUDED
+#define UAVCAN_DRIVER_CAN_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/driver/system_clock.hpp>
+
+namespace uavcan
+{
+/**
+ * This limit is defined by the specification.
+ */
+enum { MaxCanIfaces = 3 };
+
+/**
+ * Raw CAN frame, as passed to/from the CAN driver.
+ */
+struct UAVCAN_EXPORT CanFrame
+{
+ static const uint32_t MaskStdID = 0x000007FFU;
+ static const uint32_t MaskExtID = 0x1FFFFFFFU;
+ static const uint32_t FlagEFF = 1U << 31; ///< Extended frame format
+ static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request
+ static const uint32_t FlagERR = 1U << 29; ///< Error frame
+
+ static const uint8_t MaxDataLen = 8;
+
+ uint32_t id; ///< CAN ID with flags (above)
+ uint8_t data[MaxDataLen];
+ uint8_t dlc; ///< Data Length Code
+
+ CanFrame() :
+ id(0),
+ dlc(0)
+ {
+ fill(data, data + MaxDataLen, uint8_t(0));
+ }
+
+ CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) :
+ id(can_id),
+ dlc((data_len > MaxDataLen) ? MaxDataLen : data_len)
+ {
+ UAVCAN_ASSERT(can_data != UAVCAN_NULLPTR);
+ UAVCAN_ASSERT(data_len == dlc);
+ (void)copy(can_data, can_data + dlc, this->data);
+ }
+
+ bool operator!=(const CanFrame& rhs) const { return !operator==(rhs); }
+ bool operator==(const CanFrame& rhs) const
+ {
+ return (id == rhs.id) && (dlc == rhs.dlc) && equal(data, data + dlc, rhs.data);
+ }
+
+ bool isExtended() const { return id & FlagEFF; }
+ bool isRemoteTransmissionRequest() const { return id & FlagRTR; }
+ bool isErrorFrame() const { return id & FlagERR; }
+
+#if UAVCAN_TOSTRING
+ enum StringRepresentation
+ {
+ StrTight, ///< Minimum string length (default)
+ StrAligned ///< Fixed formatting for any frame
+ };
+
+ std::string toString(StringRepresentation mode = StrTight) const;
+
+#endif
+
+ /**
+ * CAN frame arbitration rules, particularly STD vs EXT:
+ * Marco Di Natale - "Understanding and using the Controller Area Network"
+ * http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf
+ */
+ bool priorityHigherThan(const CanFrame& rhs) const;
+ bool priorityLowerThan(const CanFrame& rhs) const { return rhs.priorityHigherThan(*this); }
+};
+
+/**
+ * CAN hardware filter config struct.
+ * Flags from @ref CanFrame can be applied to define frame type (EFF, EXT, etc.).
+ * @ref ICanIface::configureFilters().
+ */
+struct UAVCAN_EXPORT CanFilterConfig
+{
+ uint32_t id;
+ uint32_t mask;
+
+ bool operator==(const CanFilterConfig& rhs) const
+ {
+ return rhs.id == id && rhs.mask == mask;
+ }
+
+ CanFilterConfig() :
+ id(0),
+ mask(0)
+ { }
+};
+
+/**
+ * Events to look for during @ref ICanDriver::select() call.
+ * Bit position defines iface index, e.g. read = 1 << 2 to read from the third iface.
+ */
+struct UAVCAN_EXPORT CanSelectMasks
+{
+ uint8_t read;
+ uint8_t write;
+
+ CanSelectMasks() :
+ read(0),
+ write(0)
+ { }
+};
+
+/**
+ * Special IO flags.
+ *
+ * @ref CanIOFlagLoopback - Send the frame back to RX with true TX timestamps.
+ *
+ * @ref CanIOFlagAbortOnError - Abort transmission on first bus error instead of retransmitting. This does not
+ * affect the case of arbitration loss, in which case the retransmission will work
+ * as usual. This flag is used together with anonymous messages which allows to
+ * implement CSMA bus access. Read the spec for details.
+ */
+typedef uint16_t CanIOFlags;
+static const CanIOFlags CanIOFlagLoopback = 1;
+static const CanIOFlags CanIOFlagAbortOnError = 2;
+
+/**
+ * Single non-blocking CAN interface.
+ */
+class UAVCAN_EXPORT ICanIface
+{
+public:
+ virtual ~ICanIface() { }
+
+ /**
+ * Non-blocking transmission.
+ *
+ * If the frame wasn't transmitted upon TX deadline, the driver should discard it.
+ *
+ * Note that it is LIKELY that the library will want to send the frames that were passed into the select()
+ * method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new
+ * frames between the calls.
+ *
+ * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
+ */
+ virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) = 0;
+
+ /**
+ * Non-blocking reception.
+ *
+ * Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller.
+ *
+ * Monotonic timestamp is required and can be not precise since it is needed only for
+ * protocol timing validation (transfer timeouts and inter-transfer intervals).
+ *
+ * UTC timestamp is optional, if available it will be used for precise time synchronization;
+ * must be set to zero if not available.
+ *
+ * Refer to @ref ISystemClock to learn more about timestamps.
+ *
+ * @param [out] out_ts_monotonic Monotonic timestamp, mandatory.
+ * @param [out] out_ts_utc UTC timestamp, optional, zero if unknown.
+ * @return 1 = one frame received, 0 = RX buffer empty, negative for error.
+ */
+ virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc,
+ CanIOFlags& out_flags) = 0;
+
+ /**
+ * Configure the hardware CAN filters. @ref CanFilterConfig.
+ *
+ * @return 0 = success, negative for error.
+ */
+ virtual int16_t configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs) = 0;
+
+ /**
+ * Number of available hardware filters.
+ */
+ virtual uint16_t getNumFilters() const = 0;
+
+ /**
+ * Continuously incrementing counter of hardware errors.
+ * Arbitration lost should not be treated as a hardware error.
+ */
+ virtual uint64_t getErrorCount() const = 0;
+};
+
+/**
+ * Generic CAN driver.
+ */
+class UAVCAN_EXPORT ICanDriver
+{
+public:
+ virtual ~ICanDriver() { }
+
+ /**
+ * Returns an interface by index, or null pointer if the index is out of range.
+ */
+ virtual ICanIface* getIface(uint8_t iface_index) = 0;
+
+ /**
+ * Default implementation of this method calls the non-const overload of getIface().
+ * Can be overriden by the application if necessary.
+ */
+ virtual const ICanIface* getIface(uint8_t iface_index) const
+ {
+ return const_cast<ICanDriver*>(this)->getIface(iface_index);
+ }
+
+ /**
+ * Total number of available CAN interfaces.
+ * This value shall not change after initialization.
+ */
+ virtual uint8_t getNumIfaces() const = 0;
+
+ /**
+ * Block until the deadline, or one of the specified interfaces becomes available for read or write.
+ *
+ * Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO.
+ *
+ * Bit position in the masks defines interface index.
+ *
+ * Note that it is allowed to return from this method even if no requested events actually happened, or if
+ * there are events that were not requested by the library.
+ *
+ * The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit
+ * next, per interface. This is intended to allow the driver to properly prioritize transmissions; many
+ * drivers will not need to use it. If a write flag for the given interface is set to one in the select mask
+ * structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR).
+ *
+ * @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO.
+ * @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next.
+ * @param [in] blocking_deadline Zero means non-blocking operation.
+ * @return Positive number of ready interfaces or negative error code.
+ */
+ virtual int16_t select(CanSelectMasks& inout_masks,
+ const CanFrame* (& pending_tx)[MaxCanIfaces],
+ MonotonicTime blocking_deadline) = 0;
+};
+
+}
+
+#endif // UAVCAN_DRIVER_CAN_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/driver/system_clock.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,60 @@
+/*
+ * System clock driver interface.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED
+#define UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED
+
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/time.hpp>
+
+namespace uavcan
+{
+
+/**
+ * System clock interface - monotonic and UTC.
+ */
+class UAVCAN_EXPORT ISystemClock
+{
+public:
+ virtual ~ISystemClock() { }
+
+ /**
+ * Monototic system clock.
+ *
+ * This clock shall never jump or change rate; the base time is irrelevant.
+ * This clock is mandatory and must remain functional at all times.
+ *
+ * On POSIX systems use clock_gettime() with CLOCK_MONOTONIC.
+ */
+ virtual MonotonicTime getMonotonic() const = 0;
+
+ /**
+ * Global network clock.
+ * It doesn't have to be UTC, the name is a bit misleading - actual time base doesn't matter.
+ *
+ * This clock can be synchronized with other nodes on the bus, hence it can jump and/or change
+ * rate occasionally.
+ * This clock is optional; if it is not supported, return zero. Also return zero if the UTC time
+ * is not available yet (e.g. the device has just started up with no battery clock).
+ *
+ * For POSIX refer to clock_gettime(), gettimeofday().
+ */
+ virtual UtcTime getUtc() const = 0;
+
+ /**
+ * Adjust the network-synchronized clock.
+ * Refer to @ref getUtc() for details.
+ *
+ * For POSIX refer to adjtime(), settimeofday().
+ *
+ * @param [in] adjustment Amount of time to add to the clock value.
+ */
+ virtual void adjustUtc(UtcDuration adjustment) = 0;
+};
+
+}
+
+#endif // UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/dynamic_memory.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED
+#define UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED
+
+#include <cassert>
+#include <cstdlib>
+#include <cstring>
+#include <uavcan/std.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/placement_new.hpp>
+#include <uavcan/build_config.hpp>
+
+namespace uavcan
+{
+/**
+ * This interface is used by other library components that need dynamic memory.
+ */
+class UAVCAN_EXPORT IPoolAllocator
+{
+public:
+ virtual ~IPoolAllocator() { }
+
+ virtual void* allocate(std::size_t size) = 0;
+ virtual void deallocate(const void* ptr) = 0;
+
+ /**
+ * Returns the maximum number of blocks this allocator can allocate.
+ */
+ virtual uint16_t getBlockCapacity() const = 0;
+};
+
+/**
+ * Classic implementation of a pool allocator (Meyers).
+ *
+ * The allocator can be made thread-safe (optional) by means of providing a RAII-lock type via the second template
+ * argument. The allocator uses the lock only to access the shared state, therefore critical sections are only a few
+ * cycles long, which implies that it should be acceptable to use hardware IRQ disabling instead of a mutex for
+ * performance reasons. For example, an IRQ-based RAII-lock type can be implemented as follows:
+ * struct RaiiSynchronizer
+ * {
+ * RaiiSynchronizer() { __disable_irq(); }
+ * ~RaiiSynchronizer() { __enable_irq(); }
+ * };
+ */
+template <std::size_t PoolSize,
+ uint8_t BlockSize,
+ typename RaiiSynchronizer = char>
+class UAVCAN_EXPORT PoolAllocator : public IPoolAllocator,
+ Noncopyable
+{
+ union Node
+ {
+ uint8_t data[BlockSize];
+ Node* next;
+ };
+
+ Node* free_list_;
+ union
+ {
+ uint8_t bytes[PoolSize];
+ long double _aligner1;
+ long long _aligner2;
+ Node _aligner3;
+ } pool_;
+
+ uint16_t used_;
+ uint16_t max_used_;
+
+public:
+ static const uint16_t NumBlocks = PoolSize / BlockSize;
+
+ PoolAllocator();
+
+ virtual void* allocate(std::size_t size);
+ virtual void deallocate(const void* ptr);
+
+ virtual uint16_t getBlockCapacity() const { return NumBlocks; }
+
+ /**
+ * Return the number of blocks that are currently allocated/unallocated.
+ */
+ uint16_t getNumUsedBlocks() const
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+ return used_;
+ }
+ uint16_t getNumFreeBlocks() const
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+ return static_cast<uint16_t>(NumBlocks - used_);
+ }
+
+ /**
+ * Returns the maximum number of blocks that were ever allocated at the same time.
+ */
+ uint16_t getPeakNumUsedBlocks() const
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+ return max_used_;
+ }
+};
+
+/**
+ * Limits the maximum number of blocks that can be allocated in a given allocator.
+ */
+class LimitedPoolAllocator : public IPoolAllocator
+{
+ IPoolAllocator& allocator_;
+ const uint16_t max_blocks_;
+ uint16_t used_blocks_;
+
+public:
+ LimitedPoolAllocator(IPoolAllocator& allocator, std::size_t max_blocks)
+ : allocator_(allocator)
+ , max_blocks_(static_cast<uint16_t>(min<std::size_t>(max_blocks, 0xFFFFU)))
+ , used_blocks_(0)
+ {
+ UAVCAN_ASSERT(max_blocks_ > 0);
+ }
+
+ virtual void* allocate(std::size_t size);
+ virtual void deallocate(const void* ptr);
+
+ virtual uint16_t getBlockCapacity() const;
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * PoolAllocator<>
+ */
+template <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
+const uint16_t PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::NumBlocks;
+
+template <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
+PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::PoolAllocator() :
+ free_list_(reinterpret_cast<Node*>(pool_.bytes)),
+ used_(0),
+ max_used_(0)
+{
+ // The limit is imposed by the width of the pool usage tracking variables.
+ StaticAssert<((PoolSize / BlockSize) <= 0xFFFFU)>::check();
+
+ (void)std::memset(pool_.bytes, 0, PoolSize);
+ for (unsigned i = 0; (i + 1) < (NumBlocks - 1 + 1); i++) // -Werror=type-limits
+ {
+ // coverity[dead_error_line : FALSE]
+ free_list_[i].next = free_list_ + i + 1;
+ }
+ free_list_[NumBlocks - 1].next = UAVCAN_NULLPTR;
+}
+
+template <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
+void* PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::allocate(std::size_t size)
+{
+ if (free_list_ == UAVCAN_NULLPTR || size > BlockSize)
+ {
+ return UAVCAN_NULLPTR;
+ }
+
+ RaiiSynchronizer lock;
+ (void)lock;
+
+ void* pmem = free_list_;
+ free_list_ = free_list_->next;
+
+ // Statistics
+ UAVCAN_ASSERT(used_ < NumBlocks);
+ used_++;
+ if (used_ > max_used_)
+ {
+ max_used_ = used_;
+ }
+
+ return pmem;
+}
+
+template <std::size_t PoolSize, uint8_t BlockSize, typename RaiiSynchronizer>
+void PoolAllocator<PoolSize, BlockSize, RaiiSynchronizer>::deallocate(const void* ptr)
+{
+ if (ptr == UAVCAN_NULLPTR)
+ {
+ return;
+ }
+
+ RaiiSynchronizer lock;
+ (void)lock;
+
+ Node* p = static_cast<Node*>(const_cast<void*>(ptr));
+ p->next = free_list_;
+ free_list_ = p;
+
+ // Statistics
+ UAVCAN_ASSERT(used_ > 0);
+ used_--;
+}
+
+}
+
+#endif // UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/error.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_ERROR_HPP_INCLUDED
+#define UAVCAN_ERROR_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/std.hpp>
+
+namespace uavcan
+{
+namespace
+{
+/**
+ * Common error codes.
+ *
+ * Functions that return signed integers may also return inverted error codes,
+ * i.e. returned value should be inverted back to get the actual error code.
+ *
+ * Return code 0 (zero) means no error.
+ *
+ * @{
+ */
+const int16_t ErrFailure = 1; ///< General failure
+const int16_t ErrInvalidParam = 2;
+const int16_t ErrMemory = 3;
+const int16_t ErrDriver = 4; ///< Platform driver error
+const int16_t ErrUnknownDataType = 5;
+const int16_t ErrInvalidMarshalData = 6;
+const int16_t ErrInvalidTransferListener = 7;
+const int16_t ErrNotInited = 8;
+const int16_t ErrRecursiveCall = 9;
+const int16_t ErrLogic = 10;
+const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode
+const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type
+const int16_t ErrInvalidConfiguration = 13;
+/**
+ * @}
+ */
+
+}
+
+/**
+ * Fatal error handler.
+ * Behavior:
+ * - If exceptions are enabled, throws std::runtime_error() with the supplied message text;
+ * - If assertions are enabled (see UAVCAN_ASSERT()), aborts execution using zero assertion.
+ * - Otherwise aborts execution via std::abort().
+ */
+#if __GNUC__
+__attribute__ ((noreturn))
+#endif
+UAVCAN_EXPORT
+// coverity[+kill]
+void handleFatalError(const char* msg);
+
+}
+
+#endif // UAVCAN_ERROR_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,220 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED
+#define UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED
+
+#include <cstdlib>
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/dynamic_memory.hpp>
+
+namespace uavcan
+{
+/**
+ * A special-purpose implementation of a pool allocator that keeps the pool in the heap using malloc()/free().
+ * The pool grows dynamically, ad-hoc, thus using as little memory as possible.
+ *
+ * Allocated blocks will not be freed back automatically, but there are two ways to force their deallocation:
+ * - Call @ref shrink() - this method frees all blocks that are unused at the moment.
+ * - Destroy the object - the desctructor calls @ref shrink().
+ *
+ * The pool can be limited in growth with hard and soft limits.
+ * The soft limit defines the value that will be reported via @ref IPoolAllocator::getBlockCapacity().
+ * The hard limit defines the maximum number of blocks that can be allocated from heap.
+ * Typically, the hard limit should be equal or greater than the soft limit.
+ *
+ * The allocator can be made thread-safe (optional) by means of providing a RAII-lock type via the second template
+ * argument. The allocator uses the lock only to access the shared state, therefore critical sections are only a few
+ * cycles long, which implies that it should be acceptable to use hardware IRQ disabling instead of a mutex for
+ * performance reasons. For example, an IRQ-based RAII-lock type can be implemented as follows:
+ * struct RaiiSynchronizer
+ * {
+ * RaiiSynchronizer() { __disable_irq(); }
+ * ~RaiiSynchronizer() { __enable_irq(); }
+ * };
+ */
+template <std::size_t BlockSize,
+ typename RaiiSynchronizer = char>
+class UAVCAN_EXPORT HeapBasedPoolAllocator : public IPoolAllocator,
+ Noncopyable
+{
+ union Node
+ {
+ Node* next;
+ private:
+ uint8_t data[BlockSize];
+ long double _aligner1;
+ long long _aligner2;
+ };
+
+ const uint16_t capacity_soft_limit_;
+ const uint16_t capacity_hard_limit_;
+
+ uint16_t num_reserved_blocks_;
+ uint16_t num_allocated_blocks_;
+
+ Node* reserve_;
+
+public:
+ /**
+ * The allocator initializes with empty reserve, so first allocations will be served from heap.
+ *
+ * @param block_capacity_soft_limit Block capacity that will be reported via @ref getBlockCapacity().
+ *
+ * @param block_capacity_hard_limit Real block capacity limit; the number of allocated blocks will never
+ * exceed this value. Hard limit should be higher than soft limit.
+ * Default value is two times the soft limit.
+ */
+ HeapBasedPoolAllocator(uint16_t block_capacity_soft_limit,
+ uint16_t block_capacity_hard_limit = 0) :
+ capacity_soft_limit_(block_capacity_soft_limit),
+ capacity_hard_limit_((block_capacity_hard_limit > 0) ? block_capacity_hard_limit :
+ static_cast<uint16_t>(min(static_cast<uint32_t>(block_capacity_soft_limit) * 2U,
+ static_cast<uint32_t>(NumericTraits<uint16_t>::max())))),
+ num_reserved_blocks_(0),
+ num_allocated_blocks_(0),
+ reserve_(UAVCAN_NULLPTR)
+ { }
+
+ /**
+ * The destructor de-allocates all blocks that are currently in the reserve.
+ * BLOCKS THAT ARE CURRENTLY HELD BY THE APPLICATION WILL LEAK.
+ */
+ ~HeapBasedPoolAllocator()
+ {
+ shrink();
+#if UAVCAN_DEBUG
+ if (num_allocated_blocks_ > 0)
+ {
+ UAVCAN_TRACE("HeapBasedPoolAllocator", "%u BLOCKS LEAKED", num_allocated_blocks_);
+ }
+#endif
+ }
+
+ /**
+ * Takes a block from the reserve, unless it's empty.
+ * In the latter case, allocates a new block in the heap.
+ */
+ virtual void* allocate(std::size_t size)
+ {
+ if (size > BlockSize)
+ {
+ return UAVCAN_NULLPTR;
+ }
+
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+
+ Node* const p = reserve_;
+
+ if (UAVCAN_LIKELY(p != UAVCAN_NULLPTR))
+ {
+ reserve_ = reserve_->next;
+ num_allocated_blocks_++;
+ return p;
+ }
+
+ if (num_reserved_blocks_ >= capacity_hard_limit_) // Hard limit reached, no further allocations
+ {
+ return UAVCAN_NULLPTR;
+ }
+ }
+
+ // Unlikely branch
+ void* const m = std::malloc(sizeof(Node));
+ if (m != UAVCAN_NULLPTR)
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+
+ num_reserved_blocks_++;
+ num_allocated_blocks_++;
+ }
+ return m;
+ }
+
+ /**
+ * Puts the block back to reserve.
+ * The block will not be free()d automatically; see @ref shrink().
+ */
+ virtual void deallocate(const void* ptr)
+ {
+ if (ptr != UAVCAN_NULLPTR)
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+
+ Node* const node = static_cast<Node*>(const_cast<void*>(ptr));
+ node->next = reserve_;
+ reserve_ = node;
+
+ num_allocated_blocks_--;
+ }
+ }
+
+ /**
+ * The soft limit.
+ */
+ virtual uint16_t getBlockCapacity() const { return capacity_soft_limit_; }
+
+ /**
+ * The hard limit.
+ */
+ uint16_t getBlockCapacityHardLimit() const { return capacity_hard_limit_; }
+
+ /**
+ * Frees all blocks that are not in use at the moment.
+ * Post-condition is getNumAllocatedBlocks() == getNumReservedBlocks().
+ */
+ void shrink()
+ {
+ Node* p = UAVCAN_NULLPTR;
+ for (;;)
+ {
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+ // Removing from reserve and updating the counter.
+ p = reserve_;
+ if (p != UAVCAN_NULLPTR)
+ {
+ reserve_ = reserve_->next;
+ num_reserved_blocks_--;
+ }
+ else
+ {
+ break;
+ }
+ }
+ // Then freeing, having left the critical section.
+ std::free(p);
+ }
+ }
+
+ /**
+ * Number of blocks that are currently in use by the application.
+ */
+ uint16_t getNumAllocatedBlocks() const
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+ return num_allocated_blocks_;
+ }
+
+ /**
+ * Number of blocks that are acquired from the heap.
+ */
+ uint16_t getNumReservedBlocks() const
+ {
+ RaiiSynchronizer lock;
+ (void)lock;
+ return num_reserved_blocks_;
+ }
+};
+
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/helpers/ostream.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED
+#define UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED
+
+#include <uavcan/util/templates.hpp>
+#include <cstdio>
+
+namespace uavcan
+{
+/**
+ * Compact replacement for std::ostream for use on embedded systems.
+ * Can be used for printing UAVCAN messages to stdout.
+ *
+ * Relevant discussion: https://groups.google.com/forum/#!topic/px4users/6c1CLNutN90
+ *
+ * Usage:
+ * OStream::instance() << "Hello world!" << OStream::endl;
+ */
+class UAVCAN_EXPORT OStream : uavcan::Noncopyable
+{
+ OStream() { }
+
+public:
+ static OStream& instance()
+ {
+ static OStream s;
+ return s;
+ }
+
+ static OStream& endl(OStream& stream)
+ {
+ std::printf("\n");
+ return stream;
+ }
+};
+
+inline OStream& operator<<(OStream& s, long long x) { std::printf("%lld", x); return s; }
+inline OStream& operator<<(OStream& s, unsigned long long x) { std::printf("%llu", x); return s; }
+
+inline OStream& operator<<(OStream& s, long x) { std::printf("%ld", x); return s; }
+inline OStream& operator<<(OStream& s, unsigned long x) { std::printf("%lu", x); return s; }
+
+inline OStream& operator<<(OStream& s, int x) { std::printf("%d", x); return s; }
+inline OStream& operator<<(OStream& s, unsigned int x) { std::printf("%u", x); return s; }
+
+inline OStream& operator<<(OStream& s, short x) { return operator<<(s, static_cast<int>(x)); }
+inline OStream& operator<<(OStream& s, unsigned short x) { return operator<<(s, static_cast<unsigned>(x)); }
+
+inline OStream& operator<<(OStream& s, long double x) { std::printf("%Lg", x); return s; }
+inline OStream& operator<<(OStream& s, double x) { std::printf("%g", x); return s; }
+inline OStream& operator<<(OStream& s, float x) { return operator<<(s, static_cast<double>(x)); }
+
+inline OStream& operator<<(OStream& s, char x) { std::printf("%c", x); return s; }
+inline OStream& operator<<(OStream& s, const char* x) { std::printf("%s", x); return s; }
+
+inline OStream& operator<<(OStream& s, OStream&(*manip)(OStream&)) { return manip(s); }
+
+}
+
+#endif // UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/array.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,1270 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED
+#define UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED
+
+#include <cassert>
+#include <cstdio>
+#include <cstring>
+#include <cmath>
+#include <uavcan/error.hpp>
+#include <uavcan/util/bitset.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/marshal/type_util.hpp>
+#include <uavcan/marshal/integer_spec.hpp>
+#include <uavcan/std.hpp>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+
+#ifndef UAVCAN_EXCEPTIONS
+# error UAVCAN_EXCEPTIONS
+#endif
+
+#if UAVCAN_EXCEPTIONS
+# include <stdexcept>
+#endif
+
+namespace uavcan
+{
+
+enum ArrayMode { ArrayModeStatic, ArrayModeDynamic };
+
+/**
+ * Properties of a square matrix; assuming row-major representation.
+ */
+template <unsigned NumElements_>
+struct SquareMatrixTraits
+{
+ enum { NumElements = NumElements_ };
+
+ enum { NumRowsCols = CompileTimeIntSqrt<NumElements>::Result };
+
+ enum { NumElementsInTriangle = ((1 + NumRowsCols) * NumRowsCols) / 2 };
+
+ static inline bool isIndexOnDiagonal(unsigned index) { return (index / NumRowsCols) == (index % NumRowsCols); }
+
+ static inline int computeElementIndexAtRowCol(int row, int col) { return row * NumRowsCols + col; }
+};
+
+/**
+ * This class can be used to detect properties of square matrices.
+ * Element iterator is a random access forward constant iterator.
+ */
+template <typename ElementIterator, unsigned NumElements>
+class SquareMatrixAnalyzer : public SquareMatrixTraits<NumElements>
+{
+ typedef SquareMatrixTraits<NumElements> Traits;
+
+ const ElementIterator first_;
+
+public:
+ enum PackingMode
+ {
+ PackingModeEmpty,
+ PackingModeScalar,
+ PackingModeDiagonal,
+ PackingModeSymmetric,
+ PackingModeFull
+ };
+
+ SquareMatrixAnalyzer(ElementIterator first_element_iterator)
+ : first_(first_element_iterator)
+ {
+ StaticAssert<(NumElements > 0)>::check();
+ }
+
+ ElementIterator accessElementAtRowCol(int row, int col) const
+ {
+ return first_ + Traits::computeElementIndexAtRowCol(row, col);
+ }
+
+ bool areAllElementsNan() const
+ {
+ unsigned index = 0;
+ for (ElementIterator it = first_; index < NumElements; ++it, ++index)
+ {
+ if (!isNaN(*it))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool isScalar() const
+ {
+ unsigned index = 0;
+ for (ElementIterator it = first_; index < NumElements; ++it, ++index)
+ {
+ if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it))
+ {
+ return false;
+ }
+ if (Traits::isIndexOnDiagonal(index) && !areClose(*it, *first_))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool isDiagonal() const
+ {
+ unsigned index = 0;
+ for (ElementIterator it = first_; index < NumElements; ++it, ++index)
+ {
+ if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool isSymmetric() const
+ {
+ for (int i = 0; i < Traits::NumRowsCols; ++i)
+ {
+ for (int k = 0; k < Traits::NumRowsCols; ++k)
+ {
+ // On diagonal comparison is pointless
+ if ((i != k) &&
+ !areClose(*accessElementAtRowCol(i, k),
+ *accessElementAtRowCol(k, i)))
+ {
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ PackingMode detectOptimalPackingMode() const
+ {
+ if (areAllElementsNan())
+ {
+ return PackingModeEmpty;
+ }
+ if (isScalar())
+ {
+ return PackingModeScalar;
+ }
+ if (isDiagonal())
+ {
+ return PackingModeDiagonal;
+ }
+ if (isSymmetric())
+ {
+ return PackingModeSymmetric;
+ }
+ return PackingModeFull;
+ }
+};
+
+
+template <unsigned Size>
+class UAVCAN_EXPORT StaticArrayBase
+{
+protected:
+ typedef IntegerSpec<IntegerBitLen<Size>::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType;
+
+public:
+ enum { SizeBitLen = 0 };
+
+ typedef typename StorageType<IntegerSpec<IntegerBitLen<EnumMax<Size, 2>::Result>::Result,
+ SignednessUnsigned, CastModeSaturate> >::Type SizeType;
+
+ SizeType size() const { return SizeType(Size); }
+ SizeType capacity() const { return SizeType(Size); }
+
+protected:
+ StaticArrayBase() { }
+ ~StaticArrayBase() { }
+
+ SizeType validateRange(SizeType pos) const
+ {
+ if (pos < SizeType(Size))
+ {
+ return pos;
+ }
+#if UAVCAN_EXCEPTIONS
+ throw std::out_of_range("uavcan::Array");
+#else
+ UAVCAN_ASSERT(0);
+ return SizeType(Size - 1U); // Ha ha
+#endif
+ }
+};
+
+
+template <unsigned MaxSize>
+class UAVCAN_EXPORT DynamicArrayBase
+{
+protected:
+ typedef IntegerSpec<IntegerBitLen<MaxSize>::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType;
+public:
+ typedef typename StorageType<IntegerSpec<IntegerBitLen<EnumMax<MaxSize, 2>::Result>::Result,
+ SignednessUnsigned, CastModeSaturate> >::Type SizeType;
+
+private:
+ SizeType size_;
+
+protected:
+ DynamicArrayBase() : size_(0) { }
+ ~DynamicArrayBase() { }
+
+ SizeType validateRange(SizeType pos) const
+ {
+ if (pos < size_)
+ {
+ return pos;
+ }
+#if UAVCAN_EXCEPTIONS
+ throw std::out_of_range("uavcan::Array");
+#else
+ UAVCAN_ASSERT(0);
+ return SizeType((size_ == 0U) ? 0U : (size_ - 1U));
+#endif
+ }
+
+ void grow()
+ {
+ if (size_ >= MaxSize)
+ {
+ (void)validateRange(MaxSize); // Will throw, UAVCAN_ASSERT() or do nothing
+ }
+ else
+ {
+ size_++;
+ }
+ }
+
+ void shrink()
+ {
+ if (size_ > 0)
+ {
+ size_--;
+ }
+ }
+
+public:
+ enum { SizeBitLen = RawEncodedSizeType::BitLen };
+
+ SizeType size() const
+ {
+ UAVCAN_ASSERT(size_ ? ((size_ - 1u) <= (MaxSize - 1u)) : 1); // -Werror=type-limits
+ return size_;
+ }
+
+ SizeType capacity() const { return MaxSize; }
+
+ void clear() { size_ = 0; }
+};
+
+/**
+ * Common functionality for both static and dynamic arrays.
+ * Static arrays are of fixed size; methods that can alter the size (e.g. push_back() and such) will fail to compile.
+ * Dynamic arrays contain a fixed-size buffer (it's size is enough to fit maximum number of elements) plus the
+ * currently allocated number of elements.
+ */
+template <typename T, ArrayMode ArrayMode, unsigned MaxSize>
+class UAVCAN_EXPORT ArrayImpl : public Select<ArrayMode == ArrayModeDynamic,
+ DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result
+{
+ typedef ArrayImpl<T, ArrayMode, MaxSize> SelfType;
+ typedef typename Select<ArrayMode == ArrayModeDynamic,
+ DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result Base;
+
+public:
+ enum
+ {
+ /// True if the array contents can be interpreted as a 8-bit string (ASCII or UTF8).
+ IsStringLike = IsIntegerSpec<T>::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) &&
+ (ArrayMode == ArrayModeDynamic)
+ };
+
+private:
+ typedef typename StorageType<T>::Type BufferType[MaxSize + (IsStringLike ? 1 : 0)];
+ BufferType data_;
+
+ template <typename U>
+ typename EnableIf<sizeof(U(0) >= U())>::Type initialize(int)
+ {
+ if (ArrayMode != ArrayModeDynamic)
+ {
+ ::uavcan::fill(data_, data_ + MaxSize, U());
+ }
+ }
+ template <typename> void initialize(...) { }
+
+protected:
+ ~ArrayImpl() { }
+
+public:
+ typedef typename StorageType<T>::Type ValueType;
+ typedef typename Base::SizeType SizeType;
+
+ using Base::size;
+ using Base::capacity;
+
+ ArrayImpl() { initialize<ValueType>(0); }
+
+ /**
+ * Returns zero-terminated string, same as std::string::c_str().
+ * This method will compile only if the array can be interpreted as 8-bit string (ASCII of UTF8).
+ */
+ const char* c_str() const
+ {
+ StaticAssert<IsStringLike>::check();
+ UAVCAN_ASSERT(size() < (MaxSize + 1));
+ const_cast<BufferType&>(data_)[size()] = 0; // Ad-hoc string termination
+ return reinterpret_cast<const char*>(data_);
+ }
+
+ /**
+ * Range-checking subscript.
+ * If the index is out of range:
+ * - if exceptions are enabled, std::out_of_range will be thrown.
+ * - if exceptions are disabled and UAVCAN_ASSERT() is enabled, execution will be aborted.
+ * - if exceptions are disabled and UAVCAN_ASSERT() is disabled, index will be constrained to
+ * the closest valid value.
+ */
+ ValueType& at(SizeType pos) { return data_[Base::validateRange(pos)]; }
+ const ValueType& at(SizeType pos) const { return data_[Base::validateRange(pos)]; }
+
+ /**
+ * Range-checking subscript. @ref at()
+ */
+ ValueType& operator[](SizeType pos) { return at(pos); }
+ const ValueType& operator[](SizeType pos) const { return at(pos); }
+
+ /**
+ * Standard container methods. Applicable to both dynamic and static arrays.
+ */
+ ValueType* begin() { return data_; }
+ const ValueType* begin() const { return data_; }
+ ValueType* end() { return data_ + Base::size(); }
+ const ValueType* end() const { return data_ + Base::size(); }
+ ValueType& front() { return at(0U); }
+ const ValueType& front() const { return at(0U); }
+ ValueType& back() { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); }
+ const ValueType& back() const { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); }
+
+ /**
+ * Performs standard lexicographical compare of the elements.
+ */
+ template <typename R>
+ bool operator<(const R& rhs) const
+ {
+ return ::uavcan::lexicographical_compare(begin(), end(), rhs.begin(), rhs.end());
+ }
+
+ /**
+ * Aliases for compatibility with standard containers.
+ */
+ typedef ValueType* iterator;
+ typedef const ValueType* const_iterator;
+};
+
+/**
+ * Memory-efficient specialization for bit arrays (each element maps to a single bit rather than single byte).
+ * This should be compatible with std::bitset.
+ */
+template <unsigned MaxSize, ArrayMode ArrayMode, CastMode CastMode>
+class UAVCAN_EXPORT ArrayImpl<IntegerSpec<1, SignednessUnsigned, CastMode>, ArrayMode, MaxSize>
+ : public BitSet<MaxSize>
+ , public Select<ArrayMode == ArrayModeDynamic, DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result
+{
+ typedef typename Select<ArrayMode == ArrayModeDynamic,
+ DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result ArrayBase;
+
+public:
+ enum { IsStringLike = 0 };
+
+ typedef typename BitSet<MaxSize>::Reference Reference;
+ typedef typename ArrayBase::SizeType SizeType;
+
+ using ArrayBase::size;
+ using ArrayBase::capacity;
+
+ /**
+ * Range-checking subscript. Throws if enabled; UAVCAN_ASSERT() if enabled; else constraints the position.
+ */
+ Reference at(SizeType pos) { return BitSet<MaxSize>::operator[](ArrayBase::validateRange(pos)); }
+ bool at(SizeType pos) const { return BitSet<MaxSize>::operator[](ArrayBase::validateRange(pos)); }
+
+ /**
+ * @ref at()
+ */
+ Reference operator[](SizeType pos) { return at(pos); }
+ bool operator[](SizeType pos) const { return at(pos); }
+};
+
+/**
+ * Zero length arrays are not allowed
+ */
+template <typename T, ArrayMode ArrayMode> class ArrayImpl<T, ArrayMode, 0>;
+
+/**
+ * Generic array implementation.
+ * This class is compatible with most standard library functions operating on containers (e.g. std::sort(),
+ * std::lexicographical_compare(), etc.).
+ * No dynamic memory is used.
+ * All functions that can modify the array or access elements are range checking. If the range error occurs:
+ * - if exceptions are enabled, std::out_of_range will be thrown;
+ * - if UAVCAN_ASSERT() is enabled, program will be terminated on UAVCAN_ASSERT(0);
+ * - otherwise the index value will be constrained to the closest valid value.
+ */
+template <typename T, ArrayMode ArrayMode, unsigned MaxSize_>
+class UAVCAN_EXPORT Array : public ArrayImpl<T, ArrayMode, MaxSize_>
+{
+ typedef ArrayImpl<T, ArrayMode, MaxSize_> Base;
+ typedef Array<T, ArrayMode, MaxSize_> SelfType;
+
+ static bool isOptimizedTailArray(TailArrayOptimizationMode tao_mode)
+ {
+ return (T::MinBitLen >= 8) && (tao_mode == TailArrayOptEnabled);
+ }
+
+ int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) const /// Static
+ {
+ UAVCAN_ASSERT(size() > 0);
+ for (SizeType i = 0; i < size(); i++)
+ {
+ const bool last_item = i == (size() - 1);
+ const int res = RawValueType::encode(Base::at(i), codec, last_item ? tao_mode : TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ }
+ return 1;
+ }
+
+ int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) const /// Dynamic
+ {
+ StaticAssert<IsDynamic>::check();
+ const bool self_tao_enabled = isOptimizedTailArray(tao_mode);
+ if (!self_tao_enabled)
+ {
+ const int res_sz =
+ Base::RawEncodedSizeType::encode(typename StorageType<typename Base::RawEncodedSizeType>::Type(size()),
+ codec, TailArrayOptDisabled);
+ if (res_sz <= 0)
+ {
+ return res_sz;
+ }
+ }
+ if (size() == 0)
+ {
+ return 1;
+ }
+ return encodeImpl(codec, self_tao_enabled ? TailArrayOptDisabled : tao_mode, FalseType());
+ }
+
+ int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) /// Static
+ {
+ UAVCAN_ASSERT(size() > 0);
+ for (SizeType i = 0; i < size(); i++)
+ {
+ const bool last_item = i == (size() - 1);
+ ValueType value = ValueType(); // TODO: avoid extra copy
+ const int res = RawValueType::decode(value, codec, last_item ? tao_mode : TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ Base::at(i) = value;
+ }
+ return 1;
+ }
+
+#if __GNUC__
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wtype-limits"
+#endif
+ int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) /// Dynamic
+ {
+ StaticAssert<IsDynamic>::check();
+ Base::clear();
+ if (isOptimizedTailArray(tao_mode))
+ {
+ while (true)
+ {
+ ValueType value = ValueType();
+ const int res = RawValueType::decode(value, codec, TailArrayOptDisabled);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (res == 0) // Success: End of stream reached (even if zero items were read)
+ {
+ return 1;
+ }
+ if (size() == MaxSize_) // Error: Max array length reached, but the end of stream is not
+ {
+ return -ErrInvalidMarshalData;
+ }
+ push_back(value);
+ }
+ }
+ else
+ {
+ typename StorageType<typename Base::RawEncodedSizeType>::Type sz = 0;
+ const int res_sz = Base::RawEncodedSizeType::decode(sz, codec, TailArrayOptDisabled);
+ if (res_sz <= 0)
+ {
+ return res_sz;
+ }
+ // coverity[result_independent_of_operands]
+ if (static_cast<unsigned>(sz) > MaxSize_) // False 'type-limits' warning occurs here
+ {
+ return -ErrInvalidMarshalData;
+ }
+ resize(sz);
+ if (sz == 0)
+ {
+ return 1;
+ }
+ return decodeImpl(codec, tao_mode, FalseType());
+ }
+ UAVCAN_ASSERT(0); // Unreachable
+ return -ErrLogic;
+ }
+#if __GNUC__
+# pragma GCC diagnostic pop
+#endif
+
+ template <typename InputIter>
+ void packSquareMatrixImpl(const InputIter src_row_major)
+ {
+ StaticAssert<IsDynamic>::check();
+
+ this->clear();
+
+ typedef SquareMatrixAnalyzer<InputIter, MaxSize> Analyzer;
+ const Analyzer analyzer(src_row_major);
+
+ switch (analyzer.detectOptimalPackingMode())
+ {
+ case Analyzer::PackingModeEmpty:
+ {
+ break; // Nothing to insert
+ }
+ case Analyzer::PackingModeScalar:
+ {
+ this->push_back(ValueType(*src_row_major));
+ break;
+ }
+ case Analyzer::PackingModeDiagonal:
+ {
+ for (int i = 0; i < Analyzer::NumRowsCols; i++)
+ {
+ this->push_back(ValueType(*analyzer.accessElementAtRowCol(i, i)));
+ }
+ break;
+ }
+ case Analyzer::PackingModeSymmetric:
+ {
+ for (int row = 0; row < Analyzer::NumRowsCols; row++)
+ {
+ for (int col = row; col < Analyzer::NumRowsCols; col++)
+ {
+ this->push_back(ValueType(*analyzer.accessElementAtRowCol(row, col)));
+ }
+ }
+ UAVCAN_ASSERT(this->size() == Analyzer::NumElementsInTriangle);
+ break;
+ }
+ case Analyzer::PackingModeFull:
+ {
+ InputIter it = src_row_major;
+ for (unsigned index = 0; index < MaxSize; index++, it++)
+ {
+ this->push_back(ValueType(*it));
+ }
+ break;
+ }
+ default:
+ {
+ UAVCAN_ASSERT(0);
+ break;
+ }
+ }
+ }
+
+ template <typename ScalarType, typename OutputIter>
+ void unpackSquareMatrixImpl(const OutputIter dst_row_major) const
+ {
+ StaticAssert<IsDynamic>::check();
+ typedef SquareMatrixTraits<MaxSize> Traits;
+
+ if (this->size() == Traits::NumRowsCols || this->size() == 1) // Scalar or diagonal
+ {
+ OutputIter it = dst_row_major;
+ for (unsigned index = 0; index < MaxSize; index++)
+ {
+ if (Traits::isIndexOnDiagonal(index))
+ {
+ const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Traits::NumRowsCols));
+ *it++ = ScalarType(this->at(source_index));
+ }
+ else
+ {
+ *it++ = ScalarType(0);
+ }
+ }
+ }
+ else if (this->size() == Traits::NumElementsInTriangle) // Symmetric
+ {
+ OutputIter it = dst_row_major;
+ SizeType source_index = 0;
+ for (int row = 0; row < Traits::NumRowsCols; row++)
+ {
+ for (int col = 0; col < Traits::NumRowsCols; col++)
+ {
+ if (col >= row) // Diagonal or upper-right triangle
+ {
+ *it++ = ScalarType(this->at(source_index));
+ source_index++;
+ }
+ else // Lower-left triangle
+ {
+ // Transposing one element, argument swapping is intentional
+ // coverity[swapped_arguments]
+ *it++ = *(dst_row_major + Traits::computeElementIndexAtRowCol(col, row));
+ }
+ }
+ }
+ UAVCAN_ASSERT(source_index == Traits::NumElementsInTriangle);
+ }
+ else if (this->size() == MaxSize) // Full - no packing whatsoever
+ {
+ OutputIter it = dst_row_major;
+ for (SizeType index = 0; index < MaxSize; index++)
+ {
+ *it++ = ScalarType(this->at(index));
+ }
+ }
+ else // Everything else
+ {
+ // coverity[suspicious_sizeof : FALSE]
+ ::uavcan::fill_n(dst_row_major, MaxSize, ScalarType(0));
+ }
+ }
+
+public:
+ typedef T RawValueType; ///< This may be not the same as the element type.
+ typedef typename StorageType<T>::Type ValueType; ///< This is the actual stored element type.
+ typedef typename Base::SizeType SizeType; ///< Minimal width size type.
+
+ using Base::size;
+ using Base::capacity;
+
+ enum { IsDynamic = ArrayMode == ArrayModeDynamic };
+ enum { MaxSize = MaxSize_ };
+ enum
+ {
+ MinBitLen = (IsDynamic == 0)
+ ? (static_cast<unsigned>(RawValueType::MinBitLen) * static_cast<unsigned>(MaxSize))
+ : 0
+ };
+ enum
+ {
+ MaxBitLen = static_cast<unsigned>(Base::SizeBitLen) +
+ static_cast<unsigned>(RawValueType::MaxBitLen) * static_cast<unsigned>(MaxSize)
+ };
+
+ /**
+ * Default constructor zero-initializes the storage even if it consists of primitive types.
+ */
+ Array() { }
+
+ /**
+ * String constructor - only for string-like arrays.
+ * Refer to @ref operator+=(const char*) for details.
+ */
+ Array(const char* str) // Implicit
+ {
+ operator+=(str);
+ }
+
+ static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
+ {
+ return array.encodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
+ }
+
+ static int decode(SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
+ {
+ return array.decodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
+ }
+
+ static void extendDataTypeSignature(DataTypeSignature& signature)
+ {
+ RawValueType::extendDataTypeSignature(signature);
+ }
+
+ bool empty() const { return size() == 0; }
+
+ /**
+ * Only for dynamic arrays. Range checking.
+ */
+ void pop_back() { Base::shrink(); }
+ void push_back(const ValueType& value)
+ {
+ Base::grow();
+ Base::at(SizeType(size() - 1)) = value;
+ }
+
+ /**
+ * Only for dynamic arrays. Range checking.
+ */
+ void resize(SizeType new_size, const ValueType& filler)
+ {
+ if (new_size > size())
+ {
+ SizeType cnt = SizeType(new_size - size());
+ while (cnt-- > 0)
+ {
+ push_back(filler);
+ }
+ }
+ else if (new_size < size())
+ {
+ SizeType cnt = SizeType(size() - new_size);
+ while (cnt-- > 0)
+ {
+ pop_back();
+ }
+ }
+ else
+ {
+ ; // Exact size
+ }
+ }
+
+ /**
+ * Only for dynamic arrays. Range checking.
+ */
+ void resize(SizeType new_size)
+ {
+ resize(new_size, ValueType());
+ }
+
+ /**
+ * This operator accepts any container with size() and [].
+ * Members must be comparable via operator ==.
+ */
+ template <typename R>
+ typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->size()) &&
+ sizeof((*(reinterpret_cast<const R*>(0)))[0]), bool>::Type
+ operator==(const R& rhs) const
+ {
+ if (size() != rhs.size())
+ {
+ return false;
+ }
+ for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators
+ {
+ if (!(Base::at(i) == rhs[i]))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ /**
+ * This method compares two arrays using @ref areClose(), which ensures proper comparison of
+ * floating point values, or DSDL data structures which contain floating point fields at any depth.
+ * Please refer to the documentation of @ref areClose() to learn more about how it works and how to
+ * define custom fuzzy comparison behavior.
+ * Any container with size() and [] is acceptable.
+ */
+ template <typename R>
+ typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->size()) &&
+ sizeof((*(reinterpret_cast<const R*>(0)))[0]), bool>::Type
+ isClose(const R& rhs) const
+ {
+ if (size() != rhs.size())
+ {
+ return false;
+ }
+ for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators
+ {
+ if (!areClose(Base::at(i), rhs[i]))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ /**
+ * This operator can only be used with string-like arrays; otherwise it will fail to compile.
+ * @ref c_str()
+ */
+ bool operator==(const char* chr) const
+ {
+ if (chr == UAVCAN_NULLPTR)
+ {
+ return false;
+ }
+ return std::strncmp(Base::c_str(), chr, MaxSize) == 0;
+ }
+
+ /**
+ * @ref operator==()
+ */
+ template <typename R> bool operator!=(const R& rhs) const { return !operator==(rhs); }
+
+ /**
+ * This operator can only be used with string-like arrays; otherwise it will fail to compile.
+ * @ref c_str()
+ */
+ SelfType& operator=(const char* chr)
+ {
+ StaticAssert<Base::IsStringLike>::check();
+ StaticAssert<IsDynamic>::check();
+ Base::clear();
+ if (chr == UAVCAN_NULLPTR)
+ {
+ handleFatalError("Array::operator=(const char*)");
+ }
+ while (*chr)
+ {
+ push_back(ValueType(*chr++)); // Value type is likely to be unsigned char, so conversion may be required.
+ }
+ return *this;
+ }
+
+ /**
+ * This operator can only be used with string-like arrays; otherwise it will fail to compile.
+ * @ref c_str()
+ */
+ SelfType& operator+=(const char* chr)
+ {
+ StaticAssert<Base::IsStringLike>::check();
+ StaticAssert<IsDynamic>::check();
+ if (chr == UAVCAN_NULLPTR)
+ {
+ handleFatalError("Array::operator+=(const char*)");
+ }
+ while (*chr)
+ {
+ push_back(ValueType(*chr++));
+ }
+ return *this;
+ }
+
+ /**
+ * Appends another Array<> with the same element type. Mode and max size can be different.
+ */
+ template <uavcan::ArrayMode RhsArrayMode, unsigned RhsMaxSize>
+ SelfType& operator+=(const Array<T, RhsArrayMode, RhsMaxSize>& rhs)
+ {
+ typedef Array<T, RhsArrayMode, RhsMaxSize> Rhs;
+ StaticAssert<IsDynamic>::check();
+ for (typename Rhs::SizeType i = 0; i < rhs.size(); i++)
+ {
+ push_back(rhs[i]);
+ }
+ return *this;
+ }
+
+ /**
+ * Formatting appender.
+ * This method doesn't raise an overflow error; instead it silently truncates the data to fit the array capacity.
+ * Works only with string-like arrays, otherwise fails to compile.
+ * @param format Format string for std::snprintf(), e.g. "%08x", "%f"
+ * @param value Arbitrary value of a primitive type (should fail to compile if there's a non-primitive type)
+ */
+ template <typename A>
+ void appendFormatted(const char* const format, const A value)
+ {
+ StaticAssert<Base::IsStringLike>::check();
+ StaticAssert<IsDynamic>::check();
+
+ StaticAssert<sizeof(A() >= A(0))>::check(); // This check allows to weed out most compound types
+ StaticAssert<(sizeof(A) <= sizeof(long double)) ||
+ (sizeof(A) <= sizeof(long long))>::check(); // Another stupid check to catch non-primitive types
+
+ if (!format)
+ {
+ UAVCAN_ASSERT(0);
+ return;
+ }
+ // Add some hardcore runtime checks for the format string correctness?
+
+ ValueType* const ptr = Base::end();
+ UAVCAN_ASSERT(capacity() >= size());
+ const SizeType max_size = SizeType(capacity() - size());
+
+ // We have one extra byte for the null terminator, hence +1
+ const int ret = snprintf(reinterpret_cast<char*>(ptr), SizeType(max_size + 1U), format, value);
+
+ for (int i = 0; i < min(ret, int(max_size)); i++)
+ {
+ Base::grow();
+ }
+ if (ret < 0)
+ {
+ UAVCAN_ASSERT(0); // Likely an invalid format string
+ (*this) += format; // So we print it as is in release builds
+ }
+ }
+
+ /**
+ * Converts the string to upper/lower case in place, assuming that encoding is ASCII.
+ * These methods can only be used with string-like arrays; otherwise compilation will fail.
+ */
+ void convertToUpperCaseASCII()
+ {
+ StaticAssert<Base::IsStringLike>::check();
+
+ for (SizeType i = 0; i < size(); i++)
+ {
+ const int x = Base::at(i);
+ if ((x <= 'z') && (x >= 'a'))
+ {
+ Base::at(i) = static_cast<ValueType>(x + ('Z' - 'z'));
+ }
+ }
+ }
+
+ void convertToLowerCaseASCII()
+ {
+ StaticAssert<Base::IsStringLike>::check();
+
+ for (SizeType i = 0; i < size(); i++)
+ {
+ const int x = Base::at(i);
+ if ((x <= 'Z') && (x >= 'A'))
+ {
+ Base::at(i) = static_cast<ValueType>(x - ('Z' - 'z'));
+ }
+ }
+ }
+
+ /**
+ * Fills this array as a packed square matrix from a static array.
+ * Please refer to the specification to learn more about matrix packing.
+ * Note that matrix packing code uses @ref areClose() for comparison.
+ */
+ template <typename ScalarType>
+ void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize])
+ {
+ packSquareMatrixImpl<const ScalarType*>(src_row_major);
+ }
+
+ /**
+ * Fills this array as a packed square matrix in place.
+ * Please refer to the specification to learn more about matrix packing.
+ * Note that matrix packing code uses @ref areClose() for comparison.
+ */
+ void packSquareMatrix()
+ {
+ if (this->size() == MaxSize)
+ {
+ ValueType matrix[MaxSize];
+ for (SizeType i = 0; i < MaxSize; i++)
+ {
+ matrix[i] = this->at(i);
+ }
+ packSquareMatrix(matrix);
+ }
+ else if (this->size() == 0)
+ {
+ ; // Nothing to do - leave the matrix empty
+ }
+ else
+ {
+#if UAVCAN_EXCEPTIONS
+ throw std::out_of_range("uavcan::Array::packSquareMatrix()");
+#else
+ UAVCAN_ASSERT(0);
+ this->clear();
+#endif
+ }
+
+ }
+
+ /**
+ * Fills this array as a packed square matrix from any container that has the following public entities:
+ * - method begin()
+ * - method size()
+ * - only for C++03: type value_type
+ * Please refer to the specification to learn more about matrix packing.
+ * Note that matrix packing code uses @ref areClose() for comparison.
+ */
+ template <typename R>
+ typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->begin()) &&
+ sizeof((reinterpret_cast<const R*>(0))->size())>::Type
+ packSquareMatrix(const R& src_row_major)
+ {
+ if (src_row_major.size() == MaxSize)
+ {
+ packSquareMatrixImpl(src_row_major.begin());
+ }
+ else if (src_row_major.size() == 0)
+ {
+ this->clear();
+ }
+ else
+ {
+#if UAVCAN_EXCEPTIONS
+ throw std::out_of_range("uavcan::Array::packSquareMatrix()");
+#else
+ UAVCAN_ASSERT(0);
+ this->clear();
+#endif
+ }
+ }
+
+ /**
+ * Reconstructs full matrix, result will be saved into a static array.
+ * Please refer to the specification to learn more about matrix packing.
+ */
+ template <typename ScalarType>
+ void unpackSquareMatrix(ScalarType (&dst_row_major)[MaxSize]) const
+ {
+ unpackSquareMatrixImpl<ScalarType, ScalarType*>(dst_row_major);
+ }
+
+ /**
+ * Reconstructs full matrix in place.
+ * Please refer to the specification to learn more about matrix packing.
+ */
+ void unpackSquareMatrix()
+ {
+ ValueType matrix[MaxSize];
+ unpackSquareMatrix(matrix);
+
+ this->clear();
+ for (unsigned i = 0; i < MaxSize; i++)
+ {
+ this->push_back(matrix[i]);
+ }
+ }
+
+ /**
+ * Reconstructs full matrix, result will be saved into container that has the following public entities:
+ * - method begin()
+ * - method size()
+ * - only for C++03: type value_type
+ * Please refer to the specification to learn more about matrix packing.
+ */
+ template <typename R>
+ typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->begin()) &&
+ sizeof((reinterpret_cast<const R*>(0))->size())>::Type
+ unpackSquareMatrix(R& dst_row_major) const
+ {
+ if (dst_row_major.size() == MaxSize)
+ {
+#if UAVCAN_CPP_VERSION > UAVCAN_CPP03
+ typedef typename RemoveReference<decltype(*dst_row_major.begin())>::Type RhsValueType;
+ unpackSquareMatrixImpl<RhsValueType>(dst_row_major.begin());
+#else
+ unpackSquareMatrixImpl<typename R::value_type>(dst_row_major.begin());
+#endif
+ }
+ else
+ {
+#if UAVCAN_EXCEPTIONS
+ throw std::out_of_range("uavcan::Array::unpackSquareMatrix()");
+#else
+ UAVCAN_ASSERT(0);
+#endif
+ }
+ }
+
+ /**
+ * Aliases for compatibility with standard containers.
+ */
+ typedef ValueType value_type;
+ typedef SizeType size_type;
+};
+
+/**
+ * These operators will only be enabled if rhs and lhs are different types. This precondition allows to work-around
+ * the ambiguity arising from the scope containing two definitions: one here and the others in Array<>.
+ * Refer to https://github.com/UAVCAN/libuavcan/issues/55 for more info.
+ */
+template <typename R, typename T, ArrayMode ArrayMode, unsigned MaxSize>
+UAVCAN_EXPORT
+inline typename EnableIf<!IsSameType<R, Array<T, ArrayMode, MaxSize> >::Result, bool>::Type
+operator==(const R& rhs, const Array<T, ArrayMode, MaxSize>& lhs)
+{
+ return lhs.operator==(rhs);
+}
+
+template <typename R, typename T, ArrayMode ArrayMode, unsigned MaxSize>
+UAVCAN_EXPORT
+inline typename EnableIf<!IsSameType<R, Array<T, ArrayMode, MaxSize> >::Result, bool>::Type
+operator!=(const R& rhs, const Array<T, ArrayMode, MaxSize>& lhs)
+{
+ return lhs.operator!=(rhs);
+}
+
+/**
+ * Shortcut for string-like array type instantiation.
+ * The proper way of doing this is actually "template<> using ... = ...", but this feature is not available in
+ * older C++ revisions which the library has to support.
+ */
+template <unsigned MaxSize>
+class MakeString
+{
+ MakeString(); // This class is not instantiatable.
+public:
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, MaxSize> Type;
+};
+
+/**
+ * YAML streamer specification for any Array<>
+ */
+template <typename T, ArrayMode ArrayMode, unsigned MaxSize>
+class UAVCAN_EXPORT YamlStreamer<Array<T, ArrayMode, MaxSize> >
+{
+ typedef Array<T, ArrayMode, MaxSize> ArrayType;
+
+ static bool isNiceCharacter(int c)
+ {
+ if (c >= 32 && c <= 126)
+ {
+ return true;
+ }
+ static const char Good[] = {'\n', '\r', '\t'};
+ for (unsigned i = 0; i < sizeof(Good) / sizeof(Good[0]); i++)
+ {
+ if (Good[i] == c)
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ template <typename Stream>
+ static void streamPrimitives(Stream& s, const ArrayType& array)
+ {
+ s << '[';
+ for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
+ {
+ YamlStreamer<T>::stream(s, array.at(i), 0);
+ if ((i + 1) < array.size())
+ {
+ s << ", ";
+ }
+ }
+ s << ']';
+ }
+
+ template <typename Stream>
+ static void streamCharacters(Stream& s, const ArrayType& array)
+ {
+ s << '"';
+ for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
+ {
+ const int c = array.at(i);
+ if (c < 32 || c > 126)
+ {
+ char nibbles[2] = {char((c >> 4) & 0xF), char(c & 0xF)};
+ for (int k = 0; k < 2; k++)
+ {
+ nibbles[k] = char(nibbles[k] + '0');
+ if (nibbles[k] > '9')
+ {
+ nibbles[k] = char(nibbles[k] + 'A' - '9' - 1);
+ }
+ }
+ s << "\\x" << nibbles[0] << nibbles[1];
+ }
+ else
+ {
+ if (c == '"' || c == '\\') // YAML requires to escape these two
+ {
+ s << '\\';
+ }
+ s << char(c);
+ }
+ }
+ s << '"';
+ }
+
+ struct SelectorStringLike { };
+ struct SelectorPrimitives { };
+ struct SelectorObjects { };
+
+ template <typename Stream>
+ static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorStringLike)
+ {
+ bool printable_only = true;
+ for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
+ {
+ if (!isNiceCharacter(array[i]))
+ {
+ printable_only = false;
+ break;
+ }
+ }
+ if (printable_only)
+ {
+ streamCharacters(s, array);
+ }
+ else
+ {
+ streamPrimitives(s, array);
+ s << " # ";
+ streamCharacters(s, array);
+ }
+ }
+
+ template <typename Stream>
+ static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorPrimitives)
+ {
+ streamPrimitives(s, array);
+ }
+
+ template <typename Stream>
+ static void genericStreamImpl(Stream& s, const ArrayType& array, int level, SelectorObjects)
+ {
+ if (array.empty())
+ {
+ s << "[]";
+ return;
+ }
+ for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
+ {
+ s << '\n';
+ for (int pos = 0; pos < level; pos++)
+ {
+ s << " ";
+ }
+ s << "- ";
+ YamlStreamer<T>::stream(s, array.at(i), level + 1);
+ }
+ }
+
+public:
+ /**
+ * Prints Array<> into the stream in YAML format.
+ */
+ template <typename Stream>
+ static void stream(Stream& s, const ArrayType& array, int level)
+ {
+ typedef typename Select<ArrayType::IsStringLike, SelectorStringLike,
+ typename Select<IsPrimitiveType<typename ArrayType::RawValueType>::Result,
+ SelectorPrimitives,
+ SelectorObjects>::Result >::Result Type;
+ genericStreamImpl(s, array, level, Type());
+ }
+};
+
+}
+
+#endif // UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/bit_stream.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED
+#define UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/std.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/transport/abstract_transfer_buffer.hpp>
+#include <uavcan/build_config.hpp>
+
+namespace uavcan
+{
+/**
+ * This function implements fast copy of unaligned bit arrays. It isn't part of the library API, so it is not exported.
+ * @param src_org Source array
+ * @param src_offset Bit offset of the first source byte
+ * @param src_len Number of bits to copy
+ * @param dst_org Destination array
+ * @param dst_offset Bit offset of the first destination byte
+ */
+void bitarrayCopy(const unsigned char* src_org, std::size_t src_offset, std::size_t src_len,
+ unsigned char* dst_org, std::size_t dst_offset);
+
+/**
+ * This class treats a chunk of memory as an array of bits.
+ * It is used by the bit codec for serialization/deserialization.
+ */
+class UAVCAN_EXPORT BitStream
+{
+ static const unsigned MaxBytesPerRW = 16;
+
+ ITransferBuffer& buf_;
+ unsigned bit_offset_;
+ uint8_t byte_cache_;
+
+ static inline unsigned bitlenToBytelen(unsigned bits) { return (bits + 7) / 8; }
+
+ static inline void copyBitArrayAlignedToUnaligned(const uint8_t* src_org, unsigned src_len,
+ uint8_t* dst_org, unsigned dst_offset)
+ {
+ bitarrayCopy(reinterpret_cast<const unsigned char*>(src_org), 0, src_len,
+ reinterpret_cast<unsigned char*>(dst_org), dst_offset);
+ }
+
+ static inline void copyBitArrayUnalignedToAligned(const uint8_t* src_org, unsigned src_offset, unsigned src_len,
+ uint8_t* dst_org)
+ {
+ bitarrayCopy(reinterpret_cast<const unsigned char*>(src_org), src_offset, src_len,
+ reinterpret_cast<unsigned char*>(dst_org), 0);
+ }
+
+public:
+ static const unsigned MaxBitsPerRW = MaxBytesPerRW * 8;
+
+ enum
+ {
+ ResultOutOfBuffer = 0,
+ ResultOk = 1
+ };
+
+ explicit BitStream(ITransferBuffer& buf)
+ : buf_(buf)
+ , bit_offset_(0)
+ , byte_cache_(0)
+ {
+ StaticAssert<sizeof(uint8_t) == 1>::check();
+ }
+
+ /**
+ * Write/read calls interpret bytes as bit arrays, 8 bits per byte, where the most
+ * significant bits have lower index, i.e.:
+ * Hex: 55 2d
+ * Bits: 01010101 00101101
+ * Indices: 0 .. 7 8 .. 15
+ * Return values:
+ * Negative - Error
+ * Zero - Out of buffer space
+ * Positive - OK
+ */
+ int write(const uint8_t* bytes, const unsigned bitlen);
+ int read(uint8_t* bytes, const unsigned bitlen);
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+};
+
+}
+
+#endif // UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/char_array_formatter.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED
+#define UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/marshal/array.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <type_traits>
+#endif
+
+namespace uavcan
+{
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+template <typename ArrayType_>
+class UAVCAN_EXPORT CharArrayFormatter
+{
+ ArrayType_& array_;
+
+ template <typename T>
+ typename std::enable_if<std::is_floating_point<T>::value>::type
+ writeValue(T value)
+ {
+ array_.template appendFormatted<double>("%g", double(value));
+ }
+
+ template <typename T>
+ typename std::enable_if<std::is_integral<T>::value>::type
+ writeValue(T value)
+ {
+ if (std::is_same<T, char>())
+ {
+ if (array_.size() != array_.capacity())
+ {
+ array_.push_back(typename ArrayType_::ValueType(value));
+ }
+ }
+ else if (std::is_signed<T>())
+ {
+ array_.template appendFormatted<long long>("%lli", static_cast<long long>(value));
+ }
+ else
+ {
+ array_.template appendFormatted<unsigned long long>("%llu", static_cast<unsigned long long>(value));
+ }
+ }
+
+ template <typename T>
+ typename std::enable_if<std::is_pointer<T>::value && !std::is_same<T, const char*>::value>::type
+ writeValue(T value)
+ {
+ array_.template appendFormatted<const void*>("%p", static_cast<const void*>(value));
+ }
+
+ void writeValue(const char* value)
+ {
+ array_.template appendFormatted<const char*>("%s", value);
+ }
+
+public:
+ typedef ArrayType_ ArrayType;
+
+ explicit CharArrayFormatter(ArrayType& array)
+ : array_(array)
+ { }
+
+ ArrayType& getArray() { return array_; }
+ const ArrayType& getArray() const { return array_; }
+
+ void write(const char* text)
+ {
+ writeValue(text);
+ }
+
+ template <typename T, typename... Args>
+ void write(const char* s, T value, Args... args)
+ {
+ while (s && *s)
+ {
+ if (*s == '%')
+ {
+ s += 1;
+ if (*s != '%')
+ {
+ writeValue(value);
+ write(++s, args...);
+ break;
+ }
+ }
+ writeValue(*s++);
+ }
+ }
+};
+
+#else
+
+template <typename ArrayType_>
+class UAVCAN_EXPORT CharArrayFormatter
+{
+ ArrayType_& array_;
+
+public:
+ typedef ArrayType_ ArrayType;
+
+ CharArrayFormatter(ArrayType& array)
+ : array_(array)
+ { }
+
+ ArrayType& getArray() { return array_; }
+ const ArrayType& getArray() const { return array_; }
+
+ void write(const char* const text)
+ {
+ array_.template appendFormatted<const char*>("%s", text);
+ }
+
+ /**
+ * This version does not support more than one formatted argument, though it can be improved.
+ * And it is unsafe.
+ * There is typesafe version for C++11 above.
+ */
+ template <typename A>
+ void write(const char* const format, const A value)
+ {
+ array_.template appendFormatted<A>(format, value);
+ }
+};
+
+#endif
+
+}
+
+#endif // UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/float_spec.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,238 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED
+#define UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED
+
+#include <cmath>
+#include <uavcan/std.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/marshal/type_util.hpp>
+#include <uavcan/marshal/integer_spec.hpp>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <limits> // Assuming that in C++11 mode all standard headers are available
+#endif
+
+namespace uavcan
+{
+
+template <unsigned BitLen>
+struct NativeFloatSelector
+{
+ struct ErrorNoSuchFloat;
+ typedef typename Select<(sizeof(float) * 8 >= BitLen), float,
+ typename Select<(sizeof(double) * 8 >= BitLen), double,
+ typename Select<(sizeof(long double) * 8 >= BitLen), long double,
+ ErrorNoSuchFloat>::Result>::Result>::Result Type;
+};
+
+
+class UAVCAN_EXPORT IEEE754Converter
+{
+ // TODO: Non-IEEE float support
+
+ static uint16_t nativeIeeeToHalf(float value);
+ static float halfToNativeIeee(uint16_t value);
+
+ IEEE754Converter();
+
+ template <unsigned BitLen>
+ static void enforceIeee()
+ {
+ /*
+ * Some compilers may have is_iec559 to be defined false despite the fact that IEEE754 is supported.
+ * An acceptable workaround would be to put an #ifdef here.
+ */
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ StaticAssert<std::numeric_limits<typename NativeFloatSelector<BitLen>::Type>::is_iec559>::check();
+#endif
+ }
+
+public:
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ /// UAVCAN requires rounding to nearest for all float conversions
+ static std::float_round_style roundstyle() { return std::round_to_nearest; }
+#endif
+
+ template <unsigned BitLen>
+ static typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType
+ toIeee(typename NativeFloatSelector<BitLen>::Type value)
+ {
+ enforceIeee<BitLen>();
+ union
+ {
+ typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType i;
+ typename NativeFloatSelector<BitLen>::Type f;
+ } u;
+ StaticAssert<sizeof(u.f) * 8 == BitLen>::check();
+ u.f = value;
+ return u.i;
+ }
+
+ template <unsigned BitLen>
+ static typename NativeFloatSelector<BitLen>::Type
+ toNative(typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType value)
+ {
+ enforceIeee<BitLen>();
+ union
+ {
+ typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType i;
+ typename NativeFloatSelector<BitLen>::Type f;
+ } u;
+ StaticAssert<sizeof(u.f) * 8 == BitLen>::check();
+ u.i = value;
+ return u.f;
+ }
+};
+template <>
+inline typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType
+IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value)
+{
+ return nativeIeeeToHalf(value);
+}
+template <>
+inline typename NativeFloatSelector<16>::Type
+IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value)
+{
+ return halfToNativeIeee(value);
+}
+
+
+template <unsigned BitLen> struct IEEE754Limits;
+template <> struct IEEE754Limits<16>
+{
+ typedef typename NativeFloatSelector<16>::Type NativeType;
+ static NativeType max() { return static_cast<NativeType>(65504.0); }
+ static NativeType epsilon() { return static_cast<NativeType>(9.77e-04); }
+};
+template <> struct IEEE754Limits<32>
+{
+ typedef typename NativeFloatSelector<32>::Type NativeType;
+ static NativeType max() { return static_cast<NativeType>(3.40282346638528859812e+38); }
+ static NativeType epsilon() { return static_cast<NativeType>(1.19209289550781250000e-7); }
+};
+template <> struct IEEE754Limits<64>
+{
+ typedef typename NativeFloatSelector<64>::Type NativeType;
+ static NativeType max() { return static_cast<NativeType>(1.79769313486231570815e+308L); }
+ static NativeType epsilon() { return static_cast<NativeType>(2.22044604925031308085e-16L); }
+};
+
+
+template <unsigned BitLen_, CastMode CastMode>
+class UAVCAN_EXPORT FloatSpec : public IEEE754Limits<BitLen_>
+{
+ FloatSpec();
+
+public:
+ enum { BitLen = BitLen_ };
+ enum { MinBitLen = BitLen };
+ enum { MaxBitLen = BitLen };
+ enum { IsPrimitive = 1 };
+
+ typedef typename NativeFloatSelector<BitLen>::Type StorageType;
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+ enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) };
+#else
+ enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits<StorageType>::is_iec559 };
+#endif
+
+ using IEEE754Limits<BitLen>::max;
+ using IEEE754Limits<BitLen>::epsilon;
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ static std::float_round_style roundstyle() { return IEEE754Converter::roundstyle(); }
+#endif
+
+ static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode)
+ {
+ // cppcheck-suppress duplicateExpression
+ if (CastMode == CastModeSaturate)
+ {
+ saturate(value);
+ }
+ else
+ {
+ truncate(value);
+ }
+ return codec.encode<BitLen>(IEEE754Converter::toIeee<BitLen>(value));
+ }
+
+ static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
+ {
+ typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType ieee = 0;
+ const int res = codec.decode<BitLen>(ieee);
+ if (res <= 0)
+ {
+ return res;
+ }
+ out_value = IEEE754Converter::toNative<BitLen>(ieee);
+ return res;
+ }
+
+ static void extendDataTypeSignature(DataTypeSignature&) { }
+
+private:
+ static inline void saturate(StorageType& value)
+ {
+ if ((IsExactRepresentation == 0) && isFinite(value))
+ {
+ if (value > max())
+ {
+ value = max();
+ }
+ else if (value < -max())
+ {
+ value = -max();
+ }
+ else
+ {
+ ; // Valid range
+ }
+ }
+ }
+
+ static inline void truncate(StorageType& value)
+ {
+ if ((IsExactRepresentation == 0) && isFinite(value))
+ {
+ if (value > max())
+ {
+ value = NumericTraits<StorageType>::infinity();
+ }
+ else if (value < -max())
+ {
+ value = -NumericTraits<StorageType>::infinity();
+ }
+ else
+ {
+ ; // Valid range
+ }
+ }
+ }
+};
+
+
+template <unsigned BitLen, CastMode CastMode>
+class UAVCAN_EXPORT YamlStreamer<FloatSpec<BitLen, CastMode> >
+{
+ typedef typename FloatSpec<BitLen, CastMode>::StorageType StorageType;
+
+public:
+ template <typename Stream> // cppcheck-suppress passedByValue
+ static void stream(Stream& s, const StorageType value, int)
+ {
+ s << value;
+ }
+};
+
+}
+
+#endif // UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/integer_spec.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,210 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED
+#define UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED
+
+#include <uavcan/std.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/marshal/scalar_codec.hpp>
+#include <uavcan/marshal/type_util.hpp>
+
+namespace uavcan
+{
+
+enum Signedness { SignednessUnsigned, SignednessSigned };
+
+/**
+ * This template will be used for signed and unsigned integers more than 1 bit long.
+ * There are explicit specializations for booleans below.
+ */
+template <unsigned BitLen_, Signedness Signedness, CastMode CastMode>
+class UAVCAN_EXPORT IntegerSpec
+{
+ struct ErrorNoSuchInteger;
+
+public:
+ enum { IsSigned = Signedness == SignednessSigned };
+ enum { BitLen = BitLen_ };
+ enum { MinBitLen = BitLen };
+ enum { MaxBitLen = BitLen };
+ enum { IsPrimitive = 1 };
+
+ typedef typename Select<(BitLen <= 8), typename Select<IsSigned, int8_t, uint8_t>::Result,
+ typename Select<(BitLen <= 16), typename Select<IsSigned, int16_t, uint16_t>::Result,
+ typename Select<(BitLen <= 32), typename Select<IsSigned, int32_t, uint32_t>::Result,
+ typename Select<(BitLen <= 64), typename Select<IsSigned, int64_t, uint64_t>::Result,
+ ErrorNoSuchInteger>::Result>::Result>::Result>::Result StorageType;
+
+ typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastMode>::StorageType UnsignedStorageType;
+
+private:
+ IntegerSpec();
+
+ struct LimitsImplGeneric
+ {
+ static StorageType max()
+ {
+ StaticAssert<(sizeof(uintmax_t) >= 8)>::check();
+ if (IsSigned == 0)
+ {
+ return StorageType((uintmax_t(1) << static_cast<unsigned>(BitLen)) - 1U);
+ }
+ else
+ {
+ return StorageType((uintmax_t(1) << (static_cast<unsigned>(BitLen) - 1U)) - 1);
+ }
+ }
+ static UnsignedStorageType mask()
+ {
+ StaticAssert<(sizeof(uintmax_t) >= 8U)>::check();
+ return UnsignedStorageType((uintmax_t(1) << static_cast<unsigned>(BitLen)) - 1U);
+ }
+ };
+
+ struct LimitsImpl64
+ {
+ static StorageType max()
+ {
+ return StorageType((IsSigned == 0) ? 0xFFFFFFFFFFFFFFFFULL : 0x7FFFFFFFFFFFFFFFLL);
+ }
+ static UnsignedStorageType mask() { return 0xFFFFFFFFFFFFFFFFULL; }
+ };
+
+ typedef typename Select<(BitLen == 64), LimitsImpl64, LimitsImplGeneric>::Result Limits;
+
+ static void saturate(StorageType& value)
+ {
+ if (value > max())
+ {
+ value = max();
+ }
+ else if (value <= min()) // 'Less or Equal' allows to suppress compiler warning on unsigned types
+ {
+ value = min();
+ }
+ else
+ {
+ ; // Valid range
+ }
+ }
+
+ static void truncate(StorageType& value) { value = value & StorageType(mask()); }
+
+ static void validate()
+ {
+ StaticAssert<(BitLen <= (sizeof(StorageType) * 8))>::check();
+ // coverity[result_independent_of_operands : FALSE]
+ UAVCAN_ASSERT(max() <= NumericTraits<StorageType>::max());
+ // coverity[result_independent_of_operands : FALSE]
+ UAVCAN_ASSERT(min() >= NumericTraits<StorageType>::min());
+ }
+
+public:
+ static StorageType max() { return Limits::max(); }
+ static StorageType min() { return IsSigned ? StorageType(-max() - 1) : 0; }
+ static UnsignedStorageType mask() { return Limits::mask(); }
+
+ static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode)
+ {
+ validate();
+ // cppcheck-suppress duplicateExpression
+ if (CastMode == CastModeSaturate)
+ {
+ saturate(value);
+ }
+ else
+ {
+ truncate(value);
+ }
+ return codec.encode<BitLen>(value);
+ }
+
+ static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
+ {
+ validate();
+ return codec.decode<BitLen>(out_value);
+ }
+
+ static void extendDataTypeSignature(DataTypeSignature&) { }
+};
+
+/**
+ * Boolean specialization
+ */
+template <CastMode CastMode>
+class UAVCAN_EXPORT IntegerSpec<1, SignednessUnsigned, CastMode>
+{
+public:
+ enum { IsSigned = 0 };
+ enum { BitLen = 1 };
+ enum { MinBitLen = 1 };
+ enum { MaxBitLen = 1 };
+ enum { IsPrimitive = 1 };
+
+ typedef bool StorageType;
+ typedef bool UnsignedStorageType;
+
+private:
+ IntegerSpec();
+
+public:
+ static StorageType max() { return true; }
+ static StorageType min() { return false; }
+ static UnsignedStorageType mask() { return true; }
+
+ static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode)
+ {
+ return codec.encode<BitLen>(value);
+ }
+
+ static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
+ {
+ return codec.decode<BitLen>(out_value);
+ }
+
+ static void extendDataTypeSignature(DataTypeSignature&) { }
+};
+
+template <CastMode CastMode>
+class IntegerSpec<1, SignednessSigned, CastMode>; // Invalid instantiation
+
+template <Signedness Signedness, CastMode CastMode>
+class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation
+
+
+template <typename T>
+struct IsIntegerSpec
+{
+ enum { Result = 0 };
+};
+
+template <unsigned BitLen, Signedness Signedness, CastMode CastMode>
+struct IsIntegerSpec<IntegerSpec<BitLen, Signedness, CastMode> >
+{
+ enum { Result = 1 };
+};
+
+
+template <unsigned BitLen, Signedness Signedness, CastMode CastMode>
+class UAVCAN_EXPORT YamlStreamer<IntegerSpec<BitLen, Signedness, CastMode> >
+{
+ typedef IntegerSpec<BitLen, Signedness, CastMode> RawType;
+ typedef typename RawType::StorageType StorageType;
+
+public:
+ template <typename Stream> // cppcheck-suppress passedByValue
+ static void stream(Stream& s, const StorageType value, int)
+ {
+ // Get rid of character types - we want its integer representation, not ASCII code
+ typedef typename Select<(sizeof(StorageType) >= sizeof(int)), StorageType,
+ typename Select<RawType::IsSigned, int, unsigned>::Result >::Result TempType;
+ s << TempType(value);
+ }
+};
+
+}
+
+#endif // UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/scalar_codec.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED
+#define UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/marshal/bit_stream.hpp>
+
+namespace uavcan
+{
+/**
+ * This class implements fast encoding/decoding of primitive type scalars into/from bit arrays.
+ * It uses the compile-time type information to eliminate run-time operations where possible.
+ */
+class UAVCAN_EXPORT ScalarCodec
+{
+ BitStream& stream_;
+
+ static void swapByteOrder(uint8_t* bytes, unsigned len);
+
+ template <unsigned BitLen, unsigned Size>
+ static typename EnableIf<(BitLen > 8)>::Type
+ convertByteOrder(uint8_t (&bytes)[Size])
+ {
+#if defined(BYTE_ORDER) && defined(BIG_ENDIAN)
+ static const bool big_endian = BYTE_ORDER == BIG_ENDIAN;
+#else
+ union { long int l; char c[sizeof(long int)]; } u;
+ u.l = 1;
+ const bool big_endian = u.c[sizeof(long int) - 1] == 1;
+#endif
+ /*
+ * I didn't have any big endian machine nearby, so big endian support wasn't tested yet.
+ * It is likely to be OK anyway, so feel free to remove this UAVCAN_ASSERT() as needed.
+ */
+ UAVCAN_ASSERT(big_endian == false);
+ if (big_endian)
+ {
+ swapByteOrder(bytes, Size);
+ }
+ }
+
+ template <unsigned BitLen, unsigned Size>
+ static typename EnableIf<(BitLen <= 8)>::Type
+ convertByteOrder(uint8_t (&)[Size]) { }
+
+ template <unsigned BitLen, typename T>
+ static typename EnableIf<static_cast<bool>(NumericTraits<T>::IsSigned) && ((sizeof(T) * 8) > BitLen)>::Type
+ fixTwosComplement(T& value)
+ {
+ StaticAssert<NumericTraits<T>::IsInteger>::check(); // Not applicable to floating point types
+ if (value & (T(1) << (BitLen - 1))) // The most significant bit is set --> negative
+ {
+ value |= T(T(0xFFFFFFFFFFFFFFFFULL) & ~((T(1) << BitLen) - 1));
+ }
+ }
+
+ template <unsigned BitLen, typename T>
+ static typename EnableIf<!static_cast<bool>(NumericTraits<T>::IsSigned) || ((sizeof(T) * 8) == BitLen)>::Type
+ fixTwosComplement(T&) { }
+
+ template <unsigned BitLen, typename T>
+ static typename EnableIf<((sizeof(T) * 8) > BitLen)>::Type
+ clearExtraBits(T& value)
+ {
+ value &= (T(1) << BitLen) - 1; // Signedness doesn't matter
+ }
+
+ template <unsigned BitLen, typename T>
+ static typename EnableIf<((sizeof(T) * 8) == BitLen)>::Type
+ clearExtraBits(T&) { }
+
+ template <unsigned BitLen, typename T>
+ void validate()
+ {
+ StaticAssert<((sizeof(T) * 8) >= BitLen)>::check();
+ StaticAssert<(BitLen <= BitStream::MaxBitsPerRW)>::check();
+ StaticAssert<static_cast<bool>(NumericTraits<T>::IsSigned) ? (BitLen > 1) : true>::check();
+ }
+
+ int encodeBytesImpl(uint8_t* bytes, unsigned bitlen);
+ int decodeBytesImpl(uint8_t* bytes, unsigned bitlen);
+
+public:
+ explicit ScalarCodec(BitStream& stream)
+ : stream_(stream)
+ { }
+
+ template <unsigned BitLen, typename T>
+ int encode(const T value);
+
+ template <unsigned BitLen, typename T>
+ int decode(T& value);
+};
+
+// ----------------------------------------------------------------------------
+
+template <unsigned BitLen, typename T>
+int ScalarCodec::encode(const T value)
+{
+ validate<BitLen, T>();
+ union ByteUnion
+ {
+ T value;
+ uint8_t bytes[sizeof(T)];
+ } byte_union;
+ byte_union.value = value;
+ clearExtraBits<BitLen, T>(byte_union.value);
+ convertByteOrder<BitLen>(byte_union.bytes);
+ return encodeBytesImpl(byte_union.bytes, BitLen);
+}
+
+template <unsigned BitLen, typename T>
+int ScalarCodec::decode(T& value)
+{
+ validate<BitLen, T>();
+ union ByteUnion
+ {
+ T value;
+ uint8_t bytes[sizeof(T)];
+ } byte_union;
+ byte_union.value = T();
+ const int read_res = decodeBytesImpl(byte_union.bytes, BitLen);
+ if (read_res > 0)
+ {
+ convertByteOrder<BitLen>(byte_union.bytes);
+ fixTwosComplement<BitLen, T>(byte_union.value);
+ value = byte_union.value;
+ }
+ return read_res;
+}
+
+}
+
+#endif // UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/marshal/type_util.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED
+#define UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/comparison.hpp>
+
+namespace uavcan
+{
+/**
+ * Read the specs to learn more about cast modes.
+ */
+enum CastMode { CastModeSaturate, CastModeTruncate };
+
+/**
+ * Read the specs to learn more about tail array optimizations.
+ */
+enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled };
+
+/**
+ * Compile-time: Returns the number of bits needed to represent an integer value.
+ */
+template <unsigned long Num>
+struct IntegerBitLen
+{
+ enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result };
+};
+template <>
+struct IntegerBitLen<0>
+{
+ enum { Result = 0 };
+};
+
+/**
+ * Compile-time: Returns the number of bytes needed to contain the given number of bits. Assumes 1 byte == 8 bit.
+ */
+template <unsigned long BitLen>
+struct BitLenToByteLen
+{
+ enum { Result = (BitLen + 7) / 8 };
+};
+
+/**
+ * Compile-time: Helper for integer and float specialization classes. Returns the platform-specific storage type.
+ */
+template <typename T, typename Enable = void>
+struct StorageType
+{
+ typedef T Type;
+};
+template <typename T>
+struct StorageType<T, typename EnableIfType<typename T::StorageType>::Type>
+{
+ typedef typename T::StorageType Type;
+};
+
+/**
+ * Compile-time: Whether T is a primitive type on this platform.
+ */
+template <typename T>
+class IsPrimitiveType
+{
+ typedef char Yes;
+ struct No { Yes dummy[8]; };
+
+ template <typename U>
+ static typename EnableIf<U::IsPrimitive, Yes>::Type test(int);
+
+ template <typename>
+ static No test(...);
+
+public:
+ enum { Result = sizeof(test<T>(0)) == sizeof(Yes) };
+};
+
+/**
+ * Streams a given value into YAML string. Please see the specializations.
+ */
+template <typename T>
+class UAVCAN_EXPORT YamlStreamer;
+
+}
+
+#endif // UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/include/uavcan/marshal/types.hpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,13 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#ifndef UAVCAN_MARSHAL_TYPES_HPP_INCLUDED +#define UAVCAN_MARSHAL_TYPES_HPP_INCLUDED + +#include <uavcan/marshal/integer_spec.hpp> +#include <uavcan/marshal/float_spec.hpp> +#include <uavcan/marshal/array.hpp> +#include <uavcan/marshal/type_util.hpp> + +#endif // UAVCAN_MARSHAL_TYPES_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/abstract_node.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED
+#define UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/node/scheduler.hpp>
+
+namespace uavcan
+{
+/**
+ * Abstract node class. If you're going to implement your own node class for your application,
+ * please inherit this class so it can be used with default publisher, subscriber, server, etc. classes.
+ * Normally you don't need to use it directly though - please refer to the class Node<> instead.
+ */
+class UAVCAN_EXPORT INode
+{
+public:
+ virtual ~INode() { }
+ virtual IPoolAllocator& getAllocator() = 0;
+ virtual Scheduler& getScheduler() = 0;
+ virtual const Scheduler& getScheduler() const = 0;
+ virtual void registerInternalFailure(const char* msg) = 0;
+
+ Dispatcher& getDispatcher() { return getScheduler().getDispatcher(); }
+ const Dispatcher& getDispatcher() const { return getScheduler().getDispatcher(); }
+
+ ISystemClock& getSystemClock() { return getScheduler().getSystemClock(); }
+ MonotonicTime getMonotonicTime() const { return getScheduler().getMonotonicTime(); }
+ UtcTime getUtcTime() const { return getScheduler().getUtcTime(); }
+
+ /**
+ * Returns the Node ID of this node.
+ * If Node ID was not set yet, an invalid value will be returned.
+ */
+ NodeID getNodeID() const { return getScheduler().getDispatcher().getNodeID(); }
+
+ /**
+ * Sets the Node ID of this node.
+ * Node ID can be assigned only once. This method returns true if the Node ID was successfully assigned, otherwise
+ * it returns false.
+ * As long as a valid Node ID is not set, the node will remain in passive mode.
+ * Using a non-unicast Node ID puts the node into passive mode (as default).
+ */
+ bool setNodeID(NodeID nid)
+ {
+ return getScheduler().getDispatcher().setNodeID(nid);
+ }
+
+ /**
+ * Whether the node is in passive mode, i.e. can't transmit anything to the bus.
+ * Please read the specs to learn more.
+ */
+ bool isPassiveMode() const { return getScheduler().getDispatcher().isPassiveMode(); }
+
+ /**
+ * Same as @ref spin(MonotonicDuration), but the deadline is specified as an absolute time value
+ * rather than duration.
+ */
+ int spin(MonotonicTime deadline)
+ {
+ return getScheduler().spin(deadline);
+ }
+
+ /**
+ * Runs the node.
+ * Normally your application should not block anywhere else.
+ * Block inside this method forever or call it periodically.
+ * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp).
+ */
+ int spin(MonotonicDuration duration)
+ {
+ return getScheduler().spin(getMonotonicTime() + duration);
+ }
+
+ /**
+ * This method is designed for non-blocking applications.
+ * Instead of blocking, it returns immediately once all available CAN frames and timer events are processed.
+ * Note that this is unlike plain @ref spin(), which will strictly return when the deadline is reached,
+ * even if there still are unprocessed events.
+ * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp).
+ */
+ int spinOnce()
+ {
+ return getScheduler().spinOnce();
+ }
+
+ /**
+ * This method allows to directly transmit a raw CAN frame circumventing the whole UAVCAN stack.
+ * Mandatory parameters:
+ *
+ * @param frame CAN frame to be transmitted.
+ *
+ * @param tx_deadline The frame will be discarded if it could not be transmitted by this time.
+ *
+ * @param iface_mask This bitmask allows to select what CAN interfaces this frame should go into.
+ * Example:
+ * - 1 - the frame will be sent only to iface 0.
+ * - 4 - the frame will be sent only to iface 2.
+ * - 3 - the frame will be sent to ifaces 0 and 1.
+ *
+ * Optional parameters:
+ *
+ * @param qos Quality of service. Please refer to the CAN IO manager for details.
+ *
+ * @param flags CAN IO flags. Please refer to the CAN driver API for details.
+ */
+ int injectTxFrame(const CanFrame& frame, MonotonicTime tx_deadline, uint8_t iface_mask,
+ CanTxQueue::Qos qos = CanTxQueue::Volatile,
+ CanIOFlags flags = 0)
+ {
+ return getDispatcher().getCanIOManager().send(frame, tx_deadline, MonotonicTime(), iface_mask, qos, flags);
+ }
+
+#if !UAVCAN_TINY
+ /**
+ * The @ref IRxFrameListener interface allows one to monitor all incoming CAN frames.
+ * This feature can be used to implement multithreaded nodes, or to add secondary protocol support.
+ */
+ void removeRxFrameListener() { getDispatcher().removeRxFrameListener(); }
+ void installRxFrameListener(IRxFrameListener* lst) { getDispatcher().installRxFrameListener(lst); }
+#endif
+};
+
+}
+
+#endif // UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/generic_publisher.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED
+#define UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED
+
+#include <uavcan/error.hpp>
+#include <uavcan/node/abstract_node.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/node/global_data_type_registry.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+#include <uavcan/transport/transfer_sender.hpp>
+#include <uavcan/marshal/scalar_codec.hpp>
+#include <uavcan/marshal/types.hpp>
+
+namespace uavcan
+{
+
+class GenericPublisherBase : Noncopyable
+{
+ TransferSender sender_;
+ MonotonicDuration tx_timeout_;
+ INode& node_;
+
+protected:
+ GenericPublisherBase(INode& node, MonotonicDuration tx_timeout,
+ MonotonicDuration max_transfer_interval)
+ : sender_(node.getDispatcher(), max_transfer_interval)
+ , tx_timeout_(tx_timeout)
+ , node_(node)
+ {
+ setTxTimeout(tx_timeout);
+#if UAVCAN_DEBUG
+ UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK
+#endif
+ }
+
+ ~GenericPublisherBase() { }
+
+ bool isInited() const;
+
+ int doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos);
+
+ MonotonicTime getTxDeadline() const;
+
+ int genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type,
+ NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline);
+
+ TransferSender& getTransferSender() { return sender_; }
+ const TransferSender& getTransferSender() const { return sender_; }
+
+public:
+ static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); }
+ static MonotonicDuration getMaxTxTimeout() { return MonotonicDuration::fromMSec(60000); }
+
+ MonotonicDuration getTxTimeout() const { return tx_timeout_; }
+ void setTxTimeout(MonotonicDuration tx_timeout);
+
+ /**
+ * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive.
+ * This option allows to enable sending anonymous transfers from passive mode.
+ */
+ void allowAnonymousTransfers()
+ {
+ sender_.allowAnonymousTransfers();
+ }
+
+ /**
+ * Priority of outgoing transfers.
+ */
+ TransferPriority getPriority() const { return sender_.getPriority(); }
+ void setPriority(const TransferPriority prio) { sender_.setPriority(prio); }
+
+ INode& getNode() const { return node_; }
+};
+
+/**
+ * Generic publisher, suitable for messages and services.
+ * DataSpec - data type specification class
+ * DataStruct - instantiable class
+ */
+template <typename DataSpec, typename DataStruct>
+class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase
+{
+ struct ZeroTransferBuffer : public StaticTransferBufferImpl
+ {
+ ZeroTransferBuffer() : StaticTransferBufferImpl(UAVCAN_NULLPTR, 0) { }
+ };
+
+ typedef typename Select<DataStruct::MaxBitLen == 0,
+ ZeroTransferBuffer,
+ StaticTransferBuffer<BitLenToByteLen<DataStruct::MaxBitLen>::Result> >::Result Buffer;
+
+ enum
+ {
+ Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
+ CanTxQueue::Volatile : CanTxQueue::Persistent
+ };
+
+ int checkInit();
+
+ int doEncode(const DataStruct& message, ITransferBuffer& buffer) const;
+
+ int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
+ TransferID* tid, MonotonicTime blocking_deadline);
+
+public:
+ /**
+ * @param max_transfer_interval Maximum expected time interval between subsequent publications. Leave default.
+ */
+ GenericPublisher(INode& node, MonotonicDuration tx_timeout,
+ MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
+ : GenericPublisherBase(node, tx_timeout, max_transfer_interval)
+ { }
+
+ ~GenericPublisher() { }
+
+ /**
+ * Init method can be called prior first publication, but it's not necessary
+ * because the publisher can be automatically initialized ad-hoc.
+ */
+ int init()
+ {
+ return checkInit();
+ }
+
+ /**
+ * This overload allows to set the priority; otherwise it's the same.
+ */
+ int init(TransferPriority priority)
+ {
+ setPriority(priority);
+ return checkInit();
+ }
+
+ int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
+ MonotonicTime blocking_deadline = MonotonicTime())
+ {
+ return genericPublish(message, transfer_type, dst_node_id, UAVCAN_NULLPTR, blocking_deadline);
+ }
+
+ int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID tid,
+ MonotonicTime blocking_deadline = MonotonicTime())
+ {
+ return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline);
+ }
+};
+
+// ----------------------------------------------------------------------------
+
+template <typename DataSpec, typename DataStruct>
+int GenericPublisher<DataSpec, DataStruct>::checkInit()
+{
+ if (isInited())
+ {
+ return 0;
+ }
+ return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos));
+}
+
+template <typename DataSpec, typename DataStruct>
+int GenericPublisher<DataSpec, DataStruct>::doEncode(const DataStruct& message, ITransferBuffer& buffer) const
+{
+ BitStream bitstream(buffer);
+ ScalarCodec codec(bitstream);
+ const int encode_res = DataStruct::encode(message, codec);
+ if (encode_res <= 0)
+ {
+ UAVCAN_ASSERT(0); // Impossible, internal error
+ return -ErrInvalidMarshalData;
+ }
+ return encode_res;
+}
+
+template <typename DataSpec, typename DataStruct>
+int GenericPublisher<DataSpec, DataStruct>::genericPublish(const DataStruct& message, TransferType transfer_type,
+ NodeID dst_node_id, TransferID* tid,
+ MonotonicTime blocking_deadline)
+{
+ const int res = checkInit();
+ if (res < 0)
+ {
+ return res;
+ }
+
+ Buffer buffer;
+
+ const int encode_res = doEncode(message, buffer);
+ if (encode_res < 0)
+ {
+ return encode_res;
+ }
+
+ return GenericPublisherBase::genericPublish(buffer, transfer_type, dst_node_id, tid, blocking_deadline);
+}
+
+}
+
+#endif // UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
+#define UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
+
+#include <uavcan/error.hpp>
+#include <uavcan/node/abstract_node.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/node/global_data_type_registry.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/transport/transfer_listener.hpp>
+#include <uavcan/marshal/scalar_codec.hpp>
+#include <uavcan/marshal/types.hpp>
+
+namespace uavcan
+{
+/**
+ * This class extends the data structure with extra information obtained from the transport layer,
+ * such as Source Node ID, timestamps, Transfer ID, index of the interface this transfer was picked up from, etc.
+ *
+ * PLEASE NOTE that since this class inherits the data structure type, subscription callbacks can accept either
+ * object of this class or the data structure type directly if the extra information is not needed.
+ *
+ * For example, both of these callbacks can be used with the same data structure 'Foo':
+ * void first(const ReceivedDataStructure<Foo>& msg);
+ * void second(const Foo& msg);
+ * In the latter case, an implicit cast will happen before the callback is invoked.
+ *
+ * This class is not copyable because it holds a reference to a stack-allocated transfer descriptor object.
+ * You can slice cast it to the underlying data type though, which would be copyable:
+ * DataType dt = rds; // where rds is of type ReceivedDataStructure<DataType>
+ * // dt is now copyable
+ */
+template <typename DataType_>
+class UAVCAN_EXPORT ReceivedDataStructure : public DataType_, Noncopyable
+{
+ const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields
+
+ template <typename Ret, Ret(IncomingTransfer::*Fun) () const>
+ Ret safeget() const
+ {
+ if (_transfer_ == UAVCAN_NULLPTR)
+ {
+ return Ret();
+ }
+ return (_transfer_->*Fun)();
+ }
+
+protected:
+ ReceivedDataStructure()
+ : _transfer_(UAVCAN_NULLPTR)
+ { }
+
+ ReceivedDataStructure(const IncomingTransfer* arg_transfer)
+ : _transfer_(arg_transfer)
+ {
+ UAVCAN_ASSERT(arg_transfer != UAVCAN_NULLPTR);
+ }
+
+public:
+ typedef DataType_ DataType;
+
+ MonotonicTime getMonotonicTimestamp() const
+ {
+ return safeget<MonotonicTime, &IncomingTransfer::getMonotonicTimestamp>();
+ }
+ UtcTime getUtcTimestamp() const { return safeget<UtcTime, &IncomingTransfer::getUtcTimestamp>(); }
+ TransferPriority getPriority() const { return safeget<TransferPriority, &IncomingTransfer::getPriority>(); }
+ TransferType getTransferType() const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
+ TransferID getTransferID() const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
+ NodeID getSrcNodeID() const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
+ uint8_t getIfaceIndex() const { return safeget<uint8_t, &IncomingTransfer::getIfaceIndex>(); }
+ bool isAnonymousTransfer() const { return safeget<bool, &IncomingTransfer::isAnonymousTransfer>(); }
+};
+
+/**
+ * This operator neatly prints the data structure prepended with extra data from the transport layer.
+ * The extra data will be represented as YAML comment.
+ */
+template <typename Stream, typename DataType>
+static Stream& operator<<(Stream& s, const ReceivedDataStructure<DataType>& rds)
+{
+ s << "# Received struct ts_m=" << rds.getMonotonicTimestamp()
+ << " ts_utc=" << rds.getUtcTimestamp()
+ << " snid=" << int(rds.getSrcNodeID().get()) << "\n";
+ s << static_cast<const DataType&>(rds);
+ return s;
+}
+
+
+class GenericSubscriberBase : Noncopyable
+{
+protected:
+ INode& node_;
+ uint32_t failure_count_;
+
+ explicit GenericSubscriberBase(INode& node)
+ : node_(node)
+ , failure_count_(0)
+ { }
+
+ ~GenericSubscriberBase() { }
+
+ int genericStart(TransferListener* listener, bool (Dispatcher::*registration_method)(TransferListener*));
+
+ void stop(TransferListener* listener);
+
+public:
+ /**
+ * Returns the number of failed attempts to decode received message. Generally, a failed attempt means either:
+ * - Transient failure in the transport layer.
+ * - Incompatible data types.
+ */
+ uint32_t getFailureCount() const { return failure_count_; }
+
+ INode& getNode() const { return node_; }
+};
+
+/**
+ * Please note that the reference passed to the RX callback points to a stack-allocated object, which means
+ * that it gets invalidated shortly after the callback returns.
+ */
+template <typename DataSpec, typename DataStruct, typename TransferListenerType>
+class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase
+{
+ typedef GenericSubscriber<DataSpec, DataStruct, TransferListenerType> SelfType;
+
+ // We need to break the inheritance chain here to implement lazy initialization
+ class TransferForwarder : public TransferListenerType
+ {
+ SelfType& obj_;
+
+ void handleIncomingTransfer(IncomingTransfer& transfer)
+ {
+ obj_.handleIncomingTransfer(transfer);
+ }
+
+ public:
+ TransferForwarder(SelfType& obj,
+ const DataTypeDescriptor& data_type,
+ uint16_t max_buffer_size,
+ IPoolAllocator& allocator) :
+ TransferListenerType(obj.node_.getDispatcher().getTransferPerfCounter(),
+ data_type,
+ max_buffer_size,
+ allocator),
+ obj_(obj)
+ { }
+ };
+
+ LazyConstructor<TransferForwarder> forwarder_;
+
+ int checkInit();
+
+ void handleIncomingTransfer(IncomingTransfer& transfer);
+
+ int genericStart(bool (Dispatcher::*registration_method)(TransferListener*));
+
+protected:
+ struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
+ {
+ ReceivedDataStructureSpec() { }
+
+ ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) :
+ ReceivedDataStructure<DataStruct>(arg_transfer)
+ { }
+ };
+
+ explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node)
+ { }
+
+ virtual ~GenericSubscriber() { stop(); }
+
+ virtual void handleReceivedDataStruct(ReceivedDataStructure<DataStruct>&) = 0;
+
+ int startAsMessageListener()
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Start as message listener; dtname=%s", DataSpec::getDataTypeFullName());
+ return genericStart(&Dispatcher::registerMessageListener);
+ }
+
+ int startAsServiceRequestListener()
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Start as service request listener; dtname=%s",
+ DataSpec::getDataTypeFullName());
+ return genericStart(&Dispatcher::registerServiceRequestListener);
+ }
+
+ int startAsServiceResponseListener()
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Start as service response listener; dtname=%s",
+ DataSpec::getDataTypeFullName());
+ return genericStart(&Dispatcher::registerServiceResponseListener);
+ }
+
+ /**
+ * By default, anonymous transfers will be ignored.
+ * This option allows to enable reception of anonymous transfers.
+ */
+ void allowAnonymousTransfers()
+ {
+ forwarder_->allowAnonymousTransfers();
+ }
+
+ /**
+ * Terminate the subscription.
+ * Dispatcher core will remove this instance from the subscribers list.
+ */
+ void stop()
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName());
+ GenericSubscriberBase::stop(forwarder_);
+ }
+
+ TransferListenerType* getTransferListener() { return forwarder_; }
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * GenericSubscriber
+ */
+template <typename DataSpec, typename DataStruct, typename TransferListenerType>
+int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::checkInit()
+{
+ if (forwarder_)
+ {
+ return 0;
+ }
+
+ GlobalDataTypeRegistry::instance().freeze();
+ const DataTypeDescriptor* const descr =
+ GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName());
+ if (descr == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
+ return -ErrUnknownDataType;
+ }
+
+ static const uint16_t MaxBufferSize = BitLenToByteLen<DataStruct::MaxBitLen>::Result;
+
+ forwarder_.template construct<SelfType&, const DataTypeDescriptor&, uint16_t, IPoolAllocator&>
+ (*this, *descr, MaxBufferSize, node_.getAllocator());
+
+ return 0;
+}
+
+template <typename DataSpec, typename DataStruct, typename TransferListenerType>
+void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
+{
+ ReceivedDataStructureSpec rx_struct(&transfer);
+
+ /*
+ * Decoding into the temporary storage
+ */
+ BitStream bitstream(transfer);
+ ScalarCodec codec(bitstream);
+
+ const int decode_res = DataStruct::decode(rx_struct, codec);
+
+ // We don't need the data anymore, the memory can be reused from the callback:
+ transfer.release();
+
+ if (decode_res <= 0)
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]",
+ decode_res, DataSpec::getDataTypeFullName());
+ failure_count_++;
+ node_.getDispatcher().getTransferPerfCounter().addError();
+ return;
+ }
+
+ /*
+ * Invoking the callback
+ */
+ handleReceivedDataStruct(rx_struct);
+}
+
+template <typename DataSpec, typename DataStruct, typename TransferListenerType>
+int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::
+genericStart(bool (Dispatcher::*registration_method)(TransferListener*))
+{
+ const int res = checkInit();
+ if (res < 0)
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName());
+ return res;
+ }
+ return GenericSubscriberBase::genericStart(forwarder_, registration_method);
+}
+
+
+}
+
+#endif // UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/global_data_type_registry.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED
+#define UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/error.hpp>
+#include <uavcan/std.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/linked_list.hpp>
+#if UAVCAN_DEBUG
+# include <uavcan/debug.hpp>
+#endif
+
+namespace uavcan
+{
+/**
+ * This singleton is shared among all existing node instances. It is instantiated automatically
+ * when the C++ runtime executes contstructors before main().
+ *
+ * Its purpose is to keep the list of all UAVCAN data types known and used by this application.
+ *
+ * Also, the mapping between Data Type name and its Data Type ID is also stored in this singleton.
+ * UAVCAN data types with default Data Type ID that are autogenerated by the libuavcan DSDL compiler
+ * are registered automatically before main() (refer to the generated headers to see how exactly).
+ * Data types that don't have a default Data Type ID must be registered manually using the methods
+ * of this class (read the method documentation).
+ *
+ * Attempt to use a data type that was not registered with this singleton (e.g. publish, subscribe,
+ * perform a service call etc.) will fail with an error code @ref ErrUnknownDataType.
+ */
+class UAVCAN_EXPORT GlobalDataTypeRegistry : Noncopyable
+{
+ struct Entry : public LinkedListNode<Entry>
+ {
+ DataTypeDescriptor descriptor;
+
+ Entry() { }
+
+ Entry(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name)
+ : descriptor(kind, id, signature, name)
+ { }
+ };
+
+ struct EntryInsertionComparator
+ {
+ const DataTypeID id;
+ explicit EntryInsertionComparator(const Entry* dtd)
+ : id((dtd == UAVCAN_NULLPTR) ? DataTypeID() : dtd->descriptor.getID())
+ {
+ UAVCAN_ASSERT(dtd != UAVCAN_NULLPTR);
+ }
+ bool operator()(const Entry* entry) const
+ {
+ UAVCAN_ASSERT(entry != UAVCAN_NULLPTR);
+ return entry->descriptor.getID() > id;
+ }
+ };
+
+public:
+ /**
+ * Result of data type registration
+ */
+ enum RegistrationResult
+ {
+ RegistrationResultOk, ///< Success, data type is now registered and can be used.
+ RegistrationResultCollision, ///< Data type name or ID is not unique.
+ RegistrationResultInvalidParams, ///< Invalid input parameters.
+ RegistrationResultFrozen ///< Data Type Registery has been frozen and can't be modified anymore.
+ };
+
+private:
+ typedef LinkedListRoot<Entry> List;
+ mutable List msgs_;
+ mutable List srvs_;
+ bool frozen_;
+
+ GlobalDataTypeRegistry() : frozen_(false) { }
+
+ List* selectList(DataTypeKind kind) const;
+
+ RegistrationResult remove(Entry* dtd);
+ RegistrationResult registImpl(Entry* dtd);
+
+public:
+ /**
+ * Returns the reference to the singleton.
+ */
+ static GlobalDataTypeRegistry& instance();
+
+ /**
+ * Register a data type 'Type' with ID 'id'.
+ * If this data type was registered earlier, its old registration will be overridden.
+ * This method will fail if the data type registry is frozen.
+ *
+ * @tparam Type Autogenerated UAVCAN data type to register. Data types are generated by the
+ * libuavcan DSDL compiler from DSDL definitions.
+ *
+ * @param id Data Type ID for this data type.
+ */
+ template <typename Type>
+ RegistrationResult registerDataType(DataTypeID id);
+
+ /**
+ * Data Type registry needs to be frozen before a node instance can use it in
+ * order to prevent accidental change in data type configuration on a running
+ * node.
+ *
+ * This method will be called automatically by the node during start up, so
+ * the user does not need to call it from the application manually. Subsequent
+ * calls will not have any effect.
+ *
+ * Once frozen, data type registry can't be unfrozen.
+ */
+ void freeze();
+ bool isFrozen() const { return frozen_; }
+
+ /**
+ * Finds data type descriptor by full data type name, e.g. "uavcan.protocol.NodeStatus".
+ * Messages are searched first, then services.
+ * Returns null pointer if the data type with this name is not registered.
+ * @param name Full data type name
+ * @return Descriptor for this data type or null pointer if not found
+ */
+ const DataTypeDescriptor* find(const char* name) const;
+
+ /**
+ * Finds data type descriptor by full data type name, e.g. "uavcan.protocol.NodeStatus", and data type kind.
+ * Returns null pointer if the data type with this name is not registered.
+ * @param kind Data Type Kind - message or service
+ * @param name Full data type name
+ * @return Descriptor for this data type or null pointer if not found
+ */
+ const DataTypeDescriptor* find(DataTypeKind kind, const char* name) const;
+
+ /**
+ * Finds data type descriptor by data type ID.
+ * Returns null pointer if the data type with this ID is not registered.
+ * @param kind Data Type Kind - message or service
+ * @param dtid Data Type ID
+ * @return Descriptor for this data type or null pointer if not found
+ */
+ const DataTypeDescriptor* find(DataTypeKind kind, DataTypeID dtid) const;
+
+ /**
+ * Returns the number of registered message types.
+ */
+ unsigned getNumMessageTypes() const { return msgs_.getLength(); }
+
+ /**
+ * Returns the number of registered service types.
+ */
+ unsigned getNumServiceTypes() const { return srvs_.getLength(); }
+
+#if UAVCAN_DEBUG
+ /// Required for unit testing
+ void reset()
+ {
+ UAVCAN_TRACE("GlobalDataTypeRegistry", "Reset; was frozen: %i, num msgs: %u, num srvs: %u",
+ int(frozen_), getNumMessageTypes(), getNumServiceTypes());
+ frozen_ = false;
+ while (msgs_.get())
+ {
+ msgs_.remove(msgs_.get());
+ }
+ while (srvs_.get())
+ {
+ srvs_.remove(srvs_.get());
+ }
+ }
+#endif
+};
+
+/**
+ * This class is used by autogenerated data types to register with the
+ * data type registry automatically before main() is called. Note that
+ * if a generated data type header file is not included by any translation
+ * unit of the application, the data type will not be registered.
+ *
+ * Data type needs to have a default ID to be registrable by this class.
+ */
+template <typename Type>
+struct UAVCAN_EXPORT DefaultDataTypeRegistrator
+{
+ DefaultDataTypeRegistrator()
+ {
+#if !UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY
+ const GlobalDataTypeRegistry::RegistrationResult res =
+ GlobalDataTypeRegistry::instance().registerDataType<Type>(Type::DefaultDataTypeID);
+
+ if (res != GlobalDataTypeRegistry::RegistrationResultOk)
+ {
+ handleFatalError("Type reg failed");
+ }
+#endif
+ }
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * GlobalDataTypeRegistry
+ */
+template <typename Type>
+GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::registerDataType(DataTypeID id)
+{
+ if (isFrozen())
+ {
+ return RegistrationResultFrozen;
+ }
+
+ static Entry entry;
+
+ {
+ const RegistrationResult remove_res = remove(&entry);
+ if (remove_res != RegistrationResultOk)
+ {
+ return remove_res;
+ }
+ }
+
+ // We can't just overwrite the entry itself because it's noncopyable
+ entry.descriptor = DataTypeDescriptor(DataTypeKind(Type::DataTypeKind), id,
+ Type::getDataTypeSignature(), Type::getDataTypeFullName());
+
+ {
+ const RegistrationResult remove_res = remove(&entry);
+ if (remove_res != RegistrationResultOk)
+ {
+ return remove_res;
+ }
+ }
+ return registImpl(&entry);
+}
+
+}
+
+#endif // UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/node.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,319 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_NODE_HPP_INCLUDED
+#define UAVCAN_NODE_NODE_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/error.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/abstract_node.hpp>
+
+// High-level functionality available by default
+#include <uavcan/protocol/node_status_provider.hpp>
+
+#if !UAVCAN_TINY
+# include <uavcan/protocol/data_type_info_provider.hpp>
+# include <uavcan/protocol/logger.hpp>
+# include <uavcan/protocol/restart_request_server.hpp>
+# include <uavcan/protocol/transport_stats_provider.hpp>
+#endif
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+namespace uavcan
+{
+/**
+ * This is the top-level node API.
+ * A custom node class can be implemented if needed, in which case it shall inherit INode.
+ *
+ * @tparam MemPoolSize Size of memory pool for this node, in bytes.
+ * Please refer to the documentation for details.
+ * If this value is zero, the constructor will accept a reference to user-provided allocator.
+ */
+template <std::size_t MemPoolSize = 0>
+class UAVCAN_EXPORT Node : public INode
+{
+ typedef typename
+ Select<(MemPoolSize > 0),
+ PoolAllocator<MemPoolSize, MemPoolBlockSize>, // If pool size is specified, use default allocator
+ IPoolAllocator& // Otherwise use reference to user-provided allocator
+ >::Result Allocator;
+
+ Allocator pool_allocator_;
+ Scheduler scheduler_;
+
+ NodeStatusProvider proto_nsp_;
+#if !UAVCAN_TINY
+ DataTypeInfoProvider proto_dtp_;
+ Logger proto_logger_;
+ RestartRequestServer proto_rrs_;
+ TransportStatsProvider proto_tsp_;
+#endif
+
+ uint64_t internal_failure_cnt_;
+ bool started_;
+
+ void commonInit()
+ {
+ internal_failure_cnt_ = 0;
+ started_ = false;
+ }
+
+protected:
+ virtual void registerInternalFailure(const char* msg)
+ {
+ internal_failure_cnt_++;
+ UAVCAN_TRACE("Node", "Internal failure: %s", msg);
+#if UAVCAN_TINY
+ (void)msg;
+#else
+ (void)getLogger().log(protocol::debug::LogLevel::ERROR, "UAVCAN", msg);
+#endif
+ }
+
+public:
+ /**
+ * This overload is only valid if MemPoolSize > 0.
+ */
+ Node(ICanDriver& can_driver,
+ ISystemClock& system_clock) :
+ scheduler_(can_driver, pool_allocator_, system_clock),
+ proto_nsp_(*this)
+#if !UAVCAN_TINY
+ , proto_dtp_(*this)
+ , proto_logger_(*this)
+ , proto_rrs_(*this)
+ , proto_tsp_(*this)
+#endif
+ {
+ commonInit();
+ }
+
+ /**
+ * This overload is only valid if MemPoolSize == 0.
+ */
+ Node(ICanDriver& can_driver,
+ ISystemClock& system_clock,
+ IPoolAllocator& allocator) :
+ pool_allocator_(allocator),
+ scheduler_(can_driver, pool_allocator_, system_clock),
+ proto_nsp_(*this)
+#if !UAVCAN_TINY
+ , proto_dtp_(*this)
+ , proto_logger_(*this)
+ , proto_rrs_(*this)
+ , proto_tsp_(*this)
+#endif
+ {
+ commonInit();
+ }
+
+ virtual typename RemoveReference<Allocator>::Type& getAllocator() { return pool_allocator_; }
+
+ virtual Scheduler& getScheduler() { return scheduler_; }
+ virtual const Scheduler& getScheduler() const { return scheduler_; }
+
+ int spin(MonotonicTime deadline)
+ {
+ if (started_)
+ {
+ return INode::spin(deadline);
+ }
+ return -ErrNotInited;
+ }
+
+ int spin(MonotonicDuration duration)
+ {
+ if (started_)
+ {
+ return INode::spin(duration);
+ }
+ return -ErrNotInited;
+ }
+
+ int spinOnce()
+ {
+ if (started_)
+ {
+ return INode::spinOnce();
+ }
+ return -ErrNotInited;
+ }
+
+ bool isStarted() const { return started_; }
+
+ uint64_t getInternalFailureCount() const { return internal_failure_cnt_; }
+
+ /**
+ * Starts the node and publishes uavcan.protocol.NodeStatus immediately.
+ * Does not so anything if the node is already started.
+ * Once started, the node can't stop.
+ * If the node failed to start up, it's recommended to destroy the current node instance and start over.
+ * Returns negative error code.
+ * @param node_status_transfer_priority Transfer priority that will be used for outgoing NodeStatus messages.
+ * Normal priority is used by default.
+ */
+ int start(const TransferPriority node_status_transfer_priority = TransferPriority::Default);
+
+ /**
+ * Gets/sets the node name, e.g. "com.example.product_name". The node name can be set only once.
+ * The name must be set before the node is started, otherwise the node will refuse to start up.
+ */
+ const NodeStatusProvider::NodeName& getName() const { return proto_nsp_.getName(); }
+ void setName(const NodeStatusProvider::NodeName& name) { proto_nsp_.setName(name); }
+
+ /**
+ * Node health code helpers.
+ */
+ void setHealthOk() { proto_nsp_.setHealthOk(); }
+ void setHealthWarning() { proto_nsp_.setHealthWarning(); }
+ void setHealthError() { proto_nsp_.setHealthError(); }
+ void setHealthCritical() { proto_nsp_.setHealthCritical(); }
+
+ /**
+ * Node mode code helpers.
+ * Note that INITIALIZATION is the default mode; the application has to manually set it to OPERATIONAL.
+ */
+ void setModeOperational() { proto_nsp_.setModeOperational(); }
+ void setModeInitialization() { proto_nsp_.setModeInitialization(); }
+ void setModeMaintenance() { proto_nsp_.setModeMaintenance(); }
+ void setModeSoftwareUpdate() { proto_nsp_.setModeSoftwareUpdate(); }
+
+ void setModeOfflineAndPublish()
+ {
+ proto_nsp_.setModeOffline();
+ (void)proto_nsp_.forcePublish();
+ }
+
+ /**
+ * Updates the vendor-specific status code.
+ */
+ void setVendorSpecificStatusCode(NodeStatusProvider::VendorSpecificStatusCode code)
+ {
+ proto_nsp_.setVendorSpecificStatusCode(code);
+ }
+
+ /**
+ * Gets/sets the node version information.
+ */
+ void setSoftwareVersion(const protocol::SoftwareVersion& version) { proto_nsp_.setSoftwareVersion(version); }
+ void setHardwareVersion(const protocol::HardwareVersion& version) { proto_nsp_.setHardwareVersion(version); }
+
+ const protocol::SoftwareVersion& getSoftwareVersion() const { return proto_nsp_.getSoftwareVersion(); }
+ const protocol::HardwareVersion& getHardwareVersion() const { return proto_nsp_.getHardwareVersion(); }
+
+ NodeStatusProvider& getNodeStatusProvider() { return proto_nsp_; }
+
+#if !UAVCAN_TINY
+ /**
+ * Restart handler can be installed to handle external node restart requests (highly recommended).
+ */
+ void setRestartRequestHandler(IRestartRequestHandler* handler) { proto_rrs_.setHandler(handler); }
+
+ RestartRequestServer& getRestartRequestServer() { return proto_rrs_; }
+
+ /**
+ * Node logging.
+ * Logging calls are passed directly into the @ref Logger instance.
+ * Type safe log formatting is supported only in C++11 mode.
+ * @{
+ */
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+ template <typename... Args>
+ inline void logDebug(const char* source, const char* format, Args... args)
+ {
+ (void)proto_logger_.logDebug(source, format, args...);
+ }
+
+ template <typename... Args>
+ inline void logInfo(const char* source, const char* format, Args... args)
+ {
+ (void)proto_logger_.logInfo(source, format, args...);
+ }
+
+ template <typename... Args>
+ inline void logWarning(const char* source, const char* format, Args... args)
+ {
+ (void)proto_logger_.logWarning(source, format, args...);
+ }
+
+ template <typename... Args>
+ inline void logError(const char* source, const char* format, Args... args)
+ {
+ (void)proto_logger_.logError(source, format, args...);
+ }
+
+#else
+
+ void logDebug(const char* source, const char* text) { (void)proto_logger_.logDebug(source, text); }
+ void logInfo(const char* source, const char* text) { (void)proto_logger_.logInfo(source, text); }
+ void logWarning(const char* source, const char* text) { (void)proto_logger_.logWarning(source, text); }
+ void logError(const char* source, const char* text) { (void)proto_logger_.logError(source, text); }
+
+#endif
+ /**
+ * @}
+ */
+
+ /**
+ * Use this method to configure logging.
+ */
+ Logger& getLogger() { return proto_logger_; }
+
+#endif // UAVCAN_TINY
+};
+
+// ----------------------------------------------------------------------------
+
+template <std::size_t MemPoolSize_>
+int Node<MemPoolSize_>::start(const TransferPriority priority)
+{
+ if (started_)
+ {
+ return 0;
+ }
+ GlobalDataTypeRegistry::instance().freeze();
+
+ int res = 0;
+ res = proto_nsp_.startAndPublish(priority);
+ if (res < 0)
+ {
+ goto fail;
+ }
+#if !UAVCAN_TINY
+ res = proto_dtp_.start();
+ if (res < 0)
+ {
+ goto fail;
+ }
+ res = proto_logger_.init();
+ if (res < 0)
+ {
+ goto fail;
+ }
+ res = proto_rrs_.start();
+ if (res < 0)
+ {
+ goto fail;
+ }
+ res = proto_tsp_.start();
+ if (res < 0)
+ {
+ goto fail;
+ }
+#endif
+ started_ = res >= 0;
+ return res;
+fail:
+ UAVCAN_ASSERT(res < 0);
+ return res;
+}
+
+}
+
+#endif // UAVCAN_NODE_NODE_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/publisher.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_PUBLISHER_HPP_INCLUDED
+#define UAVCAN_NODE_PUBLISHER_HPP_INCLUDED
+
+#include <uavcan/node/generic_publisher.hpp>
+
+namespace uavcan
+{
+/**
+ * Use this class to publish messages to the bus (broadcast, unicast, or both).
+ *
+ * @tparam DataType_ Message data type
+ */
+template <typename DataType_>
+class UAVCAN_EXPORT Publisher : protected GenericPublisher<DataType_, DataType_>
+{
+ typedef GenericPublisher<DataType_, DataType_> BaseType;
+
+public:
+ typedef DataType_ DataType; ///< Message data type
+
+ /**
+ * @param node Node instance this publisher will be registered with.
+ *
+ * @param tx_timeout If CAN frames of this message are not delivered to the bus
+ * in this amount of time, they will be discarded. Default value
+ * is good enough for rather high-frequency, high-priority messages.
+ *
+ * @param max_transfer_interval Maximum expected transfer interval. It's absolutely safe
+ * to leave default value here. It just defines how soon the
+ * Transfer ID tracking objects associated with this message type
+ * will be garbage collected by the library if it's no longer
+ * being published.
+ */
+ explicit Publisher(INode& node, MonotonicDuration tx_timeout = getDefaultTxTimeout(),
+ MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
+ : BaseType(node, tx_timeout, max_transfer_interval)
+ {
+#if UAVCAN_DEBUG
+ UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK
+#endif
+ StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
+ }
+
+ /**
+ * Broadcast the message.
+ * Returns negative error code.
+ */
+ int broadcast(const DataType& message)
+ {
+ return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast);
+ }
+
+ /**
+ * Warning: You probably don't want to use this method; it's for advanced use cases like
+ * e.g. network time synchronization. Use the overloaded method with fewer arguments instead.
+ * This overload allows to explicitly specify Transfer ID.
+ * Returns negative error code.
+ */
+ int broadcast(const DataType& message, TransferID tid)
+ {
+ return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast, tid);
+ }
+
+ static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(100); }
+
+ /**
+ * Init method can be called prior first publication, but it's not necessary
+ * because the publisher can be automatically initialized ad-hoc.
+ */
+ using BaseType::init;
+
+ using BaseType::allowAnonymousTransfers;
+ using BaseType::getTransferSender;
+ using BaseType::getMinTxTimeout;
+ using BaseType::getMaxTxTimeout;
+ using BaseType::getTxTimeout;
+ using BaseType::setTxTimeout;
+ using BaseType::getPriority;
+ using BaseType::setPriority;
+ using BaseType::getNode;
+};
+
+}
+
+#endif // UAVCAN_NODE_PUBLISHER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/scheduler.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_SCHEDULER_HPP_INCLUDED
+#define UAVCAN_NODE_SCHEDULER_HPP_INCLUDED
+
+#include <uavcan/error.hpp>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/transport/dispatcher.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT Scheduler;
+
+class UAVCAN_EXPORT DeadlineHandler : public LinkedListNode<DeadlineHandler>
+{
+ MonotonicTime deadline_;
+
+protected:
+ Scheduler& scheduler_;
+
+ explicit DeadlineHandler(Scheduler& scheduler)
+ : scheduler_(scheduler)
+ { }
+
+ virtual ~DeadlineHandler() { stop(); }
+
+public:
+ virtual void handleDeadline(MonotonicTime current) = 0;
+
+ void startWithDeadline(MonotonicTime deadline);
+ void startWithDelay(MonotonicDuration delay);
+ void generateDeadlineImmediately() { startWithDeadline(MonotonicTime::fromUSec(1)); }
+
+ void stop();
+
+ bool isRunning() const;
+
+ MonotonicTime getDeadline() const { return deadline_; }
+ Scheduler& getScheduler() const { return scheduler_; }
+};
+
+
+class UAVCAN_EXPORT DeadlineScheduler : Noncopyable
+{
+ LinkedListRoot<DeadlineHandler> handlers_; // Ordered by deadline, lowest first
+
+public:
+ void add(DeadlineHandler* mdh);
+ void remove(DeadlineHandler* mdh);
+ bool doesExist(const DeadlineHandler* mdh) const;
+ unsigned getNumHandlers() const { return handlers_.getLength(); }
+
+ MonotonicTime pollAndGetMonotonicTime(ISystemClock& sysclock);
+ MonotonicTime getEarliestDeadline() const;
+};
+
+/**
+ * This class distributes processing time between library components (IO handling, deadline callbacks, ...).
+ */
+class UAVCAN_EXPORT Scheduler : Noncopyable
+{
+ enum { DefaultDeadlineResolutionMs = 5 };
+ enum { MinDeadlineResolutionMs = 1 };
+ enum { MaxDeadlineResolutionMs = 100 };
+
+ enum { DefaultCleanupPeriodMs = 1000 };
+ enum { MinCleanupPeriodMs = 10 };
+ enum { MaxCleanupPeriodMs = 10000 };
+
+ DeadlineScheduler deadline_scheduler_;
+ Dispatcher dispatcher_;
+ MonotonicTime prev_cleanup_ts_;
+ MonotonicDuration deadline_resolution_;
+ MonotonicDuration cleanup_period_;
+ bool inside_spin_;
+
+ struct InsideSpinSetter
+ {
+ Scheduler& owner;
+ InsideSpinSetter(Scheduler& o)
+ : owner(o)
+ {
+ owner.inside_spin_ = true;
+ }
+ ~InsideSpinSetter() { owner.inside_spin_ = false; }
+ };
+
+ MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const;
+ void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin);
+
+public:
+ Scheduler(ICanDriver& can_driver, IPoolAllocator& allocator, ISystemClock& sysclock)
+ : dispatcher_(can_driver, allocator, sysclock)
+ , prev_cleanup_ts_(sysclock.getMonotonic())
+ , deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs))
+ , cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs))
+ , inside_spin_(false)
+ { }
+
+ /**
+ * Spin until the deadline, or until some error occurs.
+ * This function will return strictly when the deadline is reached, even if there are unprocessed frames.
+ * Returns negative error code.
+ */
+ int spin(MonotonicTime deadline);
+
+ /**
+ * Non-blocking version of @ref spin() - spins until all pending frames and events are processed,
+ * or until some error occurs. If there's nothing to do, returns immediately.
+ * Returns negative error code.
+ */
+ int spinOnce();
+
+ DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; }
+
+ Dispatcher& getDispatcher() { return dispatcher_; }
+ const Dispatcher& getDispatcher() const { return dispatcher_; }
+
+ ISystemClock& getSystemClock() { return dispatcher_.getSystemClock(); }
+ MonotonicTime getMonotonicTime() const { return dispatcher_.getSystemClock().getMonotonic(); }
+ UtcTime getUtcTime() const { return dispatcher_.getSystemClock().getUtc(); }
+
+ /**
+ * Worst case deadline callback resolution.
+ * Higher resolution increases CPU usage.
+ */
+ MonotonicDuration getDeadlineResolution() const { return deadline_resolution_; }
+ void setDeadlineResolution(MonotonicDuration res)
+ {
+ res = min(res, MonotonicDuration::fromMSec(MaxDeadlineResolutionMs));
+ res = max(res, MonotonicDuration::fromMSec(MinDeadlineResolutionMs));
+ deadline_resolution_ = res;
+ }
+
+ /**
+ * How often the scheduler will run cleanup (listeners, outgoing transfer registry, ...).
+ * Cleanup execution time grows linearly with number of listeners and number of items
+ * in the Outgoing Transfer ID registry.
+ * Lower period increases CPU usage.
+ */
+ MonotonicDuration getCleanupPeriod() const { return cleanup_period_; }
+ void setCleanupPeriod(MonotonicDuration period)
+ {
+ period = min(period, MonotonicDuration::fromMSec(MaxCleanupPeriodMs));
+ period = max(period, MonotonicDuration::fromMSec(MinCleanupPeriodMs));
+ cleanup_period_ = period;
+ }
+};
+
+}
+
+#endif // UAVCAN_NODE_SCHEDULER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/service_client.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,585 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
+#define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/multiset.hpp>
+#include <uavcan/node/generic_publisher.hpp>
+#include <uavcan/node/generic_subscriber.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <functional>
+#endif
+
+namespace uavcan
+{
+/**
+ * This struct describes a pending service call.
+ * Refer to @ref ServiceClient to learn more about service calls.
+ */
+struct ServiceCallID
+{
+ NodeID server_node_id;
+ TransferID transfer_id;
+
+ ServiceCallID() { }
+
+ ServiceCallID(NodeID arg_server_node_id, TransferID arg_transfer_id)
+ : server_node_id(arg_server_node_id)
+ , transfer_id(arg_transfer_id)
+ { }
+
+ bool operator==(const ServiceCallID rhs) const
+ {
+ return (rhs.server_node_id == server_node_id) &&
+ (rhs.transfer_id == transfer_id);
+ }
+
+ bool isValid() const { return server_node_id.isUnicast(); }
+};
+
+/**
+ * Object of this type will be returned to the application as a result of service call.
+ * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason.
+ * The class is made noncopyable because it keeps a reference to a stack-allocated object.
+ */
+template <typename DataType>
+class UAVCAN_EXPORT ServiceCallResult : Noncopyable
+{
+public:
+ typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
+
+ enum Status { Success, ErrorTimeout };
+
+private:
+ const Status status_; ///< Whether successful or not. Failure to decode the response causes timeout.
+ ServiceCallID call_id_; ///< Identifies the call
+ ResponseFieldType& response_; ///< Returned data structure. Value undefined if the service call has failed.
+
+public:
+ ServiceCallResult(Status arg_status, ServiceCallID arg_call_id, ResponseFieldType& arg_response)
+ : status_(arg_status)
+ , call_id_(arg_call_id)
+ , response_(arg_response)
+ {
+ UAVCAN_ASSERT(call_id_.isValid());
+ UAVCAN_ASSERT((status_ == Success) || (status_ == ErrorTimeout));
+ }
+
+ /**
+ * Shortcut to quickly check whether the call was successful.
+ */
+ bool isSuccessful() const { return status_ == Success; }
+
+ Status getStatus() const { return status_; }
+
+ ServiceCallID getCallID() const { return call_id_; }
+
+ /**
+ * Returned reference points to a stack-allocated object.
+ */
+ const ResponseFieldType& getResponse() const { return response_; }
+ ResponseFieldType& getResponse() { return response_; }
+};
+
+/**
+ * This operator neatly prints the service call result prepended with extra data like Server Node ID.
+ * The extra data will be represented as YAML comment.
+ */
+template <typename Stream, typename DataType>
+static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
+{
+ s << "# Service call result [" << DataType::getDataTypeFullName() << "] "
+ << (scr.isSuccessful() ? "OK" : "FAILURE")
+ << " server_node_id=" << int(scr.getCallID().server_node_id.get())
+ << " tid=" << int(scr.getCallID().transfer_id.get()) << "\n";
+ if (scr.isSuccessful())
+ {
+ s << scr.getResponse();
+ }
+ else
+ {
+ s << "# (no data)";
+ }
+ return s;
+}
+
+/**
+ * Do not use directly.
+ */
+class ServiceClientBase : protected ITransferAcceptanceFilter
+ , protected DeadlineHandler
+{
+ const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call
+
+protected:
+ class CallState : DeadlineHandler
+ {
+ ServiceClientBase& owner_;
+ const ServiceCallID id_;
+ bool timed_out_;
+
+ virtual void handleDeadline(MonotonicTime);
+
+ public:
+ CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id)
+ : DeadlineHandler(node.getScheduler())
+ , owner_(owner)
+ , id_(call_id)
+ , timed_out_(false)
+ {
+ UAVCAN_ASSERT(id_.isValid());
+ DeadlineHandler::startWithDelay(owner_.request_timeout_);
+ }
+
+ ServiceCallID getCallID() const { return id_; }
+
+ bool hasTimedOut() const { return timed_out_; }
+
+ static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); }
+
+ bool operator==(const CallState& rhs) const
+ {
+ return (&owner_ == &rhs.owner_) && (id_ == rhs.id_);
+ }
+ };
+
+ struct CallStateMatchingPredicate
+ {
+ const ServiceCallID id;
+ CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { }
+ bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); }
+ };
+
+ struct ServerSearchPredicate
+ {
+ const NodeID server_node_id;
+ ServerSearchPredicate(NodeID nid) : server_node_id(nid) { }
+ bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; }
+ };
+
+ MonotonicDuration request_timeout_;
+
+ ServiceClientBase(INode& node)
+ : DeadlineHandler(node.getScheduler())
+ , data_type_descriptor_(UAVCAN_NULLPTR)
+ , request_timeout_(getDefaultRequestTimeout())
+ { }
+
+ virtual ~ServiceClientBase() { }
+
+ int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id);
+
+public:
+ /**
+ * It's not recommended to override default timeouts.
+ * Change of this value will not affect pending calls.
+ */
+ static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); }
+ static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); }
+ static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); }
+};
+
+/**
+ * Use this class to invoke services on remote nodes.
+ *
+ * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent
+ * calls is limited only by amount of available pool memory.
+ *
+ * Note that the reference passed to the callback points to a stack-allocated object, which means that the
+ * reference invalidates once the callback returns. If you want to use this object after the callback execution,
+ * you need to copy it somewhere.
+ *
+ * Note that by default, service client objects use lower priority than message publishers. Use @ref setPriority()
+ * to override the default if necessary.
+ *
+ * @tparam DataType_ Service data type.
+ *
+ * @tparam Callback_ Service response will be delivered through the callback of this type.
+ * In C++11 mode this type defaults to std::function<>.
+ * In C++03 mode this type defaults to a plain function pointer; use binder to
+ * call member functions as callbacks.
+ */
+template <typename DataType_,
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ typename Callback_ = std::function<void (const ServiceCallResult<DataType_>&)>
+#else
+ typename Callback_ = void (*)(const ServiceCallResult<DataType_>&)
+#endif
+ >
+class UAVCAN_EXPORT ServiceClient
+ : public GenericSubscriber<DataType_,
+ typename DataType_::Response, TransferListenerWithFilter>
+ , public ServiceClientBase
+{
+public:
+ typedef DataType_ DataType;
+ typedef typename DataType::Request RequestType;
+ typedef typename DataType::Response ResponseType;
+ typedef ServiceCallResult<DataType> ServiceCallResultType;
+ typedef Callback_ Callback;
+
+private:
+ typedef ServiceClient<DataType, Callback> SelfType;
+ typedef GenericPublisher<DataType, RequestType> PublisherType;
+ typedef GenericSubscriber<DataType, ResponseType, TransferListenerWithFilter> SubscriberType;
+
+ typedef Multiset<CallState> CallRegistry;
+
+ struct TimeoutCallbackCaller
+ {
+ ServiceClient& owner;
+
+ TimeoutCallbackCaller(ServiceClient& arg_owner) : owner(arg_owner) { }
+
+ void operator()(const CallState& state)
+ {
+ if (state.hasTimedOut())
+ {
+ UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s",
+ int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()),
+ DataType::getDataTypeFullName());
+
+ typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized
+
+ ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(),
+ rx_struct); // Mutable!
+
+ owner.invokeCallback(result);
+ }
+ }
+ };
+
+ CallRegistry call_registry_;
+
+ PublisherType publisher_;
+ Callback callback_;
+
+ virtual bool shouldAcceptFrame(const RxFrame& frame) const; // Called from the transfer listener
+
+ void invokeCallback(ServiceCallResultType& result);
+
+ virtual void handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response);
+
+ virtual void handleDeadline(MonotonicTime);
+
+ int addCallState(ServiceCallID call_id);
+
+public:
+ /**
+ * @param node Node instance this client will be registered with.
+ * @param callback Callback instance. Optional, can be assigned later.
+ */
+ explicit ServiceClient(INode& node, const Callback& callback = Callback())
+ : SubscriberType(node)
+ , ServiceClientBase(node)
+ , call_registry_(node.getAllocator())
+ , publisher_(node, getDefaultRequestTimeout())
+ , callback_(callback)
+ {
+ setPriority(TransferPriority::MiddleLower);
+ setRequestTimeout(getDefaultRequestTimeout());
+#if UAVCAN_DEBUG
+ UAVCAN_ASSERT(getRequestTimeout() == getDefaultRequestTimeout()); // Making sure default values are OK
+#endif
+ }
+
+ virtual ~ServiceClient() { cancelAllCalls(); }
+
+ /**
+ * Shall be called before first use.
+ * Returns negative error code.
+ */
+ int init()
+ {
+ return publisher_.init();
+ }
+
+ /**
+ * Shall be called before first use.
+ * This overload allows to set the priority, otherwise it's the same.
+ * Returns negative error code.
+ */
+ int init(TransferPriority priority)
+ {
+ return publisher_.init(priority);
+ }
+
+ /**
+ * Performs non-blocking service call.
+ * This method transmits the service request and returns immediately.
+ *
+ * Service response will be delivered into the application via the callback.
+ * Note that the callback will ALWAYS be called even if the service call times out; the
+ * actual result of the call (success/failure) will be passed to the callback as well.
+ *
+ * Returns negative error code.
+ */
+ int call(NodeID server_node_id, const RequestType& request);
+
+ /**
+ * Same as plain @ref call() above, but this overload also returns the call ID of the new call.
+ * The call ID structure can be used to cancel this request later if needed.
+ */
+ int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id);
+
+ /**
+ * Cancels certain call referred via call ID structure.
+ */
+ void cancelCall(ServiceCallID call_id);
+
+ /**
+ * Cancels all pending calls.
+ */
+ void cancelAllCalls();
+
+ /**
+ * Checks whether there's currently a pending call addressed to the specified node ID.
+ */
+ bool hasPendingCallToServer(NodeID server_node_id) const;
+
+ /**
+ * This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned.
+ * Warning: average complexity is O(index); worst case complexity is O(size).
+ */
+ ServiceCallID getCallIDByIndex(unsigned index) const;
+
+ /**
+ * Service response callback must be set prior service call.
+ */
+ const Callback& getCallback() const { return callback_; }
+ void setCallback(const Callback& cb) { callback_ = cb; }
+
+ /**
+ * Complexity is O(N) of number of pending calls.
+ * Note that the number of pending calls will not be updated until the callback is executed.
+ */
+ unsigned getNumPendingCalls() const { return call_registry_.getSize(); }
+
+ /**
+ * Complexity is O(1).
+ * Note that the number of pending calls will not be updated until the callback is executed.
+ */
+ bool hasPendingCalls() const { return !call_registry_.isEmpty(); }
+
+ /**
+ * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either:
+ * - Transient failure in the transport layer.
+ * - Incompatible data types.
+ */
+ uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); }
+
+ /**
+ * Request timeouts. Note that changing the request timeout will not affect calls that are already pending.
+ * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts.
+ * Not recommended to change.
+ */
+ MonotonicDuration getRequestTimeout() const { return request_timeout_; }
+ void setRequestTimeout(MonotonicDuration timeout)
+ {
+ timeout = max(timeout, getMinRequestTimeout());
+ timeout = min(timeout, getMaxRequestTimeout());
+
+ publisher_.setTxTimeout(timeout);
+ request_timeout_ = max(timeout, publisher_.getTxTimeout()); // No less than TX timeout
+ }
+
+ /**
+ * Priority of outgoing request transfers.
+ * The remote server is supposed to use the same priority for the response, but it's not guaranteed by
+ * the specification.
+ */
+ TransferPriority getPriority() const { return publisher_.getPriority(); }
+ void setPriority(const TransferPriority prio) { publisher_.setPriority(prio); }
+};
+
+// ----------------------------------------------------------------------------
+
+template <typename DataType_, typename Callback_>
+void ServiceClient<DataType_, Callback_>::invokeCallback(ServiceCallResultType& result)
+{
+ if (coerceOrFallback<bool>(callback_, true))
+ {
+ callback_(result);
+ }
+ else
+ {
+ handleFatalError("Srv client clbk");
+ }
+}
+
+template <typename DataType_, typename Callback_>
+bool ServiceClient<DataType_, Callback_>::shouldAcceptFrame(const RxFrame& frame) const
+{
+ UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher
+
+ return UAVCAN_NULLPTR != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(),
+ frame.getTransferID())));
+
+}
+
+template <typename DataType_, typename Callback_>
+void ServiceClient<DataType_, Callback_>::handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
+{
+ UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse);
+
+ ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID());
+ cancelCall(call_id);
+ ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable!
+ invokeCallback(result);
+}
+
+
+template <typename DataType_, typename Callback_>
+void ServiceClient<DataType_, Callback_>::handleDeadline(MonotonicTime)
+{
+ UAVCAN_TRACE("ServiceClient", "Shared deadline event received");
+ /*
+ * Invoking callbacks for timed out call state objects.
+ */
+ TimeoutCallbackCaller callback_caller(*this);
+ call_registry_.template forEach<TimeoutCallbackCaller&>(callback_caller);
+ /*
+ * Removing timed out objects.
+ * This operation cannot be merged with the previous one because that will not work with recursive calls.
+ */
+ call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate);
+ /*
+ * Subscriber does not need to be registered if we don't have any pending calls.
+ * Removing it makes processing of incoming frames a bit faster.
+ */
+ if (call_registry_.isEmpty())
+ {
+ SubscriberType::stop();
+ }
+}
+
+template <typename DataType_, typename Callback_>
+int ServiceClient<DataType_, Callback_>::addCallState(ServiceCallID call_id)
+{
+ if (call_registry_.isEmpty())
+ {
+ const int subscriber_res = SubscriberType::startAsServiceResponseListener();
+ if (subscriber_res < 0)
+ {
+ UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
+ return subscriber_res;
+ }
+ }
+
+ if (UAVCAN_NULLPTR == call_registry_.template emplace<INode&, ServiceClientBase&,
+ ServiceCallID>(SubscriberType::getNode(), *this, call_id))
+ {
+ SubscriberType::stop();
+ return -ErrMemory;
+ }
+
+ return 0;
+}
+
+template <typename DataType_, typename Callback_>
+int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request)
+{
+ ServiceCallID dummy;
+ return call(server_node_id, request, dummy);
+}
+
+template <typename DataType_, typename Callback_>
+int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request,
+ ServiceCallID& out_call_id)
+{
+ if (!coerceOrFallback<bool>(callback_, true))
+ {
+ UAVCAN_TRACE("ServiceClient", "Invalid callback");
+ return -ErrInvalidConfiguration;
+ }
+
+ /*
+ * Common procedures that don't depend on the struct data type
+ */
+ const int prep_res =
+ prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id);
+ if (prep_res < 0)
+ {
+ UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res);
+ return prep_res;
+ }
+
+ /*
+ * Initializing the call state - this will start the subscriber ad-hoc
+ */
+ const int call_state_res = addCallState(out_call_id);
+ if (call_state_res < 0)
+ {
+ UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res);
+ return call_state_res;
+ }
+
+ /*
+ * Configuring the listener so it will accept only the matching responses
+ * TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder)
+ */
+ TransferListenerWithFilter* const tl = SubscriberType::getTransferListener();
+ if (tl == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0); // Must have been created
+ cancelCall(out_call_id);
+ return -ErrLogic;
+ }
+ tl->installAcceptanceFilter(this);
+
+ /*
+ * Publishing the request
+ */
+ const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id,
+ out_call_id.transfer_id);
+ if (publisher_res < 0)
+ {
+ cancelCall(out_call_id);
+ return publisher_res;
+ }
+
+ UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id);
+ return publisher_res;
+}
+
+template <typename DataType_, typename Callback_>
+void ServiceClient<DataType_, Callback_>::cancelCall(ServiceCallID call_id)
+{
+ call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id));
+ if (call_registry_.isEmpty())
+ {
+ SubscriberType::stop();
+ }
+}
+
+template <typename DataType_, typename Callback_>
+void ServiceClient<DataType_, Callback_>::cancelAllCalls()
+{
+ call_registry_.clear();
+ SubscriberType::stop();
+}
+
+template <typename DataType_, typename Callback_>
+bool ServiceClient<DataType_, Callback_>::hasPendingCallToServer(NodeID server_node_id) const
+{
+ return UAVCAN_NULLPTR != call_registry_.find(ServerSearchPredicate(server_node_id));
+}
+
+template <typename DataType_, typename Callback_>
+ServiceCallID ServiceClient<DataType_, Callback_>::getCallIDByIndex(unsigned index) const
+{
+ const CallState* const id = call_registry_.getByIndex(index);
+ return (id == UAVCAN_NULLPTR) ? ServiceCallID() : id->getCallID();
+}
+
+}
+
+#endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/service_server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED
+#define UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/generic_publisher.hpp>
+#include <uavcan/node/generic_subscriber.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <functional>
+#endif
+
+namespace uavcan
+{
+/**
+ * This type can be used in place of the response type in a service server callback to get more advanced control
+ * of service request processing.
+ *
+ * PLEASE NOTE that since this class inherits the response type, service server callbacks can accept either
+ * object of this class or the response type directly if the extra options are not needed.
+ *
+ * For example, both of these callbacks can be used with the same service type 'Foo':
+ *
+ * void first(const ReceivedDataStructure<Foo::Request>& request,
+ * ServiceResponseDataStructure<Foo::Response>& response);
+ *
+ * void second(const Foo::Request& request,
+ * Foo::Response& response);
+ *
+ * In the latter case, an implicit cast will happen before the callback is invoked.
+ */
+template <typename ResponseDataType_>
+class ServiceResponseDataStructure : public ResponseDataType_
+{
+ // Fields are weirdly named to avoid name clashing with the inherited data type
+ bool _enabled_;
+
+public:
+ typedef ResponseDataType_ ResponseDataType;
+
+ ServiceResponseDataStructure()
+ : _enabled_(true)
+ { }
+
+ /**
+ * When disabled, the server will not transmit the response transfer.
+ * By default it is enabled, i.e. response will be sent.
+ */
+ void setResponseEnabled(bool x) { _enabled_ = x; }
+
+ /**
+ * Whether the response will be sent. By default it will.
+ */
+ bool isResponseEnabled() const { return _enabled_; }
+};
+
+/**
+ * Use this class to implement UAVCAN service servers.
+ *
+ * Note that the references passed to the callback may point to stack-allocated objects, which means that the
+ * references get invalidated once the callback returns.
+ *
+ * @tparam DataType_ Service data type.
+ *
+ * @tparam Callback_ Service calls will be delivered through the callback of this type, and service
+ * response will be returned via the output parameter of the callback. Note that
+ * the reference to service response data struct passed to the callback always points
+ * to a default initialized response object.
+ * Please also refer to @ref ReceivedDataStructure<> and @ref ServiceResponseDataStructure<>.
+ * In C++11 mode this type defaults to std::function<>.
+ * In C++03 mode this type defaults to a plain function pointer; use binder to
+ * call member functions as callbacks.
+ */
+template <typename DataType_,
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ typename Callback_ = std::function<void (const ReceivedDataStructure<typename DataType_::Request>&,
+ ServiceResponseDataStructure<typename DataType_::Response>&)>
+#else
+ typename Callback_ = void (*)(const ReceivedDataStructure<typename DataType_::Request>&,
+ ServiceResponseDataStructure<typename DataType_::Response>&)
+#endif
+ >
+class UAVCAN_EXPORT ServiceServer
+ : public GenericSubscriber<DataType_, typename DataType_::Request, TransferListener>
+{
+public:
+ typedef DataType_ DataType;
+ typedef typename DataType::Request RequestType;
+ typedef typename DataType::Response ResponseType;
+ typedef Callback_ Callback;
+
+private:
+ typedef GenericSubscriber<DataType, RequestType, TransferListener> SubscriberType;
+ typedef GenericPublisher<DataType, ResponseType> PublisherType;
+
+ PublisherType publisher_;
+ Callback callback_;
+ uint32_t response_failure_count_;
+
+ virtual void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request)
+ {
+ UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest);
+
+ ServiceResponseDataStructure<ResponseType> response;
+
+ if (coerceOrFallback<bool>(callback_, true))
+ {
+ UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default
+ callback_(request, response);
+ }
+ else
+ {
+ handleFatalError("Srv serv clbk");
+ }
+
+ if (response.isResponseEnabled())
+ {
+ publisher_.setPriority(request.getPriority()); // Responding at the same priority.
+
+ const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(),
+ request.getTransferID());
+ if (res < 0)
+ {
+ UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res);
+ publisher_.getNode().getDispatcher().getTransferPerfCounter().addError();
+ response_failure_count_++;
+ }
+ }
+ else
+ {
+ UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application");
+ }
+ }
+
+public:
+ explicit ServiceServer(INode& node)
+ : SubscriberType(node)
+ , publisher_(node, getDefaultTxTimeout())
+ , callback_()
+ , response_failure_count_(0)
+ {
+ UAVCAN_ASSERT(getTxTimeout() == getDefaultTxTimeout()); // Making sure it is valid
+
+ StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindService>::check();
+ }
+
+ /**
+ * Starts the server.
+ * Incoming service requests will be passed to the application via the callback.
+ */
+ int start(const Callback& callback)
+ {
+ stop();
+
+ if (!coerceOrFallback<bool>(callback, true))
+ {
+ UAVCAN_TRACE("ServiceServer", "Invalid callback");
+ return -ErrInvalidParam;
+ }
+ callback_ = callback;
+
+ const int publisher_res = publisher_.init();
+ if (publisher_res < 0)
+ {
+ UAVCAN_TRACE("ServiceServer", "Publisher initialization failure: %i", publisher_res);
+ return publisher_res;
+ }
+ return SubscriberType::startAsServiceRequestListener();
+ }
+
+ /**
+ * Stops the server.
+ */
+ using SubscriberType::stop;
+
+ static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(1000); }
+ static MonotonicDuration getMinTxTimeout() { return PublisherType::getMinTxTimeout(); }
+ static MonotonicDuration getMaxTxTimeout() { return PublisherType::getMaxTxTimeout(); }
+
+ MonotonicDuration getTxTimeout() const { return publisher_.getTxTimeout(); }
+ void setTxTimeout(MonotonicDuration tx_timeout) { publisher_.setTxTimeout(tx_timeout); }
+
+ /**
+ * Returns the number of failed attempts to decode data structs. Generally, a failed attempt means either:
+ * - Transient failure in the transport layer.
+ * - Incompatible data types.
+ */
+ uint32_t getRequestFailureCount() const { return SubscriberType::getFailureCount(); }
+ uint32_t getResponseFailureCount() const { return response_failure_count_; }
+};
+
+}
+
+#endif // UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/sub_node.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_SUB_NODE_NODE_HPP_INCLUDED
+#define UAVCAN_SUB_NODE_NODE_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/abstract_node.hpp>
+
+#if UAVCAN_TINY
+# error "This functionality is not available in tiny mode"
+#endif
+
+namespace uavcan
+{
+/**
+ * This node object can be used in multiprocess UAVCAN nodes.
+ * Please refer to the @ref Node<> for documentation concerning the template arguments; refer to the tutorials
+ * to lean how to use libuavcan in multiprocess applications.
+ */
+template <std::size_t MemPoolSize = 0>
+class UAVCAN_EXPORT SubNode : public INode
+{
+ typedef typename
+ Select<(MemPoolSize > 0),
+ PoolAllocator<MemPoolSize, MemPoolBlockSize>, // If pool size is specified, use default allocator
+ IPoolAllocator& // Otherwise use reference to user-provided allocator
+ >::Result Allocator;
+
+ Allocator pool_allocator_;
+ Scheduler scheduler_;
+
+ uint64_t internal_failure_cnt_;
+
+protected:
+ virtual void registerInternalFailure(const char* msg)
+ {
+ internal_failure_cnt_++;
+ UAVCAN_TRACE("Node", "Internal failure: %s", msg);
+ (void)msg;
+ }
+
+public:
+ /**
+ * This overload is only valid if MemPoolSize > 0.
+ */
+ SubNode(ICanDriver& can_driver,
+ ISystemClock& system_clock) :
+ scheduler_(can_driver, pool_allocator_, system_clock),
+ internal_failure_cnt_(0)
+ { }
+
+ /**
+ * This overload is only valid if MemPoolSize == 0.
+ */
+ SubNode(ICanDriver& can_driver,
+ ISystemClock& system_clock,
+ IPoolAllocator& allocator) :
+ pool_allocator_(allocator),
+ scheduler_(can_driver, pool_allocator_, system_clock),
+ internal_failure_cnt_(0)
+ { }
+
+ virtual typename RemoveReference<Allocator>::Type& getAllocator() { return pool_allocator_; }
+
+ virtual Scheduler& getScheduler() { return scheduler_; }
+ virtual const Scheduler& getScheduler() const { return scheduler_; }
+
+ uint64_t getInternalFailureCount() const { return internal_failure_cnt_; }
+};
+
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/subscriber.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED
+#define UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/generic_subscriber.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <functional>
+#endif
+
+namespace uavcan
+{
+/**
+ * Use this class to subscribe to a message.
+ *
+ * @tparam DataType_ Message data type.
+ *
+ * @tparam Callback_ Type of the callback that will be used to deliver received messages
+ * into the application. Type of the argument of the callback can be either:
+ * - DataType_&
+ * - const DataType_&
+ * - ReceivedDataStructure<DataType_>&
+ * - const ReceivedDataStructure<DataType_>&
+ * For the first two options, @ref ReceivedDataStructure<> will be casted implicitly.
+ * In C++11 mode this type defaults to std::function<>.
+ * In C++03 mode this type defaults to a plain function pointer; use binder to
+ * call member functions as callbacks.
+ */
+template <typename DataType_,
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ typename Callback_ = std::function<void (const ReceivedDataStructure<DataType_>&)>
+#else
+ typename Callback_ = void (*)(const ReceivedDataStructure<DataType_>&)
+#endif
+ >
+class UAVCAN_EXPORT Subscriber
+ : public GenericSubscriber<DataType_, DataType_, TransferListener>
+{
+public:
+ typedef Callback_ Callback;
+
+private:
+ typedef GenericSubscriber<DataType_, DataType_, TransferListener> BaseType;
+
+ Callback callback_;
+
+ virtual void handleReceivedDataStruct(ReceivedDataStructure<DataType_>& msg)
+ {
+ if (coerceOrFallback<bool>(callback_, true))
+ {
+ callback_(msg);
+ }
+ else
+ {
+ handleFatalError("Sub clbk");
+ }
+ }
+
+public:
+ typedef DataType_ DataType;
+
+ explicit Subscriber(INode& node)
+ : BaseType(node)
+ , callback_()
+ {
+ StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
+ }
+
+ /**
+ * Begin receiving messages.
+ * Each message will be passed to the application via the callback.
+ * Returns negative error code.
+ */
+ int start(const Callback& callback)
+ {
+ stop();
+
+ if (!coerceOrFallback<bool>(callback, true))
+ {
+ UAVCAN_TRACE("Subscriber", "Invalid callback");
+ return -ErrInvalidParam;
+ }
+ callback_ = callback;
+
+ return BaseType::startAsMessageListener();
+ }
+
+ using BaseType::allowAnonymousTransfers;
+ using BaseType::stop;
+ using BaseType::getFailureCount;
+};
+
+}
+
+#endif // UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/node/timer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_NODE_TIMER_HPP_INCLUDED
+#define UAVCAN_NODE_TIMER_HPP_INCLUDED
+
+#include <uavcan/std.hpp>
+#include <uavcan/error.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/node/scheduler.hpp>
+#include <uavcan/node/abstract_node.hpp>
+#include <uavcan/util/templates.hpp>
+
+#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <functional>
+#endif
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT TimerBase;
+
+/**
+ * Objects of this type will be supplied into timer callbacks.
+ */
+struct UAVCAN_EXPORT TimerEvent
+{
+ MonotonicTime scheduled_time; ///< Time when the timer callback was expected to be invoked
+ MonotonicTime real_time; ///< True time when the timer callback was invoked
+
+ TimerEvent(MonotonicTime arg_scheduled_time, MonotonicTime arg_real_time)
+ : scheduled_time(arg_scheduled_time)
+ , real_time(arg_real_time)
+ { }
+};
+
+/**
+ * Inherit this class if you need a timer callback method in your class.
+ */
+class UAVCAN_EXPORT TimerBase : private DeadlineHandler
+{
+ MonotonicDuration period_;
+
+ virtual void handleDeadline(MonotonicTime current);
+
+public:
+ using DeadlineHandler::stop;
+ using DeadlineHandler::isRunning;
+ using DeadlineHandler::getDeadline;
+ using DeadlineHandler::getScheduler;
+
+ explicit TimerBase(INode& node)
+ : DeadlineHandler(node.getScheduler())
+ , period_(MonotonicDuration::getInfinite())
+ { }
+
+ /**
+ * Various ways to start the timer - periodically or once.
+ * If it is running already, it will be restarted.
+ * If the deadline is in the past, the event will fire immediately.
+ * In periodic mode the timer does not accumulate error over time.
+ */
+ void startOneShotWithDeadline(MonotonicTime deadline);
+ void startOneShotWithDelay(MonotonicDuration delay);
+ void startPeriodic(MonotonicDuration period);
+
+ /**
+ * Returns period if the timer is in periodic mode.
+ * Returns infinite duration if the timer is in one-shot mode or stopped.
+ */
+ MonotonicDuration getPeriod() const { return period_; }
+
+ /**
+ * Implement this method in your class to receive callbacks.
+ */
+ virtual void handleTimerEvent(const TimerEvent& event) = 0;
+};
+
+/**
+ * Wrapper over TimerBase that forwards callbacks into arbitrary handlers, like
+ * functor objects, member functions or static functions.
+ *
+ * Callback must be set before the first event; otherwise the event will generate a fatal error.
+ *
+ * Also take a look at @ref MethodBinder<>, which may come useful if C++11 features are not available.
+ *
+ * @tparam Callback_ Callback type. Shall accept const reference to TimerEvent as its argument.
+ */
+template <typename Callback_>
+class UAVCAN_EXPORT TimerEventForwarder : public TimerBase
+{
+public:
+ typedef Callback_ Callback;
+
+private:
+ Callback callback_;
+
+ virtual void handleTimerEvent(const TimerEvent& event)
+ {
+ if (coerceOrFallback<bool>(callback_, true))
+ {
+ callback_(event);
+ }
+ else
+ {
+ handleFatalError("Invalid timer callback");
+ }
+ }
+
+public:
+ explicit TimerEventForwarder(INode& node)
+ : TimerBase(node)
+ , callback_()
+ { }
+
+ TimerEventForwarder(INode& node, const Callback& callback)
+ : TimerBase(node)
+ , callback_(callback)
+ { }
+
+ /**
+ * Get/set the callback object.
+ * Callback must be set before the first event happens; otherwise the event will generate a fatal error.
+ */
+ const Callback& getCallback() const { return callback_; }
+ void setCallback(const Callback& callback) { callback_ = callback; }
+};
+
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+/**
+ * Use this timer in C++11 mode.
+ * Callback type is std::function<>.
+ */
+typedef TimerEventForwarder<std::function<void (const TimerEvent& event)> > Timer;
+
+#endif
+
+}
+
+#endif // UAVCAN_NODE_TIMER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/include/uavcan/protocol/README.md Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,13 @@ +High-level protocol logic +========================= + +Classes defined in this directory implement some high-level functions of UAVCAN. + +Since most applications won't use all of them at the same time, their definitions are provided right in the header +files rather than in source files, in order to not pollute the build outputs with unused code (which is also a +requirement for most safety-critical software). + +However, some of the high-level functions that are either always used by the library itself or those that are extremely +likely to be used by the application are defined in .cpp files. + +When adding a new class here, please think twice before putting its definition into a .cpp file.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED
+
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/GetDataTypeInfo.hpp>
+#include <uavcan/debug.hpp>
+
+namespace uavcan
+{
+/**
+ * This class implements the standard services for data type introspection.
+ * Once started it does not require any attention from the application.
+ * The user does not need to deal with it directly - it's started by the node class.
+ */
+class UAVCAN_EXPORT DataTypeInfoProvider : Noncopyable
+{
+ typedef MethodBinder<DataTypeInfoProvider*,
+ void (DataTypeInfoProvider::*)(const protocol::GetDataTypeInfo::Request&,
+ protocol::GetDataTypeInfo::Response&)> GetDataTypeInfoCallback;
+
+ ServiceServer<protocol::GetDataTypeInfo, GetDataTypeInfoCallback> gdti_srv_;
+
+ INode& getNode() { return gdti_srv_.getNode(); }
+
+ static bool isValidDataTypeKind(DataTypeKind kind)
+ {
+ return (kind == DataTypeKindMessage) || (kind == DataTypeKindService);
+ }
+
+ void handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request,
+ protocol::GetDataTypeInfo::Response& response)
+ {
+ /*
+ * Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID
+ */
+ const DataTypeDescriptor* desc = UAVCAN_NULLPTR;
+
+ if (request.name.empty())
+ {
+ response.id = request.id; // Pre-setting the fields so they have meaningful values even in
+ response.kind = request.kind; // ...case of failure.
+
+ if (!isValidDataTypeKind(DataTypeKind(request.kind.value)))
+ {
+ UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i",
+ static_cast<int>(request.kind.value));
+ return;
+ }
+
+ desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id);
+ }
+ else
+ {
+ response.name = request.name;
+
+ desc = GlobalDataTypeRegistry::instance().find(request.name.c_str());
+ }
+
+ if (desc == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("DataTypeInfoProvider",
+ "Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'",
+ static_cast<int>(request.id), static_cast<int>(request.kind.value), request.name.c_str());
+ return;
+ }
+
+ UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str());
+
+ /*
+ * Filling the response struct
+ */
+ response.signature = desc->getSignature().get();
+ response.id = desc->getID().get();
+ response.kind.value = desc->getKind();
+ response.flags = protocol::GetDataTypeInfo::Response::FLAG_KNOWN;
+ response.name = desc->getFullName();
+
+ const Dispatcher& dispatcher = getNode().getDispatcher();
+
+ if (desc->getKind() == DataTypeKindService)
+ {
+ if (dispatcher.hasServer(desc->getID().get()))
+ {
+ response.flags |= protocol::GetDataTypeInfo::Response::FLAG_SERVING;
+ }
+ }
+ else if (desc->getKind() == DataTypeKindMessage)
+ {
+ if (dispatcher.hasSubscriber(desc->getID().get()))
+ {
+ response.flags |= protocol::GetDataTypeInfo::Response::FLAG_SUBSCRIBED;
+ }
+ if (dispatcher.hasPublisher(desc->getID().get()))
+ {
+ response.flags |= protocol::GetDataTypeInfo::Response::FLAG_PUBLISHING;
+ }
+ }
+ else
+ {
+ UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror.
+ }
+ }
+
+public:
+ explicit DataTypeInfoProvider(INode& node) :
+ gdti_srv_(node)
+ { }
+
+ int start()
+ {
+ int res = 0;
+
+ res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest));
+ if (res < 0)
+ {
+ goto fail;
+ }
+
+ UAVCAN_ASSERT(res >= 0);
+ return res;
+
+ fail:
+ UAVCAN_ASSERT(res < 0);
+ gdti_srv_.stop();
+ return res;
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,112 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
+
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
+#include <uavcan/protocol/HardwareVersion.hpp>
+
+namespace uavcan
+{
+/**
+ * This class implements client-side logic of dynamic node ID allocation procedure.
+ *
+ * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined
+ * by the specification, until a Node ID is granted by the allocator.
+ *
+ * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests
+ * and listening for responses.
+ *
+ * Once dynamic allocation is complete (or not needed anymore), the object can be deleted.
+ *
+ * Note that this class uses std::rand(), which must be correctly seeded before use.
+ */
+class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase
+{
+ typedef MethodBinder<DynamicNodeIDClient*,
+ void (DynamicNodeIDClient::*)
+ (const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
+ AllocationCallback;
+
+ enum Mode
+ {
+ ModeWaitingForTimeSlot,
+ ModeDelayBeforeFollowup,
+ NumModes
+ };
+
+ Publisher<protocol::dynamic_node_id::Allocation> dnida_pub_;
+ Subscriber<protocol::dynamic_node_id::Allocation, AllocationCallback> dnida_sub_;
+
+ uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize];
+ uint8_t size_of_received_unique_id_;
+
+ NodeID preferred_node_id_;
+ NodeID allocated_node_id_;
+ NodeID allocator_node_id_;
+
+ void terminate();
+
+ static MonotonicDuration getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec);
+
+ void restartTimer(const Mode mode);
+
+ virtual void handleTimerEvent(const TimerEvent&);
+
+ void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg);
+
+public:
+ typedef protocol::HardwareVersion::FieldTypes::unique_id UniqueID;
+
+ DynamicNodeIDClient(INode& node)
+ : TimerBase(node)
+ , dnida_pub_(node)
+ , dnida_sub_(node)
+ , size_of_received_unique_id_(0)
+ { }
+
+ /**
+ * @param unique_id Unique ID of the local node. Must be the same as in the hardware version struct.
+ * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if
+ * the application doesn't have any preference (this is default).
+ * @param transfer_priority Transfer priority, Normal by default.
+ * @return Zero on success
+ * Negative error code on failure
+ * -ErrLogic if 1. the node is not in passive mode or 2. the client is already started
+ */
+ int start(const UniqueID& unique_id,
+ const NodeID preferred_node_id = NodeID::Broadcast,
+ const TransferPriority transfer_priority = TransferPriority::OneHigherThanLowest);
+
+ /**
+ * Use this method to determine when allocation is complete.
+ */
+ bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); }
+
+ /**
+ * This method allows to retrieve the node ID that was allocated to the local node.
+ * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast).
+ * @return If allocation is complete, a valid unicast node ID will be returned.
+ * If allocation is not complete yet, a non-unicast node ID will be returned.
+ */
+ NodeID getAllocatedNodeID() const { return allocated_node_id_; }
+
+ /**
+ * This method allows to retrieve node ID of the allocator that granted our Node ID.
+ * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast).
+ * @return If allocation is complete, a valid unicast node ID will be returned.
+ * If allocation is not complete yet, an non-unicast node ID will be returned.
+ */
+ NodeID getAllocatorNodeID() const { return allocator_node_id_; }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+
+class AbstractServer : protected IAllocationRequestHandler
+ , protected INodeDiscoveryHandler
+{
+ UniqueID own_unique_id_;
+ MonotonicTime started_at_;
+
+protected:
+ INode& node_;
+ IEventTracer& tracer_;
+ AllocationRequestManager allocation_request_manager_;
+ NodeDiscoverer node_discoverer_;
+
+ AbstractServer(INode& node,
+ IEventTracer& tracer) :
+ node_(node),
+ tracer_(tracer),
+ allocation_request_manager_(node, tracer, *this),
+ node_discoverer_(node, tracer, *this)
+ { }
+
+ const UniqueID& getOwnUniqueID() const { return own_unique_id_; }
+
+ int init(const UniqueID& own_unique_id, const TransferPriority priority)
+ {
+ int res = 0;
+
+ own_unique_id_ = own_unique_id;
+
+ res = allocation_request_manager_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = node_discoverer_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ started_at_ = node_.getMonotonicTime();
+
+ return 0;
+ }
+
+public:
+ /**
+ * This can be used to guess if there are any un-allocated dynamic nodes left in the network.
+ */
+ bool guessIfAllDynamicNodesAreAllocated(
+ const MonotonicDuration& allocation_activity_timeout =
+ MonotonicDuration::fromMSec(Allocation::MAX_REQUEST_PERIOD_MS * 2),
+ const MonotonicDuration& min_uptime = MonotonicDuration::fromMSec(6000)) const
+ {
+ const MonotonicTime ts = node_.getMonotonicTime();
+
+ /*
+ * If uptime is not large enough, the allocator may be unaware about some nodes yet.
+ */
+ const MonotonicDuration uptime = ts - started_at_;
+ if (uptime < max(allocation_activity_timeout, min_uptime))
+ {
+ return false;
+ }
+
+ /*
+ * If there are any undiscovered nodes, assume that allocation is still happening.
+ */
+ if (node_discoverer_.hasUnknownNodes())
+ {
+ return false;
+ }
+
+ /*
+ * Lastly, check if there wasn't any allocation messages detected on the bus in the specified amount of time.
+ */
+ const MonotonicDuration since_allocation_activity =
+ ts - allocation_request_manager_.getTimeOfLastAllocationActivity();
+ if (since_allocation_activity < allocation_activity_timeout)
+ {
+ return false;
+ }
+
+ return true;
+ }
+
+ /**
+ * This is useful for debugging/testing/monitoring.
+ */
+ const NodeDiscoverer& getNodeDiscoverer() const { return node_discoverer_; }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,286 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+// UAVCAN types
+#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+/**
+ * The main allocator must implement this interface.
+ */
+class IAllocationRequestHandler
+{
+public:
+ /**
+ * Allocation request manager uses this method to detect if it is allowed to publish follow-up responses.
+ */
+ virtual bool canPublishFollowupAllocationResponse() const = 0;
+
+ /**
+ * This method will be invoked when a new allocation request is received.
+ */
+ virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0;
+
+ virtual ~IAllocationRequestHandler() { }
+};
+
+/**
+ * This class manages communication with allocation clients.
+ * Three-stage unique ID exchange is implemented here, as well as response publication.
+ */
+class AllocationRequestManager
+{
+ typedef MethodBinder<AllocationRequestManager*,
+ void (AllocationRequestManager::*)(const ReceivedDataStructure<Allocation>&)>
+ AllocationCallback;
+
+ const MonotonicDuration stage_timeout_;
+
+ MonotonicTime last_message_timestamp_;
+ MonotonicTime last_activity_timestamp_;
+ Allocation::FieldTypes::unique_id current_unique_id_;
+
+ IAllocationRequestHandler& handler_;
+ IEventTracer& tracer_;
+
+ Subscriber<Allocation, AllocationCallback> allocation_sub_;
+ Publisher<Allocation> allocation_pub_;
+
+ enum { InvalidStage = 0 };
+
+ void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
+
+ static uint8_t detectRequestStage(const Allocation& msg)
+ {
+ const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
+
+ if ((msg.unique_id.size() != max_bytes_per_request) &&
+ (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) &&
+ (msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD
+ {
+ return InvalidStage;
+ }
+ if (msg.first_part_of_unique_id)
+ {
+ return 1; // Note that CAN FD frames can deliver the unique ID in one stage!
+ }
+ if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+ {
+ return 2;
+ }
+ if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+ {
+ return 3;
+ }
+ return InvalidStage;
+ }
+
+ uint8_t getExpectedStage() const
+ {
+ if (current_unique_id_.empty())
+ {
+ return 1;
+ }
+ if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2))
+ {
+ return 3;
+ }
+ if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+ {
+ return 2;
+ }
+ return InvalidStage;
+ }
+
+ void publishFollowupAllocationResponse()
+ {
+ Allocation msg;
+ msg.unique_id = current_unique_id_;
+ UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity());
+
+ UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID",
+ unsigned(msg.unique_id.size()));
+
+ trace(TraceAllocationFollowupResponse, msg.unique_id.size());
+
+ const int res = allocation_pub_.broadcast(msg);
+ if (res < 0)
+ {
+ trace(TraceError, res);
+ allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast");
+ }
+ }
+
+ void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
+ {
+ trace(TraceAllocationActivity, msg.getSrcNodeID().get());
+ last_activity_timestamp_ = msg.getMonotonicTimestamp();
+
+ if (!msg.isAnonymousTransfer())
+ {
+ return; // This is a response from another allocator, ignore
+ }
+
+ /*
+ * Reset the expected stage on timeout
+ */
+ if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_))
+ {
+ UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset");
+ current_unique_id_.clear();
+ trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec());
+ }
+
+ /*
+ * Checking if request stage matches the expected stage
+ */
+ const uint8_t request_stage = detectRequestStage(msg);
+ if (request_stage == InvalidStage)
+ {
+ trace(TraceAllocationBadRequest, msg.unique_id.size());
+ return; // Malformed request - ignore without resetting
+ }
+
+ const uint8_t expected_stage = getExpectedStage();
+ if (expected_stage == InvalidStage)
+ {
+ UAVCAN_ASSERT(0);
+ return;
+ }
+
+ if (request_stage != expected_stage)
+ {
+ trace(TraceAllocationUnexpectedStage, request_stage);
+ return; // Ignore - stage mismatch
+ }
+
+ const uint8_t max_expected_bytes =
+ static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size());
+ UAVCAN_ASSERT(max_expected_bytes > 0);
+ if (msg.unique_id.size() > max_expected_bytes)
+ {
+ trace(TraceAllocationBadRequest, msg.unique_id.size());
+ return; // Malformed request
+ }
+
+ /*
+ * Updating the local state
+ */
+ for (uint8_t i = 0; i < msg.unique_id.size(); i++)
+ {
+ current_unique_id_.push_back(msg.unique_id[i]);
+ }
+
+ trace(TraceAllocationRequestAccepted, current_unique_id_.size());
+
+ /*
+ * Proceeding with allocation if possible
+ * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader.
+ */
+ if (current_unique_id_.size() == current_unique_id_.capacity())
+ {
+ UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d",
+ int(msg.node_id));
+
+ UniqueID unique_id;
+ copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin());
+ current_unique_id_.clear();
+
+ {
+ uint64_t event_agrument = 0;
+ for (uint8_t i = 0; i < 8; i++)
+ {
+ event_agrument |= static_cast<uint64_t>(unique_id[i]) << (56U - i * 8U);
+ }
+ trace(TraceAllocationExchangeComplete, static_cast<int64_t>(event_agrument));
+ }
+
+ handler_.handleAllocationRequest(unique_id, msg.node_id);
+ }
+ else
+ {
+ if (handler_.canPublishFollowupAllocationResponse())
+ {
+ publishFollowupAllocationResponse();
+ }
+ else
+ {
+ trace(TraceAllocationFollowupDenied, 0);
+ current_unique_id_.clear();
+ }
+ }
+
+ /*
+ * It is super important to update timestamp only if the request has been processed successfully.
+ */
+ last_message_timestamp_ = msg.getMonotonicTimestamp();
+ }
+
+public:
+ AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler)
+ : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS))
+ , handler_(handler)
+ , tracer_(tracer)
+ , allocation_sub_(node)
+ , allocation_pub_(node)
+ { }
+
+ int init(const TransferPriority priority)
+ {
+ int res = allocation_pub_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS));
+
+ res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation));
+ if (res < 0)
+ {
+ return res;
+ }
+ allocation_sub_.allowAnonymousTransfers();
+
+ return 0;
+ }
+
+ int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id)
+ {
+ Allocation msg;
+
+ msg.unique_id.resize(msg.unique_id.capacity());
+ copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
+
+ msg.node_id = allocated_node_id.get();
+
+ trace(TraceAllocationResponse, msg.node_id);
+ last_activity_timestamp_ = allocation_pub_.getNode().getMonotonicTime();
+
+ return allocation_pub_.broadcast(msg);
+ }
+
+ /**
+ * When the last allocation activity was registered.
+ * This value can be used to heuristically determine whether there are any unallocated nodes left in the network.
+ */
+ MonotonicTime getTimeOfLastAllocationActivity() const { return last_activity_timestamp_; }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
+
+#include <uavcan/protocol/dynamic_node_id_server/centralized/server.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+
+typedef centralized::Server CentralizedServer;
+
+}
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,180 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/abstract_server.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace centralized
+{
+/**
+ * This server is an alternative to @ref DistributedServer with the following differences:
+ * - It is not distributed, so using it means introducing a single point of failure into the system.
+ * - It takes less code space and requires less RAM, which makes it suitable for resource-constrained applications.
+ *
+ * This version is suitable only for simple non-critical systems.
+ */
+class Server : public AbstractServer
+{
+ Storage storage_;
+
+ /*
+ * Private methods
+ */
+ bool isNodeIDTaken(const NodeID node_id) const
+ {
+ return storage_.isNodeIDOccupied(node_id);
+ }
+
+ void tryPublishAllocationResult(const NodeID node_id, const UniqueID& unique_id)
+ {
+ const int res = allocation_request_manager_.broadcastAllocationResponse(unique_id, node_id);
+ if (res < 0)
+ {
+ tracer_.onEvent(TraceError, res);
+ node_.registerInternalFailure("Dynamic allocation response");
+ }
+ }
+
+ /*
+ * Methods of IAllocationRequestHandler
+ */
+ virtual bool canPublishFollowupAllocationResponse() const
+ {
+ return true; // Because there's only one Centralized server in the system
+ }
+
+ virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id)
+ {
+ const NodeID existing_node_id = storage_.getNodeIDForUniqueID(unique_id);
+ if (existing_node_id.isValid())
+ {
+ tryPublishAllocationResult(existing_node_id, unique_id);
+ }
+ else
+ {
+ const NodeID allocated_node_id =
+ NodeIDSelector<Server>(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id);
+
+ if (allocated_node_id.isUnicast())
+ {
+ const int res = storage_.add(allocated_node_id, unique_id);
+ if (res >= 0)
+ {
+ tryPublishAllocationResult(allocated_node_id, unique_id);
+ }
+ else
+ {
+ tracer_.onEvent(TraceError, res);
+ node_.registerInternalFailure("CentralizedServer storage add");
+ }
+ }
+ else
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left");
+ }
+ }
+ }
+
+ /*
+ * Methods of INodeDiscoveryHandler
+ */
+ virtual bool canDiscoverNewNodes() const
+ {
+ return true; // Because there's only one Centralized server in the system
+ }
+
+ virtual NodeAwareness checkNodeAwareness(NodeID node_id) const
+ {
+ return storage_.isNodeIDOccupied(node_id) ? NodeAwarenessKnownAndCommitted : NodeAwarenessUnknown;
+ }
+
+ virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
+ {
+ if (storage_.isNodeIDOccupied(node_id))
+ {
+ UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that
+ return;
+ }
+
+ const int res = storage_.add(node_id, (unique_id_or_null == UAVCAN_NULLPTR) ? UniqueID() : *unique_id_or_null);
+ if (res < 0)
+ {
+ tracer_.onEvent(TraceError, res);
+ node_.registerInternalFailure("CentralizedServer storage add");
+ }
+ }
+
+public:
+ Server(INode& node,
+ IStorageBackend& storage,
+ IEventTracer& tracer)
+ : AbstractServer(node, tracer)
+ , storage_(storage)
+ { }
+
+ int init(const UniqueID& own_unique_id,
+ const TransferPriority priority = TransferPriority::OneHigherThanLowest)
+ {
+ /*
+ * Initializing storage first, because the next step requires it to be loaded
+ */
+ int res = storage_.init();
+ if (res < 0)
+ {
+ return res;
+ }
+
+ /*
+ * Common logic
+ */
+ res = AbstractServer::init(own_unique_id, priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ /*
+ * Making sure that the server is started with the same node ID
+ */
+ {
+ const NodeID stored_own_node_id = storage_.getNodeIDForUniqueID(getOwnUniqueID());
+ if (stored_own_node_id.isValid())
+ {
+ if (stored_own_node_id != node_.getNodeID())
+ {
+ return -ErrInvalidConfiguration;
+ }
+ }
+ else
+ {
+ res = storage_.add(node_.getNodeID(), getOwnUniqueID());
+ if (res < 0)
+ {
+ return res;
+ }
+ }
+ }
+
+ return 0;
+ }
+
+ uint8_t getNumAllocations() const { return storage_.getSize(); }
+};
+
+}
+}
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+#include <uavcan/util/bitset.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace centralized
+{
+/**
+ * This class transparently replicates its state to the storage backend, keeping the most recent state in memory.
+ * Writes are slow, reads are instantaneous.
+ */
+class Storage
+{
+ typedef BitSet<NodeID::Max + 1> OccupationMask;
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeStatic,
+ BitLenToByteLen<NodeID::Max + 1>::Result>
+ OccupationMaskArray;
+
+ IStorageBackend& storage_;
+ OccupationMask occupation_mask_;
+
+ static IStorageBackend::String getOccupationMaskKey() { return "occupation_mask"; }
+
+ static OccupationMask maskFromArray(const OccupationMaskArray& array)
+ {
+ OccupationMask mask;
+ for (uint8_t byte = 0; byte < array.size(); byte++)
+ {
+ for (uint8_t bit = 0; bit < 8; bit++)
+ {
+ mask[byte * 8U + bit] = (array[byte] & (1U << bit)) != 0;
+ }
+ }
+ return mask;
+ }
+
+ static OccupationMaskArray maskToArray(const OccupationMask& mask)
+ {
+ OccupationMaskArray array;
+ for (uint8_t byte = 0; byte < array.size(); byte++)
+ {
+ for (uint8_t bit = 0; bit < 8; bit++)
+ {
+ if (mask[byte * 8U + bit])
+ {
+ array[byte] = static_cast<uint8_t>(array[byte] | (1U << bit));
+ }
+ }
+ }
+ return array;
+ }
+
+public:
+ Storage(IStorageBackend& storage) :
+ storage_(storage)
+ { }
+
+ /**
+ * This method reads only the occupation mask from the storage.
+ */
+ int init()
+ {
+ StorageMarshaller io(storage_);
+ OccupationMaskArray array;
+ io.get(getOccupationMaskKey(), array);
+ occupation_mask_ = maskFromArray(array);
+ return 0;
+ }
+
+ /**
+ * This method invokes storage IO.
+ * Returned value indicates whether the entry was successfully appended.
+ */
+ int add(const NodeID node_id, const UniqueID& unique_id)
+ {
+ if (!node_id.isUnicast())
+ {
+ return -ErrInvalidParam;
+ }
+
+ StorageMarshaller io(storage_);
+
+ // If next operations fail, we'll get a dangling entry, but it's absolutely OK.
+ {
+ uint32_t node_id_int = node_id.get();
+ int res = io.setAndGetBack(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id_int);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (node_id_int != node_id.get())
+ {
+ return -ErrFailure;
+ }
+ }
+
+ // Updating the mask in the storage
+ OccupationMask new_occupation_mask = occupation_mask_;
+ new_occupation_mask[node_id.get()] = true;
+ OccupationMaskArray occupation_array = maskToArray(new_occupation_mask);
+
+ int res = io.setAndGetBack(getOccupationMaskKey(), occupation_array);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (occupation_array != maskToArray(new_occupation_mask))
+ {
+ return -ErrFailure;
+ }
+
+ // Updating the cached mask only if the storage was updated successfully
+ occupation_mask_ = new_occupation_mask;
+
+ return 0;
+ }
+
+ /**
+ * Returns an invalid node ID if there's no such allocation.
+ */
+ NodeID getNodeIDForUniqueID(const UniqueID& unique_id) const
+ {
+ StorageMarshaller io(storage_);
+ uint32_t node_id = 0;
+ io.get(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id);
+ return (node_id > 0 && node_id <= NodeID::Max) ? NodeID(static_cast<uint8_t>(node_id)) : NodeID();
+ }
+
+ bool isNodeIDOccupied(NodeID node_id) const { return occupation_mask_[node_id.get()]; }
+
+ uint8_t getSize() const { return static_cast<uint8_t>(occupation_mask_.count()); }
+};
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED
+
+#include <uavcan/protocol/dynamic_node_id_server/distributed/server.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+
+typedef distributed::Server DistributedServer;
+
+}
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,433 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/log.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+// UAVCAN types
+#include <uavcan/protocol/dynamic_node_id/server/Discovery.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace distributed
+{
+/**
+ * This class maintains the cluster state.
+ */
+class ClusterManager : private TimerBase
+{
+public:
+ enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize };
+
+private:
+ typedef MethodBinder<ClusterManager*,
+ void (ClusterManager::*)
+ (const ReceivedDataStructure<Discovery>&)>
+ DiscoveryCallback;
+
+ struct Server
+ {
+ NodeID node_id;
+ Log::Index next_index;
+ Log::Index match_index;
+
+ Server()
+ : next_index(0)
+ , match_index(0)
+ { }
+
+ void resetIndices(const Log& log)
+ {
+ next_index = Log::Index(log.getLastIndex() + 1U);
+ match_index = 0;
+ }
+ };
+
+ IStorageBackend& storage_;
+ IEventTracer& tracer_;
+ const Log& log_;
+
+ Subscriber<Discovery, DiscoveryCallback> discovery_sub_;
+ mutable Publisher<Discovery> discovery_pub_;
+
+ Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there.
+
+ uint8_t cluster_size_;
+ uint8_t num_known_servers_;
+
+ static IStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; }
+
+ INode& getNode() { return discovery_sub_.getNode(); }
+ const INode& getNode() const { return discovery_sub_.getNode(); }
+
+ const Server* findServer(NodeID node_id) const { return const_cast<ClusterManager*>(this)->findServer(node_id); }
+ Server* findServer(NodeID node_id)
+ {
+ for (uint8_t i = 0; i < num_known_servers_; i++)
+ {
+ UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
+ if (servers_[i].node_id == node_id)
+ {
+ return &servers_[i];
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+
+ virtual void handleTimerEvent(const TimerEvent&)
+ {
+ UAVCAN_ASSERT(num_known_servers_ < cluster_size_);
+
+ tracer_.onEvent(TraceRaftDiscoveryBroadcast, num_known_servers_);
+
+ /*
+ * Filling the message
+ */
+ Discovery msg;
+ msg.configured_cluster_size = cluster_size_;
+
+ msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0
+
+ for (uint8_t i = 0; i < num_known_servers_; i++)
+ {
+ UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
+ msg.known_nodes.push_back(servers_[i].node_id.get());
+ }
+
+ UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1));
+
+ /*
+ * Broadcasting
+ */
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager",
+ "Broadcasting Discovery message; known nodes: %d of %d",
+ int(msg.known_nodes.size()), int(cluster_size_));
+
+ const int res = discovery_pub_.broadcast(msg);
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Discovery broadcst failed: %d", res);
+ getNode().registerInternalFailure("Raft discovery broadcast");
+ }
+
+ /*
+ * Termination condition
+ */
+ if (isClusterDiscovered())
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager",
+ "Discovery broadcasting timer stopped");
+ stop();
+ }
+ }
+
+ void handleDiscovery(const ReceivedDataStructure<Discovery>& msg)
+ {
+ tracer_.onEvent(TraceRaftDiscoveryReceived, msg.getSrcNodeID().get());
+
+ /*
+ * Validating cluster configuration
+ * If there's a case of misconfiguration, the message will be ignored.
+ */
+ if (msg.configured_cluster_size != cluster_size_)
+ {
+ tracer_.onEvent(TraceRaftBadClusterSizeReceived, msg.configured_cluster_size);
+ getNode().registerInternalFailure("Bad Raft cluster size");
+ return;
+ }
+
+ /*
+ * Updating the set of known servers
+ */
+ for (uint8_t i = 0; i < msg.known_nodes.size(); i++)
+ {
+ if (isClusterDiscovered())
+ {
+ break;
+ }
+
+ const NodeID node_id(msg.known_nodes[i]);
+ if (node_id.isUnicast() && !isKnownServer(node_id))
+ {
+ addServer(node_id);
+ }
+ }
+
+ /*
+ * Publishing a new Discovery request if the publishing server needs to learn about more servers.
+ */
+ if (msg.configured_cluster_size > msg.known_nodes.size())
+ {
+ startDiscoveryPublishingTimerIfNotRunning();
+ }
+ }
+
+ void startDiscoveryPublishingTimerIfNotRunning()
+ {
+ if (!isRunning())
+ {
+ startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_PERIOD_MS));
+ }
+ }
+
+public:
+ enum { ClusterSizeUnknown = 0 };
+
+ /**
+ * @param node Needed to publish and subscribe to Discovery message
+ * @param storage Needed to read the cluster size parameter from the storage
+ * @param log Needed to initialize nextIndex[] values after elections
+ */
+ ClusterManager(INode& node, IStorageBackend& storage, const Log& log, IEventTracer& tracer)
+ : TimerBase(node)
+ , storage_(storage)
+ , tracer_(tracer)
+ , log_(log)
+ , discovery_sub_(node)
+ , discovery_pub_(node)
+ , cluster_size_(0)
+ , num_known_servers_(0)
+ { }
+
+ /**
+ * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the
+ * storage backend using key 'cluster_size'.
+ * Returns negative error code.
+ */
+ int init(const uint8_t init_cluster_size, const TransferPriority priority)
+ {
+ /*
+ * Figuring out the cluster size
+ */
+ if (init_cluster_size == ClusterSizeUnknown)
+ {
+ // Reading from the storage
+ StorageMarshaller io(storage_);
+ uint32_t value = 0;
+ int res = io.get(getStorageKeyForClusterSize(), value);
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager",
+ "Cluster size is neither configured nor stored in the storage");
+ return res;
+ }
+ if ((value == 0) || (value > MaxClusterSize))
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is invalid");
+ return -ErrInvalidConfiguration;
+ }
+ cluster_size_ = static_cast<uint8_t>(value);
+ }
+ else
+ {
+ if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize))
+ {
+ return -ErrInvalidParam;
+ }
+ cluster_size_ = init_cluster_size;
+
+ // Writing the storage
+ StorageMarshaller io(storage_);
+ uint32_t value = init_cluster_size;
+ int res = io.setAndGetBack(getStorageKeyForClusterSize(), value);
+ if ((res < 0) || (value != init_cluster_size))
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Failed to store cluster size");
+ return -ErrFailure;
+ }
+ }
+
+ tracer_.onEvent(TraceRaftClusterSizeInited, cluster_size_);
+
+ UAVCAN_ASSERT(cluster_size_ > 0);
+ UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize);
+
+ /*
+ * Initializing pub/sub and timer
+ */
+ int res = discovery_pub_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ startDiscoveryPublishingTimerIfNotRunning();
+
+ /*
+ * Misc
+ */
+ resetAllServerIndices();
+ return 0;
+ }
+
+ /**
+ * Adds once server regardless of the discovery logic.
+ */
+ void addServer(NodeID node_id)
+ {
+ UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize);
+ if (!isKnownServer(node_id) && node_id.isUnicast())
+ {
+ tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get());
+ servers_[num_known_servers_].node_id = node_id;
+ servers_[num_known_servers_].resetIndices(log_);
+ num_known_servers_ = static_cast<uint8_t>(num_known_servers_ + 1U);
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * Whether such server has been discovered.
+ */
+ bool isKnownServer(NodeID node_id) const
+ {
+ if (node_id == getNode().getNodeID())
+ {
+ return true;
+ }
+ for (uint8_t i = 0; i < num_known_servers_; i++)
+ {
+ UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
+ UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID());
+ if (servers_[i].node_id == node_id)
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ /**
+ * An invalid node ID will be returned if there's no such server.
+ * The local server is not listed there.
+ */
+ NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const
+ {
+ if (index < num_known_servers_)
+ {
+ return servers_[index].node_id;
+ }
+ return NodeID();
+ }
+
+ /**
+ * See next_index[] in Raft paper.
+ */
+ Log::Index getServerNextIndex(NodeID server_node_id) const
+ {
+ const Server* const s = findServer(server_node_id);
+ if (s != UAVCAN_NULLPTR)
+ {
+ return s->next_index;
+ }
+ UAVCAN_ASSERT(0);
+ return 0;
+ }
+
+ void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment)
+ {
+ Server* const s = findServer(server_node_id);
+ if (s != UAVCAN_NULLPTR)
+ {
+ s->next_index = Log::Index(s->next_index + increment);
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ void decrementServerNextIndex(NodeID server_node_id)
+ {
+ Server* const s = findServer(server_node_id);
+ if (s != UAVCAN_NULLPTR)
+ {
+ s->next_index--;
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * See match_index[] in Raft paper.
+ */
+ Log::Index getServerMatchIndex(NodeID server_node_id) const
+ {
+ const Server* const s = findServer(server_node_id);
+ if (s != UAVCAN_NULLPTR)
+ {
+ return s->match_index;
+ }
+ UAVCAN_ASSERT(0);
+ return 0;
+ }
+
+ void setServerMatchIndex(NodeID server_node_id, Log::Index match_index)
+ {
+ Server* const s = findServer(server_node_id);
+ if (s != UAVCAN_NULLPTR)
+ {
+ s->match_index = match_index;
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * This method must be called when the current server becomes leader.
+ */
+ void resetAllServerIndices()
+ {
+ for (uint8_t i = 0; i < num_known_servers_; i++)
+ {
+ UAVCAN_ASSERT(servers_[i].node_id.isUnicast());
+ servers_[i].resetIndices(log_);
+ }
+ }
+
+ /**
+ * Number of known servers can only grow, and it never exceeds the cluster size value.
+ * This number does not include the local server.
+ */
+ uint8_t getNumKnownServers() const { return num_known_servers_; }
+
+ /**
+ * Cluster size and quorum size are constant.
+ */
+ uint8_t getClusterSize() const { return cluster_size_; }
+ uint8_t getQuorumSize() const { return static_cast<uint8_t>(cluster_size_ / 2U + 1U); }
+
+ bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); }
+};
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,310 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace distributed
+{
+/**
+ * Raft log.
+ * This class transparently replicates its state to the storage backend, keeping the most recent state in memory.
+ * Writes are slow, reads are instantaneous.
+ */
+class Log
+{
+public:
+ typedef uint8_t Index;
+
+ enum { Capacity = NodeID::Max + 1 };
+
+private:
+ IStorageBackend& storage_;
+ IEventTracer& tracer_;
+ Entry entries_[Capacity];
+ Index last_index_; // Index zero always contains an empty entry
+
+ static IStorageBackend::String getLastIndexKey() { return "log_last_index"; }
+
+ static IStorageBackend::String makeEntryKey(Index index, const char* postfix)
+ {
+ IStorageBackend::String str;
+ // "log0_foobar"
+ str += "log";
+ str.appendFormatted("%d", int(index));
+ str += "_";
+ str += postfix;
+ return str;
+ }
+
+ int readEntryFromStorage(Index index, Entry& out_entry)
+ {
+ const StorageMarshaller io(storage_);
+
+ // Term
+ if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0)
+ {
+ return -ErrFailure;
+ }
+
+ // Unique ID
+ if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0)
+ {
+ return -ErrFailure;
+ }
+
+ // Node ID
+ uint32_t node_id = 0;
+ if (io.get(makeEntryKey(index, "node_id"), node_id) < 0)
+ {
+ return -ErrFailure;
+ }
+ if (node_id > NodeID::Max)
+ {
+ return -ErrFailure;
+ }
+ out_entry.node_id = static_cast<uint8_t>(node_id);
+
+ return 0;
+ }
+
+ int writeEntryToStorage(Index index, const Entry& entry)
+ {
+ Entry temp = entry;
+
+ StorageMarshaller io(storage_);
+
+ // Term
+ if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0)
+ {
+ return -ErrFailure;
+ }
+
+ // Unique ID
+ if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0)
+ {
+ return -ErrFailure;
+ }
+
+ // Node ID
+ uint32_t node_id = entry.node_id;
+ if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0)
+ {
+ return -ErrFailure;
+ }
+ temp.node_id = static_cast<uint8_t>(node_id);
+
+ return (temp == entry) ? 0 : -ErrFailure;
+ }
+
+ int initEmptyLogStorage()
+ {
+ StorageMarshaller io(storage_);
+
+ /*
+ * Writing the zero entry - it must always be default-initialized
+ */
+ entries_[0] = Entry();
+ int res = writeEntryToStorage(0, entries_[0]);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ /*
+ * Initializing last index
+ * Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be
+ * left in an inconsistent state.
+ */
+ last_index_ = 0;
+ uint32_t stored_index = 0;
+ res = io.setAndGetBack(getLastIndexKey(), stored_index);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (stored_index != 0)
+ {
+ return -ErrFailure;
+ }
+
+ return 0;
+ }
+
+public:
+ Log(IStorageBackend& storage, IEventTracer& tracer)
+ : storage_(storage)
+ , tracer_(tracer)
+ , last_index_(0)
+ { }
+
+ int init()
+ {
+ StorageMarshaller io(storage_);
+
+ // Reading max index
+ {
+ uint32_t value = 0;
+ if (io.get(getLastIndexKey(), value) < 0)
+ {
+ if (storage_.get(getLastIndexKey()).empty())
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Initializing empty storage");
+ return initEmptyLogStorage();
+ }
+ else
+ {
+ // There's some data in the storage, but it cannot be parsed - reporting an error
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read last index");
+ return -ErrFailure;
+ }
+ }
+ if (value >= Capacity)
+ {
+ return -ErrFailure;
+ }
+ last_index_ = Index(value);
+ }
+
+ tracer_.onEvent(TraceRaftLogLastIndexRestored, last_index_);
+
+ // Restoring log entries - note that index 0 always exists
+ for (Index index = 0; index <= last_index_; index++)
+ {
+ const int result = readEntryFromStorage(index, entries_[index]);
+ if (result < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read entry at index %u: %d",
+ unsigned(index), result);
+ return result;
+ }
+ }
+
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Restored %u log entries", unsigned(last_index_));
+ return 0;
+ }
+
+ /**
+ * This method invokes storage IO.
+ * Returned value indicates whether the entry was successfully appended.
+ */
+ int append(const Entry& entry)
+ {
+ if ((last_index_ + 1) >= Capacity)
+ {
+ return -ErrLogic;
+ }
+
+ tracer_.onEvent(TraceRaftLogAppend, last_index_ + 1U);
+
+ // If next operations fail, we'll get a dangling entry, but it's absolutely OK.
+ int res = writeEntryToStorage(Index(last_index_ + 1), entry);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ // Updating the last index
+ StorageMarshaller io(storage_);
+ uint32_t new_last_index = last_index_ + 1U;
+ res = io.setAndGetBack(getLastIndexKey(), new_last_index);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (new_last_index != last_index_ + 1U)
+ {
+ return -ErrFailure;
+ }
+ entries_[new_last_index] = entry;
+ last_index_ = Index(new_last_index);
+
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "New entry, index %u, node ID %u, term %u",
+ unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term));
+ return 0;
+ }
+
+ /**
+ * This method invokes storage IO.
+ * Returned value indicates whether the requested operation has been carried out successfully.
+ */
+ int removeEntriesWhereIndexGreaterOrEqual(Index index)
+ {
+ UAVCAN_ASSERT(last_index_ < Capacity);
+
+ if (((index) >= Capacity) || (index <= 0))
+ {
+ return -ErrLogic;
+ }
+
+ uint32_t new_last_index = index - 1U;
+
+ tracer_.onEvent(TraceRaftLogRemove, new_last_index);
+
+ if (new_last_index != last_index_)
+ {
+ StorageMarshaller io(storage_);
+ int res = io.setAndGetBack(getLastIndexKey(), new_last_index);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (new_last_index != index - 1U)
+ {
+ return -ErrFailure;
+ }
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Entries removed, last index %u --> %u",
+ unsigned(last_index_), unsigned(new_last_index));
+ last_index_ = Index(new_last_index);
+ }
+
+ // Removal operation leaves dangling entries in storage, it's OK
+ return 0;
+ }
+
+ int removeEntriesWhereIndexGreater(Index index)
+ {
+ return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U));
+ }
+
+ /**
+ * Returns nullptr if there's no such index.
+ * This method does not use storage IO.
+ */
+ const Entry* getEntryAtIndex(Index index) const
+ {
+ UAVCAN_ASSERT(last_index_ < Capacity);
+ return (index <= last_index_) ? &entries_[index] : UAVCAN_NULLPTR;
+ }
+
+ Index getLastIndex() const { return last_index_; }
+
+ bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const
+ {
+ UAVCAN_ASSERT(last_index_ < Capacity);
+ // Terms are different - the one with higher term is more up-to-date
+ if (other_last_term != entries_[last_index_].term)
+ {
+ return other_last_term > entries_[last_index_].term;
+ }
+ // Terms are equal - longer log wins
+ return other_last_index >= last_index_;
+ }
+};
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,237 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/log.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace distributed
+{
+/**
+ * This class is a convenient container for persistent state variables defined by Raft.
+ * Writes are slow, reads are instantaneous.
+ */
+class PersistentState
+{
+ IStorageBackend& storage_;
+ IEventTracer& tracer_;
+
+ Term current_term_;
+ NodeID voted_for_;
+ Log log_;
+
+ static IStorageBackend::String getCurrentTermKey() { return "current_term"; }
+ static IStorageBackend::String getVotedForKey() { return "voted_for"; }
+
+public:
+ PersistentState(IStorageBackend& storage, IEventTracer& tracer)
+ : storage_(storage)
+ , tracer_(tracer)
+ , current_term_(0)
+ , log_(storage, tracer)
+ { }
+
+ /**
+ * Initialization is performed as follows (every step may fail and return an error):
+ * 1. Log is restored or initialized.
+ * 2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized
+ * with zero.
+ * 3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is
+ * zero, the value will be initialized with zero.
+ */
+ int init()
+ {
+ /*
+ * Reading log
+ */
+ int res = log_.init();
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res);
+ return res;
+ }
+
+ const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex());
+ if (last_entry == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrLogic;
+ }
+
+ const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0);
+
+ StorageMarshaller io(storage_);
+
+ /*
+ * Reading currentTerm
+ */
+ if (storage_.get(getCurrentTermKey()).empty() && log_is_empty)
+ {
+ // First initialization
+ current_term_ = 0;
+ res = io.setAndGetBack(getCurrentTermKey(), current_term_);
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
+ "Failed to init current term: %d", res);
+ return res;
+ }
+ if (current_term_ != 0)
+ {
+ return -ErrFailure;
+ }
+ }
+ else
+ {
+ // Restoring
+ res = io.get(getCurrentTermKey(), current_term_);
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
+ "Failed to read current term: %d", res);
+ return res;
+ }
+ }
+
+ tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_);
+
+ if (current_term_ < last_entry->term)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
+ "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)",
+ unsigned(current_term_), unsigned(last_entry->term));
+ return -ErrLogic;
+ }
+
+ /*
+ * Reading votedFor
+ */
+ if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0))
+ {
+ // First initialization
+ voted_for_ = NodeID(0);
+ uint32_t stored_voted_for = 0;
+ res = io.setAndGetBack(getVotedForKey(), stored_voted_for);
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
+ "Failed to init votedFor: %d", res);
+ return res;
+ }
+ if (stored_voted_for != 0)
+ {
+ return -ErrFailure;
+ }
+ }
+ else
+ {
+ // Restoring
+ uint32_t stored_voted_for = 0;
+ res = io.get(getVotedForKey(), stored_voted_for);
+ if (res < 0)
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
+ "Failed to read votedFor: %d", res);
+ return res;
+ }
+ if (stored_voted_for > NodeID::Max)
+ {
+ return -ErrInvalidConfiguration;
+ }
+ voted_for_ = NodeID(uint8_t(stored_voted_for));
+ }
+
+ tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get());
+
+ return 0;
+ }
+
+ Term getCurrentTerm() const { return current_term_; }
+
+ NodeID getVotedFor() const { return voted_for_; }
+ bool isVotedForSet() const { return voted_for_.isUnicast(); }
+
+ Log& getLog() { return log_; }
+ const Log& getLog() const { return log_; }
+
+ /**
+ * Invokes storage IO.
+ */
+ int setCurrentTerm(Term term)
+ {
+ if (term < current_term_)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+
+ tracer_.onEvent(TraceRaftCurrentTermUpdate, term);
+
+ StorageMarshaller io(storage_);
+
+ Term tmp = term;
+ int res = io.setAndGetBack(getCurrentTermKey(), tmp);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ if (tmp != term)
+ {
+ return -ErrFailure;
+ }
+
+ current_term_ = term;
+ return 0;
+ }
+
+ /**
+ * Invokes storage IO.
+ */
+ int setVotedFor(NodeID node_id)
+ {
+ if (!node_id.isValid())
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+
+ tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get());
+
+ StorageMarshaller io(storage_);
+
+ uint32_t tmp = node_id.get();
+ int res = io.setAndGetBack(getVotedForKey(), tmp);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ if (node_id.get() != tmp)
+ {
+ return -ErrFailure;
+ }
+
+ voted_for_ = node_id;
+ return 0;
+ }
+
+ int resetVotedFor() { return setVotedFor(NodeID(0)); }
+};
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,917 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED
+
+#include <cstdlib>
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+// UAVCAN types
+#include <uavcan/protocol/dynamic_node_id/server/AppendEntries.hpp>
+#include <uavcan/protocol/dynamic_node_id/server/RequestVote.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace distributed
+{
+/**
+ * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log.
+ */
+class IRaftLeaderMonitor
+{
+public:
+ /**
+ * This method will be invoked when a new log entry is committed (only if the local server is the current Leader).
+ */
+ virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0;
+
+ /**
+ * Invoked by the Raft core when the local node becomes a leader or ceases to be one.
+ * By default the local node is not leader.
+ * It is possible to commit to the log right from this method.
+ */
+ virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0;
+
+ virtual ~IRaftLeaderMonitor() { }
+};
+
+/**
+ * This class implements log replication and voting.
+ * It does not implement client-server interaction at all; instead it just exposes a public method for adding
+ * allocation entries.
+ *
+ * Note that this class uses std::rand(), so the RNG must be properly seeded by the application.
+ *
+ * Activity registration:
+ * - persistent state update error
+ * - switch to candidate (this defines timeout between reelections)
+ * - newer term in response (also switch to follower)
+ * - append entries request with term >= currentTerm
+ * - vote granted
+ */
+class RaftCore : private TimerBase
+{
+public:
+ enum ServerState
+ {
+ ServerStateFollower,
+ ServerStateCandidate,
+ ServerStateLeader
+ };
+
+private:
+ typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ReceivedDataStructure<AppendEntries::Request>&,
+ ServiceResponseDataStructure<AppendEntries::Response>&)>
+ AppendEntriesCallback;
+
+ typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ServiceCallResult<AppendEntries>&)>
+ AppendEntriesResponseCallback;
+
+ typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ReceivedDataStructure<RequestVote::Request>&,
+ ServiceResponseDataStructure<RequestVote::Response>&)>
+ RequestVoteCallback;
+
+ typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ServiceCallResult<RequestVote>&)>
+ RequestVoteResponseCallback;
+
+ struct PendingAppendEntriesFields
+ {
+ Log::Index prev_log_index;
+ Log::Index num_entries;
+
+ PendingAppendEntriesFields()
+ : prev_log_index(0)
+ , num_entries(0)
+ { }
+ };
+
+ /*
+ * Constants
+ */
+ enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 };
+
+ IEventTracer& tracer_;
+ IRaftLeaderMonitor& leader_monitor_;
+
+ /*
+ * States
+ */
+ PersistentState persistent_state_;
+ ClusterManager cluster_;
+ Log::Index commit_index_;
+
+ MonotonicTime last_activity_timestamp_;
+ MonotonicDuration randomized_activity_timeout_;
+ ServerState server_state_;
+
+ uint8_t next_server_index_; ///< Next server to query AE from
+ uint8_t num_votes_received_in_this_campaign_;
+
+ PendingAppendEntriesFields pending_append_entries_fields_;
+
+ /*
+ * Transport
+ */
+ ServiceServer<AppendEntries, AppendEntriesCallback> append_entries_srv_;
+ ServiceClient<AppendEntries, AppendEntriesResponseCallback> append_entries_client_;
+ ServiceServer<RequestVote, RequestVoteCallback> request_vote_srv_;
+ ServiceClient<RequestVote, RequestVoteResponseCallback> request_vote_client_;
+
+ /*
+ * Methods
+ */
+ void trace(TraceCode event, int64_t argument) { tracer_.onEvent(event, argument); }
+
+ INode& getNode() { return append_entries_srv_.getNode(); }
+ const INode& getNode() const { return append_entries_srv_.getNode(); }
+
+ void checkInvariants() const
+ {
+ // Commit index
+ UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex());
+
+ // Term
+ UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) !=
+ UAVCAN_NULLPTR);
+ UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term <=
+ persistent_state_.getCurrentTerm());
+
+ // Elections
+ UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() ||
+ persistent_state_.getVotedFor() == getNode().getNodeID());
+ UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize());
+
+ // Transport
+ UAVCAN_ASSERT(append_entries_client_.getNumPendingCalls() <= 1);
+ UAVCAN_ASSERT(request_vote_client_.getNumPendingCalls() <= cluster_.getNumKnownServers());
+ UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !append_entries_client_.hasPendingCalls());
+ UAVCAN_ASSERT(server_state_ != ServerStateLeader || !request_vote_client_.hasPendingCalls());
+ UAVCAN_ASSERT(server_state_ != ServerStateFollower ||
+ (!append_entries_client_.hasPendingCalls() && !request_vote_client_.hasPendingCalls()));
+ }
+
+ void registerActivity()
+ {
+ last_activity_timestamp_ = getNode().getMonotonicTime();
+
+ static const int32_t randomization_range_msec = AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS -
+ AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS;
+ // coverity[dont_call]
+ const int32_t random_msec = (std::rand() % randomization_range_msec) + 1;
+
+ randomized_activity_timeout_ =
+ MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec);
+
+ UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS);
+ UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS);
+ }
+
+ bool isActivityTimedOut() const
+ {
+ return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_);
+ }
+
+ void handlePersistentStateUpdateError(int error)
+ {
+ UAVCAN_ASSERT(error < 0);
+ trace(TraceRaftPersistStateUpdateError, error);
+ switchState(ServerStateFollower);
+ registerActivity(); // Deferring reelections
+ }
+
+ void updateFollower()
+ {
+ if (isActivityTimedOut())
+ {
+ switchState(ServerStateCandidate);
+ registerActivity();
+ }
+ }
+
+ void updateCandidate()
+ {
+ if (num_votes_received_in_this_campaign_ > 0)
+ {
+ trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_);
+ const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize();
+
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Election complete, won: %d", int(won));
+
+ switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader
+ }
+ else
+ {
+ // Set votedFor, abort on failure
+ int res = persistent_state_.setVotedFor(getNode().getNodeID());
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ return;
+ }
+
+ // Increment current term, abort on failure
+ res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U);
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ return;
+ }
+
+ num_votes_received_in_this_campaign_ = 1; // Voting for self
+
+ RequestVote::Request req;
+ req.last_log_index = persistent_state_.getLog().getLastIndex();
+ req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term;
+ req.term = persistent_state_.getCurrentTerm();
+
+ for (uint8_t i = 0; i < MaxNumFollowers; i++)
+ {
+ const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i);
+ if (!node_id.isUnicast())
+ {
+ break;
+ }
+
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore",
+ "Requesting vote from %d", int(node_id.get()));
+ trace(TraceRaftVoteRequestInitiation, node_id.get());
+
+ res = request_vote_client_.call(node_id, req);
+ if (res < 0)
+ {
+ trace(TraceError, res);
+ }
+ }
+ }
+ }
+
+ void updateLeader()
+ {
+ if (append_entries_client_.hasPendingCalls())
+ {
+ append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why
+ }
+
+ if (cluster_.getClusterSize() > 1)
+ {
+ const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_);
+ UAVCAN_ASSERT(node_id.isUnicast());
+
+ next_server_index_++;
+ if (next_server_index_ >= cluster_.getNumKnownServers())
+ {
+ next_server_index_ = 0;
+ }
+
+ AppendEntries::Request req;
+ req.term = persistent_state_.getCurrentTerm();
+ req.leader_commit = commit_index_;
+
+ req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U);
+
+ const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index);
+ if (entry == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ handlePersistentStateUpdateError(-ErrLogic);
+ return;
+ }
+
+ req.prev_log_term = entry->term;
+
+ for (Log::Index index = cluster_.getServerNextIndex(node_id);
+ index <= persistent_state_.getLog().getLastIndex();
+ index++)
+ {
+ req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index));
+ if (req.entries.size() == req.entries.capacity())
+ {
+ break;
+ }
+ }
+
+ pending_append_entries_fields_.num_entries = req.entries.size();
+ pending_append_entries_fields_.prev_log_index = req.prev_log_index;
+
+ const int res = append_entries_client_.call(node_id, req);
+ if (res < 0)
+ {
+ trace(TraceRaftAppendEntriesCallFailure, res);
+ }
+ }
+
+ propagateCommitIndex();
+ }
+
+ void switchState(ServerState new_state)
+ {
+ if (server_state_ == new_state)
+ {
+ return;
+ }
+
+ /*
+ * Logging
+ */
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d",
+ int(server_state_), int(new_state));
+ trace(TraceRaftStateSwitch, new_state);
+
+ /*
+ * Updating the current state
+ */
+ const ServerState old_state = server_state_;
+ server_state_ = new_state;
+
+ /*
+ * Resetting specific states
+ */
+ cluster_.resetAllServerIndices();
+
+ next_server_index_ = 0;
+ num_votes_received_in_this_campaign_ = 0;
+
+ request_vote_client_.cancelAllCalls();
+ append_entries_client_.cancelAllCalls();
+
+ /*
+ * Calling the switch handler
+ * Note that the handler may commit to the log directly
+ */
+ if ((old_state == ServerStateLeader) ||
+ (new_state == ServerStateLeader))
+ {
+ leader_monitor_.handleLocalLeadershipChange(new_state == ServerStateLeader);
+ }
+ }
+
+ void tryIncrementCurrentTermFromResponse(Term new_term)
+ {
+ trace(TraceRaftNewerTermInResponse, new_term);
+ const int res = persistent_state_.setCurrentTerm(new_term);
+ if (res < 0)
+ {
+ trace(TraceRaftPersistStateUpdateError, res);
+ }
+ registerActivity(); // Deferring future elections
+ switchState(ServerStateFollower);
+ }
+
+ void propagateCommitIndex()
+ {
+ // Objective is to estimate whether we can safely increment commit index value
+ UAVCAN_ASSERT(server_state_ == ServerStateLeader);
+ UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex());
+
+ if (commit_index_ < persistent_state_.getLog().getLastIndex())
+ {
+ /*
+ * Not all local entries are committed.
+ * Deciding if it is safe to increment commit index.
+ */
+ uint8_t num_nodes_with_next_log_entry_available = 1; // Local node
+ for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++)
+ {
+ const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i));
+ if (match_index > commit_index_)
+ {
+ num_nodes_with_next_log_entry_available++;
+ }
+ }
+
+ if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize())
+ {
+ commit_index_++;
+ UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed
+ trace(TraceRaftNewEntryCommitted, commit_index_);
+
+ // AT THIS POINT ALLOCATION IS COMPLETE
+ leader_monitor_.handleLogCommitOnLeader(*persistent_state_.getLog().getEntryAtIndex(commit_index_));
+ }
+ }
+ }
+
+ void handleAppendEntriesRequest(const ReceivedDataStructure<AppendEntries::Request>& request,
+ ServiceResponseDataStructure<AppendEntries::Response>& response)
+ {
+ checkInvariants();
+
+ if (!cluster_.isKnownServer(request.getSrcNodeID()))
+ {
+ if (cluster_.isClusterDiscovered())
+ {
+ trace(TraceRaftRequestIgnored, request.getSrcNodeID().get());
+ response.setResponseEnabled(false);
+ return;
+ }
+ else
+ {
+ cluster_.addServer(request.getSrcNodeID());
+ }
+ }
+
+ UAVCAN_ASSERT(response.isResponseEnabled()); // This is default
+
+ /*
+ * Checking if our current state is up to date.
+ * The request will be ignored if persistent state cannot be updated.
+ */
+ if (request.term > persistent_state_.getCurrentTerm())
+ {
+ int res = persistent_state_.setCurrentTerm(request.term);
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ response.setResponseEnabled(false);
+ return;
+ }
+
+ res = persistent_state_.resetVotedFor();
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ response.setResponseEnabled(false);
+ return;
+ }
+ }
+
+ /*
+ * Preparing the response
+ */
+ response.term = persistent_state_.getCurrentTerm();
+ response.success = false;
+
+ /*
+ * Step 1 (see Raft paper)
+ * Reject the request if the leader has stale term number.
+ */
+ if (request.term < persistent_state_.getCurrentTerm())
+ {
+ response.setResponseEnabled(true);
+ return;
+ }
+
+ registerActivity();
+ switchState(ServerStateFollower);
+
+ /*
+ * Step 2
+ * Reject the request if the assumed log index does not exist on the local node.
+ */
+ const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index);
+ if (prev_entry == UAVCAN_NULLPTR)
+ {
+ response.setResponseEnabled(true);
+ return;
+ }
+
+ /*
+ * Step 3
+ * Drop log entries if term number does not match.
+ * Ignore the request if the persistent state cannot be updated.
+ */
+ if (prev_entry->term != request.prev_log_term)
+ {
+ const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index);
+ response.setResponseEnabled(res >= 0);
+ if (res < 0)
+ {
+ trace(TraceRaftPersistStateUpdateError, res);
+ }
+ return;
+ }
+
+ /*
+ * Step 4
+ * Update the log with new entries - this will possibly require to rewrite existing entries.
+ * Ignore the request if the persistent state cannot be updated.
+ */
+ if (request.prev_log_index != persistent_state_.getLog().getLastIndex())
+ {
+ const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index);
+ if (res < 0)
+ {
+ trace(TraceRaftPersistStateUpdateError, res);
+ response.setResponseEnabled(false);
+ return;
+ }
+ }
+
+ for (uint8_t i = 0; i < request.entries.size(); i++)
+ {
+ const int res = persistent_state_.getLog().append(request.entries[i]);
+ if (res < 0)
+ {
+ trace(TraceRaftPersistStateUpdateError, res);
+ response.setResponseEnabled(false);
+ return; // Response will not be sent, the server will assume that we're dead
+ }
+ }
+
+ /*
+ * Step 5
+ * Update the commit index.
+ */
+ if (request.leader_commit > commit_index_)
+ {
+ commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex());
+ trace(TraceRaftCommitIndexUpdate, commit_index_);
+ }
+
+ response.setResponseEnabled(true);
+ response.success = true;
+ }
+
+ void handleAppendEntriesResponse(const ServiceCallResult<AppendEntries>& result)
+ {
+ UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled
+ checkInvariants();
+
+ if (!result.isSuccessful())
+ {
+ return;
+ }
+
+ if (result.getResponse().term > persistent_state_.getCurrentTerm())
+ {
+ tryIncrementCurrentTermFromResponse(result.getResponse().term);
+ }
+ else
+ {
+ if (result.getResponse().success)
+ {
+ cluster_.incrementServerNextIndexBy(result.getCallID().server_node_id,
+ pending_append_entries_fields_.num_entries);
+ cluster_.setServerMatchIndex(result.getCallID().server_node_id,
+ Log::Index(pending_append_entries_fields_.prev_log_index +
+ pending_append_entries_fields_.num_entries));
+ }
+ else
+ {
+ cluster_.decrementServerNextIndex(result.getCallID().server_node_id);
+ trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get());
+ }
+ }
+
+ pending_append_entries_fields_ = PendingAppendEntriesFields();
+ // Rest of the logic is implemented in periodic update handlers.
+ }
+
+ void handleRequestVoteRequest(const ReceivedDataStructure<RequestVote::Request>& request,
+ ServiceResponseDataStructure<RequestVote::Response>& response)
+ {
+ checkInvariants();
+ trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get());
+
+ if (!cluster_.isKnownServer(request.getSrcNodeID()))
+ {
+ trace(TraceRaftRequestIgnored, request.getSrcNodeID().get());
+ response.setResponseEnabled(false);
+ return;
+ }
+
+ UAVCAN_ASSERT(response.isResponseEnabled()); // This is default
+
+ /*
+ * Checking if our current state is up to date.
+ * The request will be ignored if persistent state cannot be updated.
+ */
+ if (request.term > persistent_state_.getCurrentTerm())
+ {
+ switchState(ServerStateFollower); // Our term is stale, so we can't serve as leader
+
+ int res = persistent_state_.setCurrentTerm(request.term);
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ response.setResponseEnabled(false);
+ return;
+ }
+
+ res = persistent_state_.resetVotedFor();
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ response.setResponseEnabled(false);
+ return;
+ }
+ }
+
+ /*
+ * Preparing the response
+ */
+ response.term = persistent_state_.getCurrentTerm();
+
+ if (request.term < response.term)
+ {
+ response.vote_granted = false;
+ }
+ else
+ {
+ const bool can_vote = !persistent_state_.isVotedForSet() ||
+ (persistent_state_.getVotedFor() == request.getSrcNodeID());
+ const bool log_is_up_to_date =
+ persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term);
+
+ response.vote_granted = can_vote && log_is_up_to_date;
+
+ if (response.vote_granted)
+ {
+ switchState(ServerStateFollower); // Avoiding race condition when Candidate
+ registerActivity(); // This is necessary to avoid excessive elections
+
+ const int res = persistent_state_.setVotedFor(request.getSrcNodeID());
+ if (res < 0)
+ {
+ trace(TraceRaftPersistStateUpdateError, res);
+ response.setResponseEnabled(false);
+ return;
+ }
+ }
+ }
+ }
+
+ void handleRequestVoteResponse(const ServiceCallResult<RequestVote>& result)
+ {
+ UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled
+ checkInvariants();
+
+ if (!result.isSuccessful())
+ {
+ return;
+ }
+
+ trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get());
+
+ if (result.getResponse().term > persistent_state_.getCurrentTerm())
+ {
+ tryIncrementCurrentTermFromResponse(result.getResponse().term);
+ }
+ else
+ {
+ if (result.getResponse().vote_granted)
+ {
+ num_votes_received_in_this_campaign_++;
+ }
+ }
+ // Rest of the logic is implemented in periodic update handlers.
+ // I'm no fan of asynchronous programming. At all.
+ }
+
+ virtual void handleTimerEvent(const TimerEvent&)
+ {
+ checkInvariants();
+
+ switch (server_state_)
+ {
+ case ServerStateFollower:
+ {
+ updateFollower();
+ break;
+ }
+ case ServerStateCandidate:
+ {
+ updateCandidate();
+ break;
+ }
+ case ServerStateLeader:
+ {
+ updateLeader();
+ break;
+ }
+ default:
+ {
+ UAVCAN_ASSERT(0);
+ break;
+ }
+ }
+ }
+
+public:
+ RaftCore(INode& node,
+ IStorageBackend& storage,
+ IEventTracer& tracer,
+ IRaftLeaderMonitor& leader_monitor)
+ : TimerBase(node)
+ , tracer_(tracer)
+ , leader_monitor_(leader_monitor)
+ , persistent_state_(storage, tracer)
+ , cluster_(node, storage, persistent_state_.getLog(), tracer)
+ , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero
+ , last_activity_timestamp_(node.getMonotonicTime())
+ , randomized_activity_timeout_(
+ MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS))
+ , server_state_(ServerStateFollower)
+ , next_server_index_(0)
+ , num_votes_received_in_this_campaign_(0)
+ , append_entries_srv_(node)
+ , append_entries_client_(node)
+ , request_vote_srv_(node)
+ , request_vote_client_(node)
+ { }
+
+ /**
+ * Once started, the logic runs in the background until destructor is called.
+ * @param cluster_size If set, this value will be used and stored in the persistent storage. If not set,
+ * value from the persistent storage will be used. If not set and there's no such key
+ * in the persistent storage, initialization will fail.
+ */
+ int init(const uint8_t cluster_size, const TransferPriority priority)
+ {
+ /*
+ * Initializing state variables
+ */
+ server_state_ = ServerStateFollower;
+ next_server_index_ = 0;
+ num_votes_received_in_this_campaign_ = 0;
+ commit_index_ = 0;
+
+ registerActivity();
+
+ /*
+ * Initializing internals
+ */
+ int res = persistent_state_.init();
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = cluster_.init(cluster_size, priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = append_entries_client_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ append_entries_client_.setCallback(AppendEntriesResponseCallback(this,
+ &RaftCore::handleAppendEntriesResponse));
+
+ res = request_vote_client_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse));
+
+ /*
+ * Initializing timing constants
+ * Refer to the specification for the formula
+ */
+ const uint8_t num_followers = static_cast<uint8_t>(cluster_.getClusterSize() - 1);
+
+ const MonotonicDuration update_interval =
+ MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS /
+ 2 / max(static_cast<uint8_t>(2), num_followers));
+
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore",
+ "Update interval: %ld msec", static_cast<long>(update_interval.toMSec()));
+
+ append_entries_client_.setRequestTimeout(min(append_entries_client_.getDefaultRequestTimeout(),
+ update_interval));
+
+ request_vote_client_.setRequestTimeout(min(request_vote_client_.getDefaultRequestTimeout(),
+ update_interval));
+
+ startPeriodic(update_interval);
+
+ trace(TraceRaftCoreInited, update_interval.toUSec());
+
+ UAVCAN_ASSERT(res >= 0);
+ return 0;
+ }
+
+ /**
+ * This function is mostly needed for testing.
+ */
+ Log::Index getCommitIndex() const { return commit_index_; }
+
+ /**
+ * This essentially indicates whether the server could replicate log since last allocation.
+ */
+ bool areAllLogEntriesCommitted() const { return commit_index_ == persistent_state_.getLog().getLastIndex(); }
+
+ /**
+ * Only the leader can call @ref appendLog().
+ */
+ bool isLeader() const { return server_state_ == ServerStateLeader; }
+
+ /**
+ * Inserts one entry into log.
+ * This method will trigger an assertion failure and return error if the current node is not the leader.
+ * If operation fails, the node may give up its Leader status.
+ */
+ void appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id)
+ {
+ if (isLeader())
+ {
+ Entry entry;
+ entry.node_id = node_id.get();
+ entry.unique_id = unique_id;
+ entry.term = persistent_state_.getCurrentTerm();
+
+ trace(TraceRaftNewLogEntry, entry.node_id);
+ const int res = persistent_state_.getLog().append(entry);
+ if (res < 0)
+ {
+ handlePersistentStateUpdateError(res);
+ }
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * This class is used to perform log searches.
+ */
+ struct LogEntryInfo
+ {
+ Entry entry;
+ bool committed;
+
+ LogEntryInfo(const Entry& arg_entry, bool arg_committed)
+ : entry(arg_entry)
+ , committed(arg_committed)
+ { }
+ };
+
+ /**
+ * This method is used by the allocator to query existence of certain entries in the Raft log.
+ * Predicate is a callable of the following prototype:
+ * bool (const LogEntryInfo& entry)
+ * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy
+ * contructor with the last visited entry; otherwise the constructor will not be initialized.
+ * In this case, lazy constructor is used as boost::optional.
+ * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last.
+ */
+ template <typename Predicate>
+ inline LazyConstructor<LogEntryInfo> traverseLogFromEndUntil(const Predicate& predicate) const
+ {
+ UAVCAN_ASSERT(coerceOrFallback<bool>(predicate, true));
+ for (int index = static_cast<int>(persistent_state_.getLog().getLastIndex()); index >= 0; index--)
+ {
+ const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index));
+ UAVCAN_ASSERT(entry != UAVCAN_NULLPTR);
+ const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_);
+ if (predicate(info))
+ {
+ LazyConstructor<LogEntryInfo> ret;
+ ret.template construct<const LogEntryInfo&>(info);
+ return ret;
+ }
+ }
+ return LazyConstructor<LogEntryInfo>();
+ }
+
+ Log::Index getNumAllocations() const
+ {
+ // Remember that index zero contains a special-purpose entry that doesn't count as allocation
+ return persistent_state_.getLog().getLastIndex();
+ }
+
+ /**
+ * These accessors are needed for debugging, visualization and testing.
+ */
+ const PersistentState& getPersistentState() const { return persistent_state_; }
+ const ClusterManager& getClusterManager() const { return cluster_; }
+ MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; }
+ ServerState getServerState() const { return server_state_; }
+ MonotonicDuration getUpdateInterval() const { return getPeriod(); }
+ MonotonicDuration getRandomizedTimeout() const { return randomized_activity_timeout_; }
+};
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,354 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/abstract_server.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace distributed
+{
+/**
+ * This class implements the top-level allocation logic and server API.
+ */
+class UAVCAN_EXPORT Server : public AbstractServer
+ , IRaftLeaderMonitor
+{
+ struct UniqueIDLogPredicate
+ {
+ const UniqueID unique_id;
+
+ UniqueIDLogPredicate(const UniqueID& uid)
+ : unique_id(uid)
+ { }
+
+ bool operator()(const RaftCore::LogEntryInfo& info) const
+ {
+ return info.entry.unique_id == unique_id;
+ }
+ };
+
+ struct NodeIDLogPredicate
+ {
+ const NodeID node_id;
+
+ NodeIDLogPredicate(const NodeID& nid)
+ : node_id(nid)
+ { }
+
+ bool operator()(const RaftCore::LogEntryInfo& info) const
+ {
+ return info.entry.node_id == node_id.get();
+ }
+ };
+
+ /*
+ * States
+ */
+ RaftCore raft_core_;
+
+ /*
+ * Methods of IAllocationRequestHandler
+ */
+ virtual bool canPublishFollowupAllocationResponse() const
+ {
+ /*
+ * The server is allowed to publish follow-up allocation responses only if both conditions are met:
+ * - The server is leader.
+ * - The last allocation request has been completed successfully.
+ *
+ * Why second condition? Imagine a case when there's two Raft nodes that don't hear each other - A and B,
+ * both of them are leaders (but only A can commit to the log, B is in a minor partition); then there's a
+ * client X that can exchange with both leaders, and a client Y that can exchange only with A. Such a
+ * situation can occur in case of a very unlikely failure of redundant interfaces.
+ *
+ * Both clients X and Y initially send a first-stage Allocation request; A responds to Y with a first-stage
+ * response, whereas B responds to X. Both X and Y will issue a follow-up second-stage requests, which may
+ * cause A to mix second-stage Allocation requests from different nodes, leading to reception of an invalid
+ * unique ID. When both leaders receive full unique IDs (A will receive an invalid one, B will receive a valid
+ * unique ID of X), only A will be able to make a commit, because B is in a minority. Since both clients were
+ * unable to receive node ID values in this round, they will try again later.
+ *
+ * Now, in order to prevent B from disrupting client-server communication second time around, we introduce this
+ * second restriction: the server cannot exchange with clients as long as its log contains uncommitted entries.
+ *
+ * Note that this restriction does not apply to allocation requests sent via CAN FD frames, as in this case
+ * no follow-up responses are necessary. So only CAN FD can offer reliable Allocation exchange.
+ */
+ return raft_core_.isLeader() && raft_core_.areAllLogEntriesCommitted();
+ }
+
+ virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id)
+ {
+ /*
+ * Note that it is possible that the local node is not leader. We will still perform the log search
+ * and try to find the node that requested allocation. If the node is found, response will be sent;
+ * otherwise the request will be ignored because only leader can add new allocations.
+ */
+ const LazyConstructor<RaftCore::LogEntryInfo> result =
+ raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id));
+
+ if (result.isConstructed())
+ {
+ if (result->committed)
+ {
+ tryPublishAllocationResult(result->entry);
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Server",
+ "Allocation request served with existing allocation; node ID %d",
+ int(result->entry.node_id));
+ }
+ else
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Server",
+ "Allocation request ignored - allocation exists but not committed yet; node ID %d",
+ int(result->entry.node_id));
+ }
+ }
+ else
+ {
+ if (raft_core_.isLeader() && !node_discoverer_.hasUnknownNodes())
+ {
+ allocateNewNode(unique_id, preferred_node_id);
+ }
+ }
+ }
+
+ /*
+ * Methods of INodeDiscoveryHandler
+ */
+ virtual bool canDiscoverNewNodes() const
+ {
+ return raft_core_.isLeader();
+ }
+
+ virtual NodeAwareness checkNodeAwareness(NodeID node_id) const
+ {
+ const LazyConstructor<RaftCore::LogEntryInfo> result =
+ raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id));
+ if (result.isConstructed())
+ {
+ return result->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
+ }
+ else
+ {
+ return NodeAwarenessUnknown;
+ }
+ }
+
+ virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
+ {
+ if (raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)).isConstructed())
+ {
+ UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that
+ return;
+ }
+
+ const UniqueID uid = (unique_id_or_null == UAVCAN_NULLPTR) ? UniqueID() : *unique_id_or_null;
+
+ if (raft_core_.isLeader())
+ {
+ raft_core_.appendLog(uid, node_id);
+ }
+ }
+
+ /*
+ * Methods of IRaftLeaderMonitor
+ */
+ virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry)
+ {
+ /*
+ * Maybe this node did not request allocation at all, we don't care, we publish anyway.
+ */
+ tryPublishAllocationResult(entry);
+ }
+
+ virtual void handleLocalLeadershipChange(bool local_node_is_leader)
+ {
+ if (!local_node_is_leader)
+ {
+ return;
+ }
+
+ const LazyConstructor<RaftCore::LogEntryInfo> result =
+ raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID()));
+
+ if (!result.isConstructed())
+ {
+ raft_core_.appendLog(getOwnUniqueID(), node_.getNodeID());
+ }
+ }
+
+ /*
+ * Private methods
+ */
+ bool isNodeIDTaken(const NodeID node_id) const
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Server",
+ "Testing if node ID %d is taken", int(node_id.get()));
+ return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id));
+ }
+
+ void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id)
+ {
+ const NodeID allocated_node_id =
+ NodeIDSelector<Server>(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id);
+ if (!allocated_node_id.isUnicast())
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left");
+ return;
+ }
+
+ UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d",
+ int(allocated_node_id.get()));
+ raft_core_.appendLog(unique_id, allocated_node_id);
+ }
+
+ void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry)
+ {
+ const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id);
+ if (res < 0)
+ {
+ tracer_.onEvent(TraceError, res);
+ node_.registerInternalFailure("Dynamic allocation response");
+ }
+ }
+
+public:
+ Server(INode& node,
+ IStorageBackend& storage,
+ IEventTracer& tracer)
+ : AbstractServer(node, tracer)
+ , raft_core_(node, storage, tracer, *this)
+ { }
+
+ int init(const UniqueID& own_unique_id,
+ const uint8_t cluster_size = ClusterManager::ClusterSizeUnknown,
+ const TransferPriority priority = TransferPriority::OneHigherThanLowest)
+ {
+ /*
+ * Initializing Raft core first, because the next step requires Log to be loaded
+ */
+ int res = raft_core_.init(cluster_size, priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ /*
+ * Common logic
+ */
+ res = AbstractServer::init(own_unique_id, priority);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ /*
+ * Making sure that the server is started with the same node ID
+ */
+ const LazyConstructor<RaftCore::LogEntryInfo> own_log_entry =
+ raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID()));
+
+ if (own_log_entry.isConstructed())
+ {
+ if (own_log_entry->entry.unique_id != getOwnUniqueID())
+ {
+ return -ErrInvalidConfiguration;
+ }
+ }
+
+ return 0;
+ }
+
+ Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); }
+
+ /**
+ * These accessors are needed for debugging, visualization and testing.
+ */
+ const RaftCore& getRaftCore() const { return raft_core_; }
+};
+
+/**
+ * This structure represents immediate state of the server.
+ * It can be used for state visualization and debugging.
+ */
+struct StateReport
+{
+ uint8_t cluster_size;
+
+ RaftCore::ServerState state;
+
+ Log::Index last_log_index;
+ Log::Index commit_index;
+
+ Term last_log_term;
+ Term current_term;
+
+ NodeID voted_for;
+
+ MonotonicTime last_activity_timestamp;
+ MonotonicDuration randomized_timeout;
+
+ uint8_t num_unknown_nodes;
+
+ struct FollowerState
+ {
+ NodeID node_id;
+ Log::Index next_index;
+ Log::Index match_index;
+
+ FollowerState()
+ : next_index(0)
+ , match_index(0)
+ { }
+ } followers[ClusterManager::MaxClusterSize - 1];
+
+ StateReport(const Server& s)
+ : cluster_size (s.getRaftCore().getClusterManager().getClusterSize())
+ , state (s.getRaftCore().getServerState())
+ , last_log_index (s.getRaftCore().getPersistentState().getLog().getLastIndex())
+ , commit_index (s.getRaftCore().getCommitIndex())
+ , last_log_term (0) // See below
+ , current_term (s.getRaftCore().getPersistentState().getCurrentTerm())
+ , voted_for (s.getRaftCore().getPersistentState().getVotedFor())
+ , last_activity_timestamp(s.getRaftCore().getLastActivityTimestamp())
+ , randomized_timeout (s.getRaftCore().getRandomizedTimeout())
+ , num_unknown_nodes (s.getNodeDiscoverer().getNumUnknownNodes())
+ {
+ const Entry* const e = s.getRaftCore().getPersistentState().getLog().getEntryAtIndex(last_log_index);
+ UAVCAN_ASSERT(e != UAVCAN_NULLPTR);
+ if (e != UAVCAN_NULLPTR)
+ {
+ last_log_term = e->term;
+ }
+
+ for (uint8_t i = 0; i < (cluster_size - 1U); i++)
+ {
+ const ClusterManager& mgr = s.getRaftCore().getClusterManager();
+ const NodeID node_id = mgr.getRemoteServerNodeIDAtIndex(i);
+ if (node_id.isUnicast())
+ {
+ followers[i].node_id = node_id;
+ followers[i].next_index = mgr.getServerNextIndex(node_id);
+ followers[i].match_index = mgr.getServerMatchIndex(node_id);
+ }
+ }
+ }
+};
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace distributed
+{
+
+using namespace ::uavcan::protocol::dynamic_node_id::server;
+
+/**
+ * Raft term
+ */
+typedef StorageType<Entry::FieldTypes::term>::Type Term;
+
+}
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+/**
+ * @ref IEventTracer.
+ * Event codes cannot be changed, only new ones can be added.
+ */
+enum UAVCAN_EXPORT TraceCode
+{
+ // Event name Argument
+ // 0
+ TraceError, // error code (may be negated)
+ TraceRaftLogLastIndexRestored, // recovered last index value
+ TraceRaftLogAppend, // index of new entry
+ TraceRaftLogRemove, // new last index value
+ TraceRaftCurrentTermRestored, // current term
+ // 5
+ TraceRaftCurrentTermUpdate, // current term
+ TraceRaftVotedForRestored, // value of votedFor
+ TraceRaftVotedForUpdate, // value of votedFor
+ TraceRaftDiscoveryBroadcast, // number of known servers
+ TraceRaftNewServerDiscovered, // node ID of the new server
+ // 10
+ TraceRaftDiscoveryReceived, // node ID of the sender
+ TraceRaftClusterSizeInited, // cluster size
+ TraceRaftBadClusterSizeReceived, // received cluster size
+ TraceRaftCoreInited, // update interval in usec
+ TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader
+ // 15
+ Trace0,
+ TraceRaftNewLogEntry, // node ID value
+ TraceRaftRequestIgnored, // node ID of the client
+ TraceRaftVoteRequestReceived, // node ID of the client
+ TraceRaftVoteRequestSucceeded, // node ID of the server
+ // 20
+ TraceRaftVoteRequestInitiation, // node ID of the server
+ TraceRaftPersistStateUpdateError, // negative error code
+ TraceRaftCommitIndexUpdate, // new commit index value
+ TraceRaftNewerTermInResponse, // new term value
+ TraceRaftNewEntryCommitted, // new commit index value
+ // 25
+ TraceRaftAppendEntriesCallFailure, // error code (may be negated)
+ TraceRaftElectionComplete, // number of votes collected
+ TraceRaftAppendEntriesRespUnsucfl, // node ID of the client
+ Trace2,
+ Trace3,
+ // 30
+ TraceAllocationFollowupResponse, // number of unique ID bytes in this response
+ TraceAllocationFollowupDenied, // reason code (see sources for details)
+ TraceAllocationFollowupTimeout, // timeout value in microseconds
+ TraceAllocationBadRequest, // number of unique ID bytes in this request
+ TraceAllocationUnexpectedStage, // stage number in the request - 1, 2, or 3
+ // 35
+ TraceAllocationRequestAccepted, // number of bytes of unique ID after request
+ TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit big endian
+ TraceAllocationResponse, // allocated node ID
+ TraceAllocationActivity, // source node ID of the message
+ Trace12,
+ // 40
+ TraceDiscoveryNewNodeFound, // node ID
+ TraceDiscoveryCommitCacheUpdated, // node ID marked as committed
+ TraceDiscoveryNodeFinalized, // node ID in lower 7 bits, bit 8 (256, 0x100) is set if unique ID is known
+ TraceDiscoveryGetNodeInfoFailure, // node ID
+ TraceDiscoveryTimerStart, // interval in microseconds
+ // 45
+ TraceDiscoveryTimerStop, // reason code (see sources for details)
+ TraceDiscoveryGetNodeInfoRequest, // target node ID
+ TraceDiscoveryNodeRestartDetected, // node ID
+ TraceDiscoveryNodeRemoved, // node ID
+ Trace22,
+ // 50
+
+ NumTraceCodes
+};
+
+/**
+ * This interface allows the application to trace events that happen in the server.
+ */
+class UAVCAN_EXPORT IEventTracer
+{
+public:
+#if UAVCAN_TOSTRING
+ /**
+ * It is safe to call this function with any argument.
+ * If the event code is out of range, an assertion failure will be triggered and an error text will be returned.
+ */
+ static const char* getEventName(TraceCode code)
+ {
+ // import re;m = lambda s:',\n'.join('"%s"' % x for x in re.findall(r'\ {4}Trace[0-9]*([A-Za-z0-9]*),',s))
+ static const char* const Strings[] =
+ {
+ "Error",
+ "RaftLogLastIndexRestored",
+ "RaftLogAppend",
+ "RaftLogRemove",
+ "RaftCurrentTermRestored",
+ "RaftCurrentTermUpdate",
+ "RaftVotedForRestored",
+ "RaftVotedForUpdate",
+ "RaftDiscoveryBroadcast",
+ "RaftNewServerDiscovered",
+ "RaftDiscoveryReceived",
+ "RaftClusterSizeInited",
+ "RaftBadClusterSizeReceived",
+ "RaftCoreInited",
+ "RaftStateSwitch",
+ "",
+ "RaftNewLogEntry",
+ "RaftRequestIgnored",
+ "RaftVoteRequestReceived",
+ "RaftVoteRequestSucceeded",
+ "RaftVoteRequestInitiation",
+ "RaftPersistStateUpdateError",
+ "RaftCommitIndexUpdate",
+ "RaftNewerTermInResponse",
+ "RaftNewEntryCommitted",
+ "RaftAppendEntriesCallFailure",
+ "RaftElectionComplete",
+ "RaftAppendEntriesRespUnsucfl",
+ "",
+ "",
+ "AllocationFollowupResponse",
+ "AllocationFollowupDenied",
+ "AllocationFollowupTimeout",
+ "AllocationBadRequest",
+ "AllocationUnexpectedStage",
+ "AllocationRequestAccepted",
+ "AllocationExchangeComplete",
+ "AllocationResponse",
+ "AllocationActivity",
+ "",
+ "DiscoveryNewNodeFound",
+ "DiscoveryCommitCacheUpdated",
+ "DiscoveryNodeFinalized",
+ "DiscoveryGetNodeInfoFailure",
+ "DiscoveryTimerStart",
+ "DiscoveryTimerStop",
+ "DiscoveryGetNodeInfoRequest",
+ "DiscoveryNodeRestartDetected",
+ "DiscoveryNodeRemoved",
+ ""
+ };
+ uavcan::StaticAssert<sizeof(Strings) / sizeof(Strings[0]) == NumTraceCodes>::check();
+ UAVCAN_ASSERT(code < NumTraceCodes);
+ // coverity[dead_error_line]
+ return (code < NumTraceCodes) ? Strings[static_cast<unsigned>(code)] : "INVALID_EVENT_CODE";
+ }
+#endif
+
+ /**
+ * The server invokes this method every time it believes that a noteworthy event has happened.
+ * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures
+ * full backward compatibility.
+ * @param event_code Event code, see the sources for the enum with values.
+ * @param event_argument Value associated with the event; its meaning depends on the event code.
+ */
+ virtual void onEvent(TraceCode event_code, int64_t event_argument) = 0;
+
+ virtual ~IEventTracer() { }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,348 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/map.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/util/bitset.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+#include <cassert>
+// UAVCAN types
+#include <uavcan/protocol/NodeStatus.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+/**
+ * The allocator must implement this interface.
+ */
+class INodeDiscoveryHandler
+{
+public:
+ /**
+ * In order to avoid bus congestion, normally only leader can discover new nodes.
+ */
+ virtual bool canDiscoverNewNodes() const = 0;
+
+ /**
+ * These values represent the level of awareness of a certain node by the server.
+ */
+ enum NodeAwareness
+ {
+ NodeAwarenessUnknown,
+ NodeAwarenessKnownButNotCommitted,
+ NodeAwarenessKnownAndCommitted
+ };
+
+ /**
+ * It is OK to do full log traversal in this method, because the unique ID collector will cache the
+ * result when possible.
+ */
+ virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0;
+
+ /**
+ * This method will be called when a new node responds to GetNodeInfo request.
+ * If this method fails to register the node, the node will be queried again later and this method will be
+ * invoked again.
+ * Unique ID will be UAVCAN_NULLPTR if the node is assumed to not implement the GetNodeInfo service.
+ */
+ virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0;
+
+ virtual ~INodeDiscoveryHandler() { }
+};
+
+/**
+ * This class listens to NodeStatus messages from other nodes and retrieves their unique ID if they are not
+ * known to the allocator.
+ */
+class NodeDiscoverer : TimerBase
+{
+ typedef MethodBinder<NodeDiscoverer*, void (NodeDiscoverer::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
+ GetNodeInfoResponseCallback;
+
+ typedef MethodBinder<NodeDiscoverer*, void (NodeDiscoverer::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
+ NodeStatusCallback;
+
+ struct NodeData
+ {
+ uint32_t last_seen_uptime;
+ uint8_t num_get_node_info_attempts;
+
+ NodeData()
+ : last_seen_uptime(0)
+ , num_get_node_info_attempts(0)
+ { }
+ };
+
+ typedef Map<NodeID, NodeData> NodeMap;
+
+ /**
+ * When this number of attempts has been made, the discoverer will give up and assume that the node
+ * does not implement this service.
+ */
+ enum { MaxAttemptsToGetNodeInfo = 5 };
+
+ enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3)
+
+ /*
+ * States
+ */
+ INodeDiscoveryHandler& handler_;
+ IEventTracer& tracer_;
+
+ BitSet<NodeID::Max + 1> committed_node_mask_; ///< Nodes that are marked will not be queried
+ NodeMap node_map_;
+
+ ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
+ Subscriber<protocol::NodeStatus, NodeStatusCallback> node_status_sub_;
+
+ /*
+ * Methods
+ */
+ void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
+
+ INode& getNode() { return node_status_sub_.getNode(); }
+
+ void removeNode(const NodeID node_id)
+ {
+ node_map_.remove(node_id);
+ trace(TraceDiscoveryNodeRemoved, node_id.get());
+ }
+
+ NodeID pickNextNodeToQuery() const
+ {
+ // This essentially means that we pick first available node. Remember that the map is unordered.
+ const NodeMap::KVPair* const pair = node_map_.getByIndex(0);
+ return (pair == UAVCAN_NULLPTR) ? NodeID() : pair->key;
+ }
+
+ bool needToQuery(NodeID node_id)
+ {
+ UAVCAN_ASSERT(node_id.isUnicast());
+
+ /*
+ * Fast check
+ */
+ if (committed_node_mask_[node_id.get()])
+ {
+ return false;
+ }
+
+ /*
+ * Slow check - may involve full log search
+ */
+ const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id);
+
+ if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown)
+ {
+ return true;
+ }
+ else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted)
+ {
+ removeNode(node_id);
+ return false;
+ }
+ else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted)
+ {
+ trace(TraceDiscoveryCommitCacheUpdated, node_id.get());
+ committed_node_mask_[node_id.get()] = true;
+ removeNode(node_id);
+ return false;
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ return false;
+ }
+ }
+
+ NodeID pickNextNodeToQueryAndCleanupMap()
+ {
+ NodeID node_id;
+ do
+ {
+ node_id = pickNextNodeToQuery();
+ if (node_id.isUnicast())
+ {
+ if (needToQuery(node_id))
+ {
+ return node_id;
+ }
+ else
+ {
+ removeNode(node_id);
+ }
+ }
+ }
+ while (node_id.isUnicast());
+ return NodeID();
+ }
+
+ void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
+ {
+ trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == UAVCAN_NULLPTR) ? 0U : 0x100U));
+ removeNode(node_id);
+ /*
+ * It is paramount to check if the server is still interested to receive this data.
+ * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with
+ * duplicate node ID in the log.
+ */
+ if (needToQuery(node_id))
+ {
+ handler_.handleNewNodeDiscovery(unique_id_or_null, node_id);
+ }
+ }
+
+ void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
+ {
+ if (result.isSuccessful())
+ {
+ UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
+ int(result.getCallID().server_node_id.get()));
+ finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
+ }
+ else
+ {
+ trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
+
+ NodeData* const data = node_map_.access(result.getCallID().server_node_id);
+ if (data == UAVCAN_NULLPTR)
+ {
+ return; // Probably it is a known node now
+ }
+
+ UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
+ "GetNodeInfo request to %d has timed out, %d attempts",
+ int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
+ data->num_get_node_info_attempts++;
+ if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
+ {
+ finalizeNodeDiscovery(UAVCAN_NULLPTR, result.getCallID().server_node_id);
+ // Warning: data pointer is invalidated now
+ }
+ }
+ }
+
+ void handleTimerEvent(const TimerEvent&)
+ {
+ if (get_node_info_client_.hasPendingCalls())
+ {
+ return;
+ }
+
+ const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
+ if (!node_id.isUnicast())
+ {
+ trace(TraceDiscoveryTimerStop, 0);
+ stop();
+ return;
+ }
+
+ if (!handler_.canDiscoverNewNodes())
+ {
+ return; // Timer must continue to run in order to not stuck when it unlocks
+ }
+
+ trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
+
+ UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
+ int(node_id.get()));
+ const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
+ if (res < 0)
+ {
+ getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call");
+ }
+ }
+
+ void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+ {
+ if (!needToQuery(msg.getSrcNodeID()))
+ {
+ return;
+ }
+
+ NodeData* data = node_map_.access(msg.getSrcNodeID());
+ if (data == UAVCAN_NULLPTR)
+ {
+ trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());
+
+ data = node_map_.insert(msg.getSrcNodeID(), NodeData());
+ if (data == UAVCAN_NULLPTR)
+ {
+ getNode().registerInternalFailure("NodeDiscoverer OOM");
+ return;
+ }
+ }
+ UAVCAN_ASSERT(data != UAVCAN_NULLPTR);
+
+ if (msg.uptime_sec < data->last_seen_uptime)
+ {
+ trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get());
+ data->num_get_node_info_attempts = 0;
+ }
+ data->last_seen_uptime = msg.uptime_sec;
+
+ if (!isRunning())
+ {
+ startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
+ trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
+ }
+ }
+
+public:
+ NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler)
+ : TimerBase(node)
+ , handler_(handler)
+ , tracer_(tracer)
+ , node_map_(node.getAllocator())
+ , get_node_info_client_(node)
+ , node_status_sub_(node)
+ { }
+
+ int init(const TransferPriority priority)
+ {
+ int res = get_node_info_client_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ get_node_info_client_.setCallback(
+ GetNodeInfoResponseCallback(this, &NodeDiscoverer::handleGetNodeInfoResponse));
+
+ res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ // Note: the timer starts ad-hoc from the node status callback, not here.
+
+ return 0;
+ }
+
+ /**
+ * Returns true if there's at least one node with pending GetNodeInfo.
+ */
+ bool hasUnknownNodes() const { return !node_map_.isEmpty(); }
+
+ /**
+ * Returns number of nodes that are being queried at the moment.
+ * This method is needed for testing and state visualization.
+ */
+ uint8_t getNumUnknownNodes() const { return static_cast<uint8_t>(node_map_.getSize()); }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+/**
+ * Node ID allocation logic
+ */
+template <typename Owner>
+class NodeIDSelector
+{
+ typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const;
+
+ const Owner* const owner_;
+ const IsNodeIDTakenMethod is_node_id_taken_;
+
+public:
+ NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken)
+ : owner_(owner)
+ , is_node_id_taken_(is_node_id_taken)
+ {
+ UAVCAN_ASSERT(owner_ != UAVCAN_NULLPTR);
+ UAVCAN_ASSERT(is_node_id_taken_ != UAVCAN_NULLPTR);
+ }
+
+ /**
+ * Reutrns a default-constructed (invalid) node ID if a free one could not be found.
+ */
+ NodeID findFreeNodeID(const NodeID preferred) const
+ {
+ uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
+
+ // Up
+ while (candidate <= NodeID::MaxRecommendedForRegularNodes)
+ {
+ if (!(owner_->*is_node_id_taken_)(candidate))
+ {
+ return candidate;
+ }
+ candidate++;
+ }
+
+ candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
+
+ // Down
+ while (candidate > 0)
+ {
+ if (!(owner_->*is_node_id_taken_)(candidate))
+ {
+ return candidate;
+ }
+ candidate--;
+ }
+
+ return NodeID();
+ }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/marshal/types.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+/**
+ * This interface is used by the server to read and write stable storage.
+ * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32
+ * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward
+ * and efficient implementation of storage backends, e.g. based on text files.
+ * Keys and values may contain only non-whitespace, non-formatting printable characters.
+ */
+class UAVCAN_EXPORT IStorageBackend
+{
+public:
+ /**
+ * Maximum length of keys and values. One pair takes twice as much space.
+ */
+ enum { MaxStringLength = 32 };
+
+ /**
+ * It is guaranteed that the server will never require more than this number of key/value pairs.
+ * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead.
+ */
+ enum { MaxKeyValuePairs = 512 };
+
+ /**
+ * This type is used to exchange data chunks with the backend.
+ * It doesn't use any dynamic memory; please refer to the Array<> class for details.
+ */
+ typedef MakeString<MaxStringLength>::Type String;
+
+ /**
+ * Read one value from the storage.
+ * If such key does not exist, or if read failed, an empty string will be returned.
+ * This method should not block for more than 50 ms.
+ */
+ virtual String get(const String& key) const = 0;
+
+ /**
+ * Create or update value for the given key. Empty value should be regarded as a request to delete the key.
+ * This method should not block for more than 50 ms.
+ * Failures will be ignored.
+ */
+ virtual void set(const String& key, const String& value) = 0;
+
+ virtual ~IStorageBackend() { }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_backend.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+#include <cstdlib>
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <cerrno>
+#endif
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+/**
+ * This class extends the storage backend interface with serialization/deserialization functionality.
+ */
+class StorageMarshaller
+{
+ IStorageBackend& storage_;
+
+ static uint8_t convertLowerCaseHexCharToNibble(char ch)
+ {
+ const uint8_t ret = (ch > '9') ? static_cast<uint8_t>(ch - 'a' + 10) : static_cast<uint8_t>(ch - '0');
+ UAVCAN_ASSERT(ret < 16);
+ return ret;
+ }
+
+public:
+ StorageMarshaller(IStorageBackend& storage)
+ : storage_(storage)
+ { }
+
+ /**
+ * Turns a unique ID into a hex string that can be used as a key or as a value.
+ */
+ static IStorageBackend::String convertUniqueIDToHex(const UniqueID& key)
+ {
+ IStorageBackend::String serialized;
+ for (uint8_t i = 0; i < UniqueID::MaxSize; i++)
+ {
+ serialized.appendFormatted("%02x", key.at(i));
+ }
+ UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2);
+ return serialized;
+ }
+
+ /**
+ * These methods set the value and then immediately read it back.
+ * 1. Serialize the value.
+ * 2. Update the value on the backend.
+ * 3. Call get() with the same value argument.
+ * The caller then is supposed to check whether the argument has the desired value.
+ */
+ int setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value)
+ {
+ IStorageBackend::String serialized;
+ serialized.appendFormatted("%llu", static_cast<unsigned long long>(inout_value));
+
+ UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str());
+ storage_.set(key, serialized);
+
+ return get(key, inout_value);
+ }
+
+ int setAndGetBack(const IStorageBackend::String& key, UniqueID& inout_value)
+ {
+ const IStorageBackend::String serialized = convertUniqueIDToHex(inout_value);
+
+ UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str());
+ storage_.set(key, serialized);
+
+ return get(key, inout_value);
+ }
+
+ /**
+ * Getters simply read and deserialize the value.
+ * 1. Read the value back from the backend; return false if read fails.
+ * 2. Deserealize the newly read value; return false if deserialization fails.
+ * 3. Update the argument with deserialized value.
+ * 4. Return true.
+ */
+ int get(const IStorageBackend::String& key, uint32_t& out_value) const
+ {
+ /*
+ * Reading the storage
+ */
+ const IStorageBackend::String val = storage_.get(key);
+ if (val.empty())
+ {
+ return -ErrFailure;
+ }
+
+ /*
+ * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno.
+ * The value must contain only numeric characters.
+ */
+ for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it)
+ {
+ if (static_cast<char>(*it) < '0' || static_cast<char>(*it) > '9')
+ {
+ return -ErrFailure;
+ }
+ }
+
+ if (val.size() > 10) // len(str(0xFFFFFFFF))
+ {
+ return -ErrFailure;
+ }
+
+ /*
+ * Conversion is carried out here
+ */
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ errno = 0;
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ const unsigned long long x = std::strtoull(val.c_str(), UAVCAN_NULLPTR, 10);
+#else
+ // There was no strtoull() before C++11, so we need to resort to strtoul()
+ StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check();
+ const unsigned long x = std::strtoul(val.c_str(), UAVCAN_NULLPTR, 10);
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ if (errno != 0)
+ {
+ return -ErrFailure;
+ }
+#endif
+
+ out_value = static_cast<uint32_t>(x);
+ return 0;
+ }
+
+ int get(const IStorageBackend::String& key, UniqueID& out_value) const
+ {
+ static const uint8_t NumBytes = UniqueID::MaxSize;
+
+ /*
+ * Reading the storage
+ */
+ IStorageBackend::String val = storage_.get(key);
+ if (val.size() != NumBytes * 2)
+ {
+ return -ErrFailure;
+ }
+
+ /*
+ * The value must contain only hexadecimal numbers.
+ */
+ val.convertToLowerCaseASCII();
+ for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it)
+ {
+ if ((static_cast<char>(*it) < '0' || static_cast<char>(*it) > '9') &&
+ (static_cast<char>(*it) < 'a' || static_cast<char>(*it) > 'f'))
+ {
+ return -ErrFailure;
+ }
+ }
+
+ /*
+ * Conversion is carried out here
+ */
+ IStorageBackend::String::const_iterator it = val.begin();
+
+ for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++)
+ {
+ out_value[byte_index] =
+ static_cast<uint8_t>(convertLowerCaseHexCharToNibble(static_cast<char>(*it++)) << 4);
+ out_value[byte_index] =
+ static_cast<uint8_t>(convertLowerCaseHexCharToNibble(static_cast<char>(*it++)) | out_value[byte_index]);
+ }
+
+ return 0;
+ }
+};
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+// UAVCAN types
+#include <uavcan/protocol/dynamic_node_id/server/Entry.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+
+using namespace ::uavcan::protocol::dynamic_node_id;
+
+/**
+ * Node Unique ID
+ */
+typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID;
+
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/file_server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,268 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+// UAVCAN types
+#include <uavcan/protocol/file/GetInfo.hpp>
+#include <uavcan/protocol/file/GetDirectoryEntryInfo.hpp>
+#include <uavcan/protocol/file/Read.hpp>
+#include <uavcan/protocol/file/Write.hpp>
+#include <uavcan/protocol/file/Delete.hpp>
+
+namespace uavcan
+{
+/**
+ * The file server backend should implement this interface.
+ * Note that error codes returned by these methods are defined in uavcan.protocol.file.Error; these are
+ * not the same as libuavcan-internal error codes defined in uavcan.error.hpp.
+ */
+class UAVCAN_EXPORT IFileServerBackend
+{
+public:
+ typedef protocol::file::Path::FieldTypes::path Path;
+ typedef protocol::file::EntryType EntryType;
+ typedef protocol::file::Error Error;
+
+ /**
+ * All read operations must return this number of bytes, unless end of file is reached.
+ */
+ enum { ReadSize = protocol::file::Read::Response::FieldTypes::data::MaxSize };
+
+ /**
+ * Shortcut for uavcan.protocol.file.Path.SEPARATOR.
+ */
+ static char getPathSeparator() { return static_cast<char>(protocol::file::Path::SEPARATOR); }
+
+ /**
+ * Backend for uavcan.protocol.file.GetInfo.
+ * Refer to uavcan.protocol.file.EntryType for the list of available bit flags.
+ * Implementation of this method is required.
+ * On success the method must return zero.
+ */
+ virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) = 0;
+
+ /**
+ * Backend for uavcan.protocol.file.Read.
+ * Implementation of this method is required.
+ * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except
+ * if the end of file is reached.
+ * On success the method must return zero.
+ */
+ virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0;
+
+ // Methods below are optional.
+
+ /**
+ * Backend for uavcan.protocol.file.Write.
+ * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
+ * On success the method must return zero.
+ */
+ virtual int16_t write(const Path& path, const uint64_t offset, const uint8_t* buffer, const uint16_t size)
+ {
+ (void)path;
+ (void)offset;
+ (void)buffer;
+ (void)size;
+ return Error::NOT_IMPLEMENTED;
+ }
+
+ /**
+ * Backend for uavcan.protocol.file.Delete. ('delete' is a C++ keyword, so 'remove' is used instead)
+ * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
+ * On success the method must return zero.
+ */
+ virtual int16_t remove(const Path& path)
+ {
+ (void)path;
+ return Error::NOT_IMPLEMENTED;
+ }
+
+ /**
+ * Backend for uavcan.protocol.file.GetDirectoryEntryInfo.
+ * Refer to uavcan.protocol.file.EntryType for the list of available bit flags.
+ * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
+ * On success the method must return zero.
+ */
+ virtual int16_t getDirectoryEntryInfo(const Path& directory_path, const uint32_t entry_index,
+ EntryType& out_type, Path& out_entry_full_path)
+ {
+ (void)directory_path;
+ (void)entry_index;
+ (void)out_type;
+ (void)out_entry_full_path;
+ return Error::NOT_IMPLEMENTED;
+ }
+
+ virtual ~IFileServerBackend() { }
+};
+
+/**
+ * Basic file server implements only the following services:
+ * uavcan.protocol.file.GetInfo
+ * uavcan.protocol.file.Read
+ * Also see @ref IFileServerBackend.
+ */
+class BasicFileServer
+{
+ typedef MethodBinder<BasicFileServer*,
+ void (BasicFileServer::*)(const protocol::file::GetInfo::Request&, protocol::file::GetInfo::Response&)>
+ GetInfoCallback;
+
+ typedef MethodBinder<BasicFileServer*,
+ void (BasicFileServer::*)(const protocol::file::Read::Request&, protocol::file::Read::Response&)>
+ ReadCallback;
+
+ ServiceServer<protocol::file::GetInfo, GetInfoCallback> get_info_srv_;
+ ServiceServer<protocol::file::Read, ReadCallback> read_srv_;
+
+ void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp)
+ {
+ resp.error.value = backend_.getInfo(req.path.path, resp.size, resp.entry_type);
+ }
+
+ void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp)
+ {
+ uint16_t inout_size = resp.data.capacity();
+
+ resp.data.resize(inout_size);
+
+ resp.error.value = backend_.read(req.path.path, req.offset, resp.data.begin(), inout_size);
+
+ if (resp.error.value != protocol::file::Error::OK)
+ {
+ inout_size = 0;
+ }
+
+ if (inout_size > resp.data.capacity())
+ {
+ UAVCAN_ASSERT(0);
+ resp.error.value = protocol::file::Error::UNKNOWN_ERROR;
+ }
+ else
+ {
+ resp.data.resize(inout_size);
+ }
+ }
+
+protected:
+ IFileServerBackend& backend_; ///< Derived types can use it
+
+public:
+ BasicFileServer(INode& node, IFileServerBackend& backend)
+ : get_info_srv_(node)
+ , read_srv_(node)
+ , backend_(backend)
+ { }
+
+ int start()
+ {
+ int res = get_info_srv_.start(GetInfoCallback(this, &BasicFileServer::handleGetInfo));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = read_srv_.start(ReadCallback(this, &BasicFileServer::handleRead));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ return 0;
+ }
+};
+
+/**
+ * Full file server implements all file services:
+ * uavcan.protocol.file.GetInfo
+ * uavcan.protocol.file.Read
+ * uavcan.protocol.file.Write
+ * uavcan.protocol.file.Delete
+ * uavcan.protocol.file.GetDirectoryEntryInfo
+ * Also see @ref IFileServerBackend.
+ */
+class FileServer : protected BasicFileServer
+{
+ typedef MethodBinder<FileServer*,
+ void (FileServer::*)(const protocol::file::Write::Request&, protocol::file::Write::Response&)>
+ WriteCallback;
+
+ typedef MethodBinder<FileServer*,
+ void (FileServer::*)(const protocol::file::Delete::Request&, protocol::file::Delete::Response&)>
+ DeleteCallback;
+
+ typedef MethodBinder<FileServer*,
+ void (FileServer::*)(const protocol::file::GetDirectoryEntryInfo::Request&,
+ protocol::file::GetDirectoryEntryInfo::Response&)>
+ GetDirectoryEntryInfoCallback;
+
+ ServiceServer<protocol::file::Write, WriteCallback> write_srv_;
+ ServiceServer<protocol::file::Delete, DeleteCallback> delete_srv_;
+ ServiceServer<protocol::file::GetDirectoryEntryInfo, GetDirectoryEntryInfoCallback> get_directory_entry_info_srv_;
+
+ void handleWrite(const protocol::file::Write::Request& req, protocol::file::Write::Response& resp)
+ {
+ resp.error.value = backend_.write(req.path.path, req.offset, req.data.begin(), req.data.size());
+ }
+
+ void handleDelete(const protocol::file::Delete::Request& req, protocol::file::Delete::Response& resp)
+ {
+ resp.error.value = backend_.remove(req.path.path);
+ }
+
+ void handleGetDirectoryEntryInfo(const protocol::file::GetDirectoryEntryInfo::Request& req,
+ protocol::file::GetDirectoryEntryInfo::Response& resp)
+ {
+ resp.error.value = backend_.getDirectoryEntryInfo(req.directory_path.path, req.entry_index,
+ resp.entry_type, resp.entry_full_path.path);
+ }
+
+public:
+ FileServer(INode& node, IFileServerBackend& backend)
+ : BasicFileServer(node, backend)
+ , write_srv_(node)
+ , delete_srv_(node)
+ , get_directory_entry_info_srv_(node)
+ { }
+
+ int start()
+ {
+ int res = BasicFileServer::start();
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = write_srv_.start(WriteCallback(this, &FileServer::handleWrite));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = delete_srv_.start(DeleteCallback(this, &FileServer::handleDelete));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = get_directory_entry_info_srv_.start(
+ GetDirectoryEntryInfoCallback(this, &FileServer::handleGetDirectoryEntryInfo));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ return 0;
+ }
+};
+
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,476 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/util/map.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+// UAVCAN types
+#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
+
+namespace uavcan
+{
+/**
+ * Application-specific firmware version checking logic.
+ * Refer to @ref FirmwareUpdateTrigger for details.
+ */
+class IFirmwareVersionChecker
+{
+public:
+ /**
+ * This value is limited by the pool block size minus some extra data. Please refer to the Map<> documentation
+ * for details. If this size is set too high, the compilation will fail in the Map<> template.
+ */
+ enum { MaxFirmwareFilePathLength = 40 };
+
+ /**
+ * This type is used to store path to firmware file that the target node will retrieve using the
+ * service uavcan.protocol.file.Read. Note that the maximum length is limited due to some specifics of
+ * libuavcan (@ref MaxFirmwareFilePathLength), this is NOT a protocol-level limitation.
+ */
+ typedef MakeString<MaxFirmwareFilePathLength>::Type FirmwareFilePath;
+
+ /**
+ * This method will be invoked when the class obtains a response to GetNodeInfo request.
+ *
+ * @param node_id Node ID that this GetNodeInfo response was received from.
+ *
+ * @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details.
+ *
+ * @param out_firmware_file_path The implementation should return the firmware image path via this argument.
+ * Note that this path must be reachable via uavcan.protocol.file.Read service.
+ * Refer to @ref FileServer and @ref BasicFileServer for details.
+ *
+ * @return True - the class will begin sending update requests.
+ * False - the node will be ignored, no request will be sent.
+ */
+ virtual bool shouldRequestFirmwareUpdate(NodeID node_id, const protocol::GetNodeInfo::Response& node_info,
+ FirmwareFilePath& out_firmware_file_path) = 0;
+
+ /**
+ * This method will be invoked when a node responds to the update request with an error. If the request simply
+ * times out, this method will not be invoked.
+ * Note that if by the time of arrival of the response the node is already removed, this method will not be called.
+ *
+ * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting
+ * is not needed anymore. This method will not be invoked.
+ *
+ * @param node_id Node ID that returned this error.
+ *
+ * @param error_response Contents of the error response. It contains error code and text.
+ *
+ * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be
+ * initialized with old path, so if the same path needs to be reused, this
+ * argument should be left unchanged.
+ *
+ * @return True - the class will continue sending update requests with new firmware path.
+ * False - the node will be forgotten, new requests will not be sent.
+ */
+ virtual bool shouldRetryFirmwareUpdate(NodeID node_id,
+ const protocol::file::BeginFirmwareUpdate::Response& error_response,
+ FirmwareFilePath& out_firmware_file_path) = 0;
+
+ /**
+ * This node is invoked when the node responds to the update request with confirmation.
+ * Note that if by the time of arrival of the response the node is already removed, this method will not be called.
+ *
+ * Implementation is optional; default one does nothing.
+ *
+ * @param node_id Node ID that confirmed the request.
+ *
+ * @param response Actual response.
+ */
+ virtual void handleFirmwareUpdateConfirmation(NodeID node_id,
+ const protocol::file::BeginFirmwareUpdate::Response& response)
+ {
+ (void)node_id;
+ (void)response;
+ }
+
+ virtual ~IFirmwareVersionChecker() { }
+};
+
+/**
+ * This class subscribes to updates from @ref NodeInfoRetriever in order to detect nodes that need firmware
+ * updates. The decision process of whether a firmware update is needed is relayed to the application via
+ * @ref IFirmwareVersionChecker. If the application confirms that the update is needed, this class will begin
+ * sending uavcan.protocol.file.BeginFirmwareUpdate periodically (period is configurable) to every node that
+ * needs an update in a round-robin fashion. There are the following termination conditions for the periodical
+ * sending process:
+ *
+ * - The node responds with confirmation. In this case the class will forget about the node on the assumption
+ * that its job is done here. Confirmation will be reported to the application via the interface.
+ *
+ * - The node responds with an error, and the error code is not ERROR_IN_PROGRESS. In this case the class will
+ * request the application via the interface mentioned above about its further actions - either give up or
+ * retry, possibly with a different firmware.
+ *
+ * - The node responds with error ERROR_IN_PROGRESS. In this case the class behaves exactly in the same way as if
+ * response was successful (because the firmware is alredy being updated, so the goal is fulfilled).
+ * Confirmation will be reported to the application via the interface.
+ *
+ * - The node goes offline or restarts. In this case the node will be immediately forgotten, and the process
+ * will repeat again later because the node info retriever re-queries GetNodeInfo every time when a node restarts.
+ *
+ * Since the target node (i.e. node that is being updated) will try to retrieve the specified firmware file using
+ * the file services (uavcan.protocol.file.*), the provided firmware path must be reachable for the file server
+ * (@ref FileServer, @ref BasicFileServer). Normally, an application that serves as UAVCAN firmware update server
+ * will include at least the following components:
+ * - this firmware update trigger;
+ * - dynamic node ID allocation server;
+ * - file server.
+ *
+ * Implementation details: the class uses memory pool to keep the list of nodes that have not responded yet, which
+ * limits the maximum length of the path to the firmware file, which is covered in @ref IFirmwareVersionChecker.
+ * To somewhat relieve the maximum path length limitation, the class can be supplied with a common prefix that
+ * will be prepended to firmware pathes before sending requests.
+ * Interval at which requests are being sent is configurable, but the default value should cover the needs of
+ * virtually all use cases (as always).
+ */
+class FirmwareUpdateTrigger : public INodeInfoListener,
+ private TimerBase
+{
+ typedef MethodBinder<FirmwareUpdateTrigger*,
+ void (FirmwareUpdateTrigger::*)(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>&)>
+ BeginFirmwareUpdateResponseCallback;
+
+ typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath;
+
+ enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout.
+
+ struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable
+ {
+ enum { DefaultOutput = 0xFFU };
+
+ const FirmwareUpdateTrigger& owner;
+ uint8_t output;
+
+ NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner)
+ : owner(arg_owner)
+ , output(DefaultOutput)
+ { }
+
+ bool operator()(const NodeID node_id, const FirmwareFilePath&)
+ {
+ if (node_id.get() > owner.last_queried_node_id_ &&
+ !owner.begin_fw_update_client_.hasPendingCallToServer(node_id))
+ {
+ output = min(output, node_id.get());
+ }
+ return false;
+ }
+ };
+
+ /*
+ * State
+ */
+ ServiceClient<protocol::file::BeginFirmwareUpdate, BeginFirmwareUpdateResponseCallback> begin_fw_update_client_;
+
+ IFirmwareVersionChecker& checker_;
+
+ NodeInfoRetriever* node_info_retriever_;
+
+ Map<NodeID, FirmwareFilePath> pending_nodes_;
+
+ MonotonicDuration request_interval_;
+
+ FirmwareFilePath common_path_prefix_;
+
+ mutable uint8_t last_queried_node_id_;
+
+ /*
+ * Methods of INodeInfoListener
+ */
+ virtual void handleNodeInfoUnavailable(NodeID node_id)
+ {
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get()));
+ pending_nodes_.remove(node_id); // For extra paranoia
+ }
+
+ virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info)
+ {
+ FirmwareFilePath firmware_file_path;
+ const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path);
+ if (update_needed)
+ {
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s",
+ int(node_id.get()), firmware_file_path.c_str());
+ trySetPendingNode(node_id, firmware_file_path);
+ }
+ else
+ {
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get()));
+ pending_nodes_.remove(node_id);
+ }
+ }
+
+ virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
+ {
+ if (event.status.mode == protocol::NodeStatus::MODE_OFFLINE)
+ {
+ pending_nodes_.remove(event.node_id);
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get()));
+ }
+ }
+
+ /*
+ * Own methods
+ */
+ INode& getNode() { return begin_fw_update_client_.getNode(); }
+
+ void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path)
+ {
+ if (UAVCAN_NULLPTR != pending_nodes_.insert(node_id, path))
+ {
+ if (!TimerBase::isRunning())
+ {
+ TimerBase::startPeriodic(request_interval_);
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer started");
+ }
+ }
+ else
+ {
+ getNode().registerInternalFailure("FirmwareUpdateTrigger OOM");
+ }
+ }
+
+ NodeID pickNextNodeID() const
+ {
+ // We can't do index search because indices are unstable in Map<>
+ // First try - from the current node up
+ NextNodeIDSearchPredicate s1(*this);
+ (void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s1);
+
+ if (s1.output != NextNodeIDSearchPredicate::DefaultOutput)
+ {
+ last_queried_node_id_ = s1.output;
+ }
+ else if (last_queried_node_id_ != 0)
+ {
+ // Nothing was found, resetting the selector and trying again
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_));
+ last_queried_node_id_ = 0;
+
+ NextNodeIDSearchPredicate s2(*this);
+ (void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s2);
+
+ if (s2.output != NextNodeIDSearchPredicate::DefaultOutput)
+ {
+ last_queried_node_id_ = s2.output;
+ }
+ }
+ else
+ {
+ ; // Hopeless
+ }
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u",
+ int(last_queried_node_id_), pending_nodes_.getSize(),
+ begin_fw_update_client_.getNumPendingCalls());
+ return last_queried_node_id_;
+ }
+
+ void handleBeginFirmwareUpdateResponse(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>& result)
+ {
+ if (!result.isSuccessful())
+ {
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry",
+ int(result.getCallID().server_node_id.get()));
+ return;
+ }
+
+ FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id);
+ if (old_path == UAVCAN_NULLPTR)
+ {
+ // The entry has been removed, assuming that it's not needed anymore
+ return;
+ }
+
+ const bool confirmed =
+ result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK ||
+ result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS;
+
+ if (confirmed)
+ {
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request",
+ int(result.getCallID().server_node_id.get()));
+ pending_nodes_.remove(result.getCallID().server_node_id);
+ checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse());
+ }
+ else
+ {
+ UAVCAN_ASSERT(old_path != UAVCAN_NULLPTR);
+ UAVCAN_ASSERT(TimerBase::isRunning());
+ // We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer
+
+ const bool update_needed =
+ checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path);
+
+ if (!update_needed)
+ {
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry",
+ int(result.getCallID().server_node_id.get()));
+ pending_nodes_.remove(result.getCallID().server_node_id);
+ }
+ }
+ }
+
+ virtual void handleTimerEvent(const TimerEvent&)
+ {
+ if (pending_nodes_.isEmpty())
+ {
+ TimerBase::stop();
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped");
+ return;
+ }
+
+ const NodeID node_id = pickNextNodeID();
+ if (!node_id.isUnicast())
+ {
+ return;
+ }
+
+ FirmwareFilePath* const path = pending_nodes_.access(node_id);
+ if (path == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map
+ return;
+ }
+
+ protocol::file::BeginFirmwareUpdate::Request req;
+
+ req.source_node_id = getNode().getNodeID().get();
+ if (!common_path_prefix_.empty())
+ {
+ req.image_file_remote_path.path += common_path_prefix_.c_str();
+ req.image_file_remote_path.path.push_back(protocol::file::Path::SEPARATOR);
+ }
+ req.image_file_remote_path.path += path->c_str();
+
+ UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s",
+ int(node_id.get()), req.image_file_remote_path.path.c_str());
+
+ const int call_res = begin_fw_update_client_.call(node_id, req);
+ if (call_res < 0)
+ {
+ getNode().registerInternalFailure("FirmwareUpdateTrigger call");
+ }
+ }
+
+public:
+ FirmwareUpdateTrigger(INode& node, IFirmwareVersionChecker& checker)
+ : TimerBase(node)
+ , begin_fw_update_client_(node)
+ , checker_(checker)
+ , node_info_retriever_(UAVCAN_NULLPTR)
+ , pending_nodes_(node.getAllocator())
+ , request_interval_(MonotonicDuration::fromMSec(DefaultRequestIntervalMs))
+ , last_queried_node_id_(0)
+ { }
+
+ ~FirmwareUpdateTrigger()
+ {
+ if (node_info_retriever_ != UAVCAN_NULLPTR)
+ {
+ node_info_retriever_->removeListener(this);
+ }
+ }
+
+ /**
+ * Starts the object. Once started, it can't be stopped unless destroyed.
+ *
+ * @param node_info_retriever The object will register itself against this retriever.
+ * When the destructor is called, the object will unregister itself.
+ *
+ * @param common_path_prefix If set, this path will be prefixed to all firmware pathes provided by the
+ * application interface. The prefix does not need to end with path separator;
+ * the last trailing one will be removed (so use '//' if you need '/').
+ * By default the prefix is empty.
+ *
+ * @return Negative error code.
+ */
+ int start(NodeInfoRetriever& node_info_retriever,
+ const FirmwareFilePath& arg_common_path_prefix = FirmwareFilePath(),
+ const TransferPriority priority = TransferPriority::OneHigherThanLowest)
+ {
+ /*
+ * Configuring the node info retriever
+ */
+ node_info_retriever_ = &node_info_retriever;
+
+ int res = node_info_retriever_->addListener(this);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ /*
+ * Initializing the prefix, removing only the last trailing path separator.
+ */
+ common_path_prefix_ = arg_common_path_prefix;
+
+ if (!common_path_prefix_.empty() &&
+ *(common_path_prefix_.end() - 1) == protocol::file::Path::SEPARATOR)
+ {
+ common_path_prefix_.resize(uint8_t(common_path_prefix_.size() - 1U));
+ }
+
+ /*
+ * Initializing the client
+ */
+ res = begin_fw_update_client_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ begin_fw_update_client_.setCallback(
+ BeginFirmwareUpdateResponseCallback(this, &FirmwareUpdateTrigger::handleBeginFirmwareUpdateResponse));
+
+ // The timer will be started ad-hoc
+ return 0;
+ }
+
+ /**
+ * Interval at which uavcan.protocol.file.BeginFirmwareUpdate requests are being sent.
+ * Note that default value should be OK for any use case.
+ */
+ MonotonicDuration getRequestInterval() const { return request_interval_; }
+ void setRequestInterval(const MonotonicDuration interval)
+ {
+ if (interval.isPositive())
+ {
+ request_interval_ = interval;
+ if (TimerBase::isRunning()) // Restarting with new interval
+ {
+ TimerBase::startPeriodic(request_interval_);
+ }
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * This method is mostly needed for testing.
+ * When triggering is not in progress, the class consumes zero CPU time.
+ */
+ bool isTimerRunning() const { return TimerBase::isRunning(); }
+
+ unsigned getNumPendingNodes() const
+ {
+ const unsigned ret = pending_nodes_.getSize();
+ UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true);
+ return ret;
+ }
+
+};
+
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,262 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/protocol/GlobalTimeSync.hpp>
+#include <uavcan/debug.hpp>
+#include <cstdlib>
+#include <cassert>
+
+namespace uavcan
+{
+/**
+ * Please read the specs to learn how the time synchronization works.
+ *
+ * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen.
+ *
+ * NOTE: In order for this class to work, the platform driver must implement
+ * CAN bus TX loopback with both UTC and monotonic timestamping.
+ *
+ * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus"
+ *
+ * TODO: Enforce max one master per node
+ */
+class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
+{
+ class IfaceMaster
+ {
+ Publisher<protocol::GlobalTimeSync> pub_;
+ MonotonicTime iface_prev_pub_mono_;
+ UtcTime prev_tx_utc_;
+ const uint8_t iface_index_;
+
+ public:
+ IfaceMaster(INode& node, uint8_t iface_index)
+ : pub_(node)
+ , iface_index_(iface_index)
+ {
+ UAVCAN_ASSERT(iface_index < MaxCanIfaces);
+ }
+
+ int init(TransferPriority priority)
+ {
+ const int res = pub_.init(priority);
+ if (res >= 0)
+ {
+ pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_));
+ pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback);
+ }
+ return res;
+ }
+
+ void setTxTimestamp(UtcTime ts)
+ {
+ if (ts.isZero())
+ {
+ UAVCAN_ASSERT(0);
+ pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts");
+ return;
+ }
+ if (!prev_tx_utc_.isZero())
+ {
+ prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it
+ pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict");
+ return;
+ }
+ prev_tx_utc_ = ts;
+ }
+
+ int publish(TransferID tid, MonotonicTime current_time)
+ {
+ UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback);
+ UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_));
+
+ const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_;
+ iface_prev_pub_mono_ = current_time;
+ UAVCAN_ASSERT(since_prev_pub.isPositive());
+ const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS;
+
+ protocol::GlobalTimeSync msg;
+ msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec();
+ prev_tx_utc_ = UtcTime();
+
+ UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i",
+ static_cast<unsigned long long>(msg.previous_transmission_timestamp_usec),
+ int(iface_index_), int(tid.get()));
+ return pub_.broadcast(msg, tid);
+ }
+ };
+
+ INode& node_;
+ LazyConstructor<IfaceMaster> iface_masters_[MaxCanIfaces];
+ MonotonicTime prev_pub_mono_;
+ DataTypeID dtid_;
+ bool initialized_;
+
+ virtual void handleLoopbackFrame(const RxFrame& frame)
+ {
+ const uint8_t iface = frame.getIfaceIndex();
+ if (initialized_ && iface < MaxCanIfaces)
+ {
+ if (frame.getDataTypeID() == dtid_ &&
+ frame.getTransferType() == TransferTypeMessageBroadcast &&
+ frame.isStartOfTransfer() && frame.isEndOfTransfer() &&
+ frame.getSrcNodeID() == node_.getNodeID())
+ {
+ iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp());
+ }
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ int getNextTransferID(TransferID& tid)
+ {
+ const MonotonicDuration max_transfer_interval =
+ MonotonicDuration::fromMSec(protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS);
+
+ const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast);
+ const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval;
+ TransferID* const tid_ptr =
+ node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
+ if (tid_ptr == UAVCAN_NULLPTR)
+ {
+ return -ErrMemory;
+ }
+
+ tid = *tid_ptr;
+ tid_ptr->increment();
+ return 0;
+ }
+
+public:
+ explicit GlobalTimeSyncMaster(INode& node)
+ : LoopbackFrameListenerBase(node.getDispatcher())
+ , node_(node)
+ , initialized_(false)
+ { }
+
+ /**
+ * Merely prepares the class to work, doesn't do anything else.
+ * Must be called before the master can be used.
+ * Returns negative error code.
+ */
+ int init(const TransferPriority priority = TransferPriority::OneLowerThanHighest)
+ {
+ if (initialized_)
+ {
+ return 0;
+ }
+
+ // Data type ID
+ const DataTypeDescriptor* const desc =
+ GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName());
+ if (desc == UAVCAN_NULLPTR)
+ {
+ return -ErrUnknownDataType;
+ }
+ dtid_ = desc->getID();
+
+ // Iface master array
+ int res = -ErrLogic;
+ for (uint8_t i = 0; i < MaxCanIfaces; i++)
+ {
+ if (!iface_masters_[i].isConstructed())
+ {
+ iface_masters_[i].construct<INode&, uint8_t>(node_, i);
+ }
+ res = iface_masters_[i]->init(priority);
+ if (res < 0)
+ {
+ break;
+ }
+ }
+
+ // Loopback listener
+ initialized_ = res >= 0;
+ if (initialized_)
+ {
+ LoopbackFrameListenerBase::startListening();
+ }
+ return res;
+ }
+
+ /**
+ * Whether the master instance has been initialized.
+ */
+ bool isInitialized() const { return initialized_; }
+
+ /**
+ * Publishes one sync message.
+ *
+ * Every call to this method hints the master to publish the next sync message once. Exact time will
+ * be obtained from the TX loopback timestamp field.
+ *
+ * This method shall be called with a proper interval - refer to the time sync message definition
+ * for min/max interval values.
+ *
+ * Returns negative error code.
+ */
+ int publish()
+ {
+ if (!initialized_)
+ {
+ const int res = init();
+ if (res < 0)
+ {
+ return res;
+ }
+ }
+
+ /*
+ * Enforce max frequency
+ */
+ const MonotonicTime current_time = node_.getMonotonicTime();
+ {
+ const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_;
+ UAVCAN_ASSERT(since_prev_pub.isPositive());
+ if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_BROADCASTING_PERIOD_MS)
+ {
+ UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped");
+ return 0;
+ }
+ prev_pub_mono_ = current_time;
+ }
+
+ /*
+ * Obtain common Transfer ID for all masters
+ */
+ TransferID tid;
+ {
+ const int tid_res = getNextTransferID(tid);
+ if (tid_res < 0)
+ {
+ return tid_res;
+ }
+ }
+
+ for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
+ {
+ const int res = iface_masters_[i]->publish(tid, current_time);
+ if (res < 0)
+ {
+ return res;
+ }
+ }
+ return 0;
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,198 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
+
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/GlobalTimeSync.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+/**
+ * Please read the specs to learn how the time synchronization works.
+ *
+ * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen.
+ *
+ * NOTE: In order for this class to work, the platform driver must implement:
+ * - CAN bus RX UTC timestamping;
+ * - Clock adjustment method in the system clock interface @ref ISystemClock::adjustUtc().
+ *
+ * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus"
+ * http://modecs.cs.uni-salzburg.at/results/related_documents/CAN_clock.pdf
+ */
+class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable
+{
+ typedef MethodBinder<GlobalTimeSyncSlave*,
+ void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure<protocol::GlobalTimeSync>&)>
+ GlobalTimeSyncCallback;
+
+ Subscriber<protocol::GlobalTimeSync, GlobalTimeSyncCallback> sub_;
+
+ UtcTime prev_ts_utc_;
+ MonotonicTime prev_ts_mono_;
+ MonotonicTime last_adjustment_ts_;
+ enum State { Update, Adjust } state_;
+ NodeID master_nid_;
+ TransferID prev_tid_;
+ uint8_t prev_iface_index_;
+ bool suppressed_;
+
+ ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
+
+ void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
+ {
+ UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0);
+ const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_;
+
+ UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i",
+ static_cast<long long>(adjustment.toUSec()),
+ int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_));
+
+ if (!suppressed_)
+ {
+ getSystemClock().adjustUtc(adjustment);
+ }
+ last_adjustment_ts_ = msg.getMonotonicTimestamp();
+ state_ = Update;
+ }
+
+ void updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
+ {
+ UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i",
+ int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
+
+ prev_ts_utc_ = msg.getUtcTimestamp();
+ prev_ts_mono_ = msg.getMonotonicTimestamp();
+ master_nid_ = msg.getSrcNodeID();
+ prev_iface_index_ = msg.getIfaceIndex();
+ prev_tid_ = msg.getTransferID();
+ state_ = Adjust;
+ }
+
+ void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
+ {
+ const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_;
+ UAVCAN_ASSERT(!since_prev_msg.isNegative());
+
+ const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero();
+ const bool switch_master = msg.getSrcNodeID() < master_nid_;
+ // TODO: Make configurable
+ const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS;
+
+ if (switch_master || pub_timeout || needs_init)
+ {
+ UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i",
+ int(needs_init), int(switch_master), int(pub_timeout));
+ updateFromMsg(msg);
+ }
+ else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
+ {
+ if (state_ == Adjust)
+ {
+ const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0;
+ const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1;
+ const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS;
+ if (msg_invalid || wrong_tid || wrong_timing)
+ {
+ UAVCAN_TRACE("GlobalTimeSyncSlave",
+ "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i",
+ int(msg_invalid), int(wrong_tid), int(wrong_timing));
+ state_ = Update;
+ }
+ }
+ if (state_ == Adjust)
+ {
+ adjustFromMsg(msg);
+ }
+ else
+ {
+ updateFromMsg(msg);
+ }
+ }
+ else
+ {
+ UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i",
+ int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
+ }
+ }
+
+ void handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
+ {
+ if (msg.getTransferType() == TransferTypeMessageBroadcast)
+ {
+ processMsg(msg);
+ }
+ else
+ {
+ UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
+ }
+ }
+
+public:
+ explicit GlobalTimeSyncSlave(INode& node)
+ : sub_(node)
+ , state_(Update)
+ , prev_iface_index_(0xFF)
+ , suppressed_(false)
+ { }
+
+ /**
+ * Starts the time sync slave. Once started, it works on its own and does not require any
+ * attention from the application, other than to handle a clock adjustment request occasionally.
+ * Returns negative error code.
+ */
+ int start()
+ {
+ return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
+ }
+
+ /**
+ * Enable or disable the suppressed mode.
+ *
+ * In suppressed mode the slave will continue tracking time sync masters in the network, but will not
+ * perform local clock adjustments. So it's kind of a dry run - all the time sync logic works except
+ * the local clock will not receive adjustments.
+ *
+ * Suppressed mode is useful for nodes that can act as a back-up clock sync masters - as long as the
+ * node sees a higher priority time sync master in the network, its slave will be NOT suppressed
+ * in order to sync the local clock with the global master. As soon as all other higher priority
+ * masters go down, the local node will suppress its time sync slave instance and become a new master.
+ *
+ * Suppressed mode is disabled by default.
+ */
+ void suppress(bool suppressed) { suppressed_ = suppressed; }
+ bool isSuppressed() const { return suppressed_; }
+
+ /**
+ * If the clock sync slave sees any clock sync masters in the network, it is ACTIVE.
+ * When the last master times out (PUBLISHER_TIMEOUT), the slave will be INACTIVE.
+ * Note that immediately after start up the slave will be INACTIVE until it finds a master.
+ * Please read the specs to learn more.
+ */
+ bool isActive() const
+ {
+ const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
+ return !last_adjustment_ts_.isZero() &&
+ (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS);
+ }
+
+ /**
+ * Node ID of the master the slave is currently locked on.
+ * Returns an invalid Node ID if there's no active master.
+ */
+ NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); }
+
+ /**
+ * Last time when the local clock adjustment was performed.
+ */
+ MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/logger.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,286 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED
+
+#include <uavcan/time.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/debug/LogMessage.hpp>
+#include <uavcan/marshal/char_array_formatter.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <cstdlib>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+namespace uavcan
+{
+/**
+ * External log sink interface.
+ * External log sink allows the application to install a hook on the logger output.
+ * This can be used for application-wide logging.
+ * Please refer to the @ref Logger class docs.
+ */
+class UAVCAN_EXPORT ILogSink
+{
+public:
+ typedef typename StorageType<typename protocol::debug::LogLevel::FieldTypes::value>::Type LogLevel;
+
+ virtual ~ILogSink() { }
+
+ /**
+ * Logger will not sink messages with a severity level lower than returned by this method.
+ * Default level is DEBUG.
+ */
+ virtual LogLevel getLogLevel() const { return protocol::debug::LogLevel::DEBUG; }
+
+ /**
+ * Logger will call this method for every log message which severity level
+ * is not less than the current level of this sink.
+ */
+ virtual void log(const protocol::debug::LogMessage& message) = 0;
+};
+
+/**
+ * Node logging convenience class.
+ *
+ * This class is based on the standard UAVCAN message type for logging - uavcan.protocol.debug.LogMessage.
+ *
+ * Provides logging methods of different severity; implements two sinks for the log messages:
+ * - Broadcast via the UAVCAN bus;
+ * - Sink into the application via @ref ILogSink.
+ *
+ * For each sink an individual severity threshold filter can be configured.
+ */
+class UAVCAN_EXPORT Logger
+{
+public:
+ typedef ILogSink::LogLevel LogLevel;
+
+ /**
+ * This value is higher than any valid severity value.
+ * Use it to completely suppress the output.
+ */
+ static LogLevel getLogLevelAboveAll() { return (1U << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1U; }
+
+private:
+ enum { DefaultTxTimeoutMs = 2000 };
+
+ Publisher<protocol::debug::LogMessage> logmsg_pub_;
+ protocol::debug::LogMessage msg_buf_;
+ LogLevel level_;
+ ILogSink* external_sink_;
+
+ LogLevel getExternalSinkLevel() const
+ {
+ return (external_sink_ == UAVCAN_NULLPTR) ? getLogLevelAboveAll() : external_sink_->getLogLevel();
+ }
+
+public:
+ explicit Logger(INode& node)
+ : logmsg_pub_(node)
+ , external_sink_(UAVCAN_NULLPTR)
+ {
+ level_ = protocol::debug::LogLevel::ERROR;
+ setTxTimeout(MonotonicDuration::fromMSec(DefaultTxTimeoutMs));
+ UAVCAN_ASSERT(getTxTimeout() == MonotonicDuration::fromMSec(DefaultTxTimeoutMs));
+ }
+
+ /**
+ * Initializes the logger, does not perform any network activity.
+ * Must be called once before use.
+ * Returns negative error code.
+ */
+ int init(const TransferPriority priority = TransferPriority::Lowest)
+ {
+ const int res = logmsg_pub_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ return 0;
+ }
+
+ /**
+ * Logs one message. Please consider using helper methods instead of this one.
+ *
+ * The message will be broadcasted via the UAVCAN bus if the severity level of the
+ * message is >= severity level of the logger.
+ *
+ * The message will be reported into the external log sink if the external sink is
+ * installed and the severity level of the message is >= severity level of the external sink.
+ *
+ * Returns negative error code.
+ */
+ int log(const protocol::debug::LogMessage& message)
+ {
+ int retval = 0;
+ if (message.level.value >= getExternalSinkLevel())
+ {
+ external_sink_->log(message);
+ }
+ if (message.level.value >= level_)
+ {
+ retval = logmsg_pub_.broadcast(message);
+ }
+ return retval;
+ }
+
+ /**
+ * Severity filter for UAVCAN broadcasting.
+ * Log message will be broadcasted via the UAVCAN network only if its severity is >= getLevel().
+ * This does not affect the external sink.
+ * Default level is ERROR.
+ */
+ LogLevel getLevel() const { return level_; }
+ void setLevel(LogLevel level) { level_ = level; }
+
+ /**
+ * External log sink allows the application to install a hook on the logger output.
+ * This can be used for application-wide logging.
+ * Null pointer means that there's no log sink (can be used to remove it).
+ * By default there's no log sink.
+ */
+ ILogSink* getExternalSink() const { return external_sink_; }
+ void setExternalSink(ILogSink* sink) { external_sink_ = sink; }
+
+ /**
+ * Log message broadcast transmission timeout.
+ * The default value should be acceptable for any use case.
+ */
+ MonotonicDuration getTxTimeout() const { return logmsg_pub_.getTxTimeout(); }
+ void setTxTimeout(MonotonicDuration val) { logmsg_pub_.setTxTimeout(val); }
+
+ /**
+ * Helper methods for various severity levels and with formatting support.
+ * These methods build a formatted log message and pass it into the method @ref log().
+ *
+ * Format string usage is a bit unusual: use "%*" for any argument type, use "%%" to print a percent character.
+ * No other formating options are supported. Insufficient/extra arguments are ignored.
+ *
+ * Example format string:
+ * "What do you get if you %* %* by %*? %*. Extra arguments: %* %* %%"
+ * ...with the following arguments:
+ * "multiply", 6, 9.0F 4.2e1
+ * ...will likely produce this (floating point representation is platform dependent):
+ * "What do you get if you multiply 6 by 9.000000? 42.000000. Extra arguments: %* %* %"
+ *
+ * Formatting is not supported in C++03 mode.
+ * @{
+ */
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+ template <typename... Args>
+ int log(LogLevel level, const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT;
+
+ template <typename... Args>
+ inline int logDebug(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::DEBUG, source, format, args...);
+ }
+
+ template <typename... Args>
+ inline int logInfo(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::INFO, source, format, args...);
+ }
+
+ template <typename... Args>
+ inline int logWarning(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::WARNING, source, format, args...);
+ }
+
+ template <typename... Args>
+ inline int logError(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::ERROR, source, format, args...);
+ }
+
+#else
+
+ int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT
+ {
+ #if UAVCAN_EXCEPTIONS
+ try
+ #endif
+ {
+ if (level >= level_ || level >= getExternalSinkLevel())
+ {
+ msg_buf_.level.value = level;
+ msg_buf_.source = source;
+ msg_buf_.text = text;
+ return log(msg_buf_);
+ }
+ return 0;
+ }
+ #if UAVCAN_EXCEPTIONS
+ catch (...)
+ {
+ return -ErrFailure;
+ }
+ #endif
+ }
+
+ int logDebug(const char* source, const char* text) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::DEBUG, source, text);
+ }
+
+ int logInfo(const char* source, const char* text) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::INFO, source, text);
+ }
+
+ int logWarning(const char* source, const char* text) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::WARNING, source, text);
+ }
+
+ int logError(const char* source, const char* text) UAVCAN_NOEXCEPT
+ {
+ return log(protocol::debug::LogLevel::ERROR, source, text);
+ }
+
+#endif
+ /**
+ * @}
+ */
+};
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+template <typename... Args>
+int Logger::log(LogLevel level, const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT
+{
+#if UAVCAN_EXCEPTIONS
+ try
+#endif
+ {
+ if (level >= level_ || level >= getExternalSinkLevel())
+ {
+ msg_buf_.level.value = level;
+ msg_buf_.source = source;
+ msg_buf_.text.clear();
+ CharArrayFormatter<typename protocol::debug::LogMessage::FieldTypes::text> formatter(msg_buf_.text);
+ formatter.write(format, args...);
+ return log(msg_buf_);
+ }
+ return 0;
+ }
+#if UAVCAN_EXCEPTIONS
+ catch (...)
+ {
+ return -ErrFailure;
+ }
+#endif
+}
+
+#endif
+
+}
+
+#endif // UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,464 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/util/multiset.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+
+namespace uavcan
+{
+/**
+ * Classes that need to receive GetNodeInfo responses should implement this interface.
+ */
+class UAVCAN_EXPORT INodeInfoListener
+{
+public:
+ /**
+ * Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or
+ * becomes online for the first time.
+ * @param node_id Node ID of the node
+ * @param response Node info struct
+ */
+ virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0;
+
+ /**
+ * Called when the retriever decides that the node does not support the GetNodeInfo service.
+ * This method will never be called if the number of attempts is unlimited.
+ */
+ virtual void handleNodeInfoUnavailable(NodeID node_id) = 0;
+
+ /**
+ * This call is routed directly from @ref NodeStatusMonitor.
+ * Default implementation does nothing.
+ * @param event Node status change event
+ */
+ virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
+ {
+ (void)event;
+ }
+
+ /**
+ * This call is routed directly from @ref NodeStatusMonitor.
+ * Default implementation does nothing.
+ * @param msg Node status message
+ */
+ virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+ {
+ (void)msg;
+ }
+
+ virtual ~INodeInfoListener() { }
+};
+
+/**
+ * This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts.
+ * It does a number of attempts in case if there's a communication failure before assuming that the node does not
+ * implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit
+ * virtually any use case, but they can be overriden if needed - refer to the setter methods below for details.
+ *
+ * Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local
+ * node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo
+ * service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are
+ * unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate
+ * for the end application, the request interval can be overriden via @ref setRequestInterval().
+ *
+ * Following the above explained requirements, the default request interval is defined as follows:
+ * request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes)
+ * Which yields 40 ms.
+ *
+ * Given default service timeout 1000 ms and the defined above request frequency 40 ms, the maximum number of
+ * concurrent requests will be:
+ * max concurrent requests = ceil(1000 [ms] timeout / 40 [ms] request interval)
+ * Which yields 25 requests.
+ *
+ * Keep the above equations in mind when changing the default request interval.
+ *
+ * Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never
+ * exceed one. This is actually the most likely scenario.
+ *
+ * Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc.
+ *
+ * Events from this class can be routed to many listeners, @ref INodeInfoListener.
+ */
+class UAVCAN_EXPORT NodeInfoRetriever : public NodeStatusMonitor
+ , TimerBase
+{
+public:
+ enum { MaxNumRequestAttempts = 254 };
+ enum { UnlimitedRequestAttempts = 0 };
+
+private:
+ typedef MethodBinder<NodeInfoRetriever*,
+ void (NodeInfoRetriever::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
+ GetNodeInfoResponseCallback;
+
+ struct Entry
+ {
+ uint32_t uptime_sec;
+ uint8_t num_attempts_made;
+ bool request_needed; ///< Always false for unknown nodes
+ bool updated_since_last_attempt; ///< Always false for unknown nodes
+
+ Entry()
+ : uptime_sec(0)
+ , num_attempts_made(0)
+ , request_needed(false)
+ , updated_since_last_attempt(false)
+ {
+#if UAVCAN_DEBUG
+ StaticAssert<sizeof(Entry) <= 8>::check();
+#endif
+ }
+ };
+
+ struct NodeInfoRetrievedHandlerCaller
+ {
+ const NodeID node_id;
+ const protocol::GetNodeInfo::Response& node_info;
+
+ NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info)
+ : node_id(arg_node_id)
+ , node_info(arg_node_info)
+ { }
+
+ bool operator()(INodeInfoListener* key)
+ {
+ UAVCAN_ASSERT(key != UAVCAN_NULLPTR);
+ key->handleNodeInfoRetrieved(node_id, node_info);
+ return false;
+ }
+ };
+
+ template <typename Event>
+ struct GenericHandlerCaller
+ {
+ void (INodeInfoListener::* const method)(Event);
+ Event event;
+
+ GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event)
+ : method(arg_method)
+ , event(arg_event)
+ { }
+
+ bool operator()(INodeInfoListener* key)
+ {
+ UAVCAN_ASSERT(key != UAVCAN_NULLPTR);
+ (key->*method)(event);
+ return false;
+ }
+ };
+
+ enum { DefaultNumRequestAttempts = 16 };
+ enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation
+
+ /*
+ * State
+ */
+ Entry entries_[NodeID::Max]; // [1, NodeID::Max]
+
+ Multiset<INodeInfoListener*> listeners_;
+
+ ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
+
+ MonotonicDuration request_interval_;
+
+ mutable uint8_t last_picked_node_;
+
+ uint8_t num_attempts_;
+
+ /*
+ * Methods
+ */
+ const Entry& getEntry(NodeID node_id) const { return const_cast<NodeInfoRetriever*>(this)->getEntry(node_id); }
+ Entry& getEntry(NodeID node_id)
+ {
+ if (node_id.get() < 1 || node_id.get() > NodeID::Max)
+ {
+ handleFatalError("NodeInfoRetriever NodeID");
+ }
+ return entries_[node_id.get() - 1];
+ }
+
+ void startTimerIfNotRunning()
+ {
+ if (!TimerBase::isRunning())
+ {
+ TimerBase::startPeriodic(request_interval_);
+ UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str());
+ }
+ }
+
+ NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
+ {
+ out_at_least_one_request_needed = false;
+
+ for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
+ {
+ last_picked_node_++;
+ if (last_picked_node_ > NodeID::Max)
+ {
+ last_picked_node_ = 1;
+ }
+ UAVCAN_ASSERT((last_picked_node_ >= 1) &&
+ (last_picked_node_ <= NodeID::Max));
+
+ const Entry& entry = getEntry(last_picked_node_);
+
+ if (entry.request_needed)
+ {
+ out_at_least_one_request_needed = true;
+
+ if (entry.updated_since_last_attempt &&
+ !get_node_info_client_.hasPendingCallToServer(last_picked_node_))
+ {
+ UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
+ return NodeID(last_picked_node_);
+ }
+ }
+ }
+
+ return NodeID(); // No node could be found
+ }
+
+ virtual void handleTimerEvent(const TimerEvent&)
+ {
+ bool at_least_one_request_needed = false;
+ const NodeID next = pickNextNodeToQuery(at_least_one_request_needed);
+
+ if (next.isUnicast())
+ {
+ UAVCAN_ASSERT(at_least_one_request_needed);
+ getEntry(next).updated_since_last_attempt = false;
+ const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request());
+ if (res < 0)
+ {
+ get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call");
+ }
+ }
+ else
+ {
+ if (!at_least_one_request_needed)
+ {
+ TimerBase::stop();
+ UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped");
+ }
+ }
+ }
+
+ virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
+ {
+ const bool was_offline = !event.was_known ||
+ (event.old_status.mode == protocol::NodeStatus::MODE_OFFLINE);
+
+ const bool offline_now = event.status.mode == protocol::NodeStatus::MODE_OFFLINE;
+
+ if (was_offline || offline_now)
+ {
+ Entry& entry = getEntry(event.node_id);
+
+ entry.request_needed = !offline_now;
+ entry.num_attempts_made = 0;
+
+ UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
+ int(event.node_id.get()), int(entry.request_needed));
+
+ if (entry.request_needed)
+ {
+ startTimerIfNotRunning();
+ }
+ }
+
+ listeners_.forEach(
+ GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
+ }
+
+ virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+ {
+ Entry& entry = getEntry(msg.getSrcNodeID());
+
+ if (msg.uptime_sec < entry.uptime_sec)
+ {
+ entry.request_needed = true;
+ entry.num_attempts_made = 0;
+
+ startTimerIfNotRunning();
+ }
+ entry.uptime_sec = msg.uptime_sec;
+ entry.updated_since_last_attempt = true;
+
+ listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
+ &INodeInfoListener::handleNodeStatusMessage, msg));
+ }
+
+ void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
+ {
+ Entry& entry = getEntry(result.getCallID().server_node_id);
+
+ if (result.isSuccessful())
+ {
+ /*
+ * Updating the uptime here allows to properly handle a corner case where the service response arrives
+ * after the device has restarted and published its new NodeStatus (although it's unlikely to happen).
+ */
+ entry.uptime_sec = result.getResponse().status.uptime_sec;
+ entry.request_needed = false;
+ listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id,
+ result.getResponse()));
+ }
+ else
+ {
+ if (num_attempts_ != UnlimitedRequestAttempts)
+ {
+ entry.num_attempts_made++;
+ if (entry.num_attempts_made >= num_attempts_)
+ {
+ entry.request_needed = false;
+ listeners_.forEach(GenericHandlerCaller<NodeID>(&INodeInfoListener::handleNodeInfoUnavailable,
+ result.getCallID().server_node_id));
+ }
+ }
+ }
+ }
+
+public:
+ NodeInfoRetriever(INode& node)
+ : NodeStatusMonitor(node)
+ , TimerBase(node)
+ , listeners_(node.getAllocator())
+ , get_node_info_client_(node)
+ , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec))
+ , last_picked_node_(1)
+ , num_attempts_(DefaultNumRequestAttempts)
+ { }
+
+ /**
+ * Starts the retriever.
+ * Destroy the object to stop it.
+ * Returns negative error code.
+ */
+ int start(const TransferPriority priority = TransferPriority::OneHigherThanLowest)
+ {
+ int res = NodeStatusMonitor::start();
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = get_node_info_client_.init(priority);
+ if (res < 0)
+ {
+ return res;
+ }
+ get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this,
+ &NodeInfoRetriever::handleGetNodeInfoResponse));
+ // Note: the timer will be started ad-hoc
+ return 0;
+ }
+
+ /**
+ * This method forces the class to re-request uavcan.protocol.GetNodeInfo from all nodes as if they
+ * have just appeared in the network.
+ */
+ void invalidateAll()
+ {
+ NodeStatusMonitor::forgetAllNodes();
+ get_node_info_client_.cancelAllCalls();
+
+ for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++)
+ {
+ entries_[i] = Entry();
+ }
+ // It is not necessary to reset the last picked node index
+ }
+
+ /**
+ * Adds one listener. Does nothing if such listener already exists.
+ * May return -ErrMemory if there's no space to add the listener.
+ */
+ int addListener(INodeInfoListener* listener)
+ {
+ if (listener != UAVCAN_NULLPTR)
+ {
+ removeListener(listener);
+ return (UAVCAN_NULLPTR == listeners_.emplace(listener)) ? -ErrMemory : 0;
+ }
+ else
+ {
+ return -ErrInvalidParam;
+ }
+ }
+
+ /**
+ * Removes the listener.
+ * If the listener was not registered, nothing will be done.
+ */
+ void removeListener(INodeInfoListener* listener)
+ {
+ if (listener != UAVCAN_NULLPTR)
+ {
+ listeners_.removeAll(listener);
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ unsigned getNumListeners() const { return listeners_.getSize(); }
+
+ /**
+ * Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is
+ * not implemented.
+ * Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts.
+ */
+ uint8_t getNumRequestAttempts() const { return num_attempts_; }
+ void setNumRequestAttempts(const uint8_t num)
+ {
+ num_attempts_ = min(static_cast<uint8_t>(MaxNumRequestAttempts), num);
+ }
+
+ /**
+ * Request interval also implicitly defines the maximum number of concurrent requests.
+ * Read the class documentation for details.
+ */
+ MonotonicDuration getRequestInterval() const { return request_interval_; }
+ void setRequestInterval(const MonotonicDuration interval)
+ {
+ if (interval.isPositive())
+ {
+ request_interval_ = interval;
+ if (TimerBase::isRunning())
+ {
+ TimerBase::startPeriodic(request_interval_);
+ }
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * These methods are needed mostly for testing.
+ */
+ bool isRetrievingInProgress() const { return TimerBase::isRunning(); }
+
+ uint8_t getNumPendingRequests() const
+ {
+ const unsigned num = get_node_info_client_.getNumPendingCalls();
+ UAVCAN_ASSERT(num <= 0xFF);
+ return static_cast<uint8_t>(num);
+ }
+};
+
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
+
+#include <uavcan/debug.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+#include <cassert>
+#include <cstdlib>
+
+namespace uavcan
+{
+/**
+ * This class implements the core functionality of a network monitor.
+ * It can be extended by inheritance to add more complex logic, or used directly as is.
+ */
+class UAVCAN_EXPORT NodeStatusMonitor
+{
+public:
+ struct NodeStatus
+ {
+ uint8_t health : 2;
+ uint8_t mode : 3;
+ uint8_t sub_mode : 3;
+
+ NodeStatus() :
+ health(protocol::NodeStatus::HEALTH_CRITICAL),
+ mode(protocol::NodeStatus::MODE_OFFLINE),
+ sub_mode(0)
+ {
+ StaticAssert<protocol::NodeStatus::FieldTypes::health::BitLen == 2>::check();
+ StaticAssert<protocol::NodeStatus::FieldTypes::mode::BitLen == 3>::check();
+ StaticAssert<protocol::NodeStatus::FieldTypes::sub_mode::BitLen == 3>::check();
+ }
+
+ bool operator!=(const NodeStatus rhs) const { return !operator==(rhs); }
+ bool operator==(const NodeStatus rhs) const
+ {
+ return std::memcmp(this, &rhs, sizeof(NodeStatus)) == 0;
+ }
+
+#if UAVCAN_TOSTRING
+ std::string toString() const
+ {
+ char buf[40];
+ (void)snprintf(buf, sizeof(buf), "health=%d mode=%d sub_mode=%d", int(health), int(mode), int(sub_mode));
+ return std::string(buf);
+ }
+#endif
+ };
+
+ struct NodeStatusChangeEvent
+ {
+ NodeID node_id;
+ NodeStatus status;
+ NodeStatus old_status;
+ bool was_known;
+
+ NodeStatusChangeEvent() :
+ was_known(false)
+ { }
+ };
+
+private:
+ enum { TimerPeriodMs100 = 2 };
+
+ typedef MethodBinder<NodeStatusMonitor*,
+ void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
+ NodeStatusCallback;
+
+ typedef MethodBinder<NodeStatusMonitor*, void (NodeStatusMonitor::*)(const TimerEvent&)> TimerCallback;
+
+ Subscriber<protocol::NodeStatus, NodeStatusCallback> sub_;
+
+ TimerEventForwarder<TimerCallback> timer_;
+
+ struct Entry
+ {
+ NodeStatus status;
+ int8_t time_since_last_update_ms100;
+ Entry() :
+ time_since_last_update_ms100(-1)
+ { }
+ };
+
+ mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max]
+
+ Entry& getEntry(NodeID node_id) const
+ {
+ if (node_id.get() < 1 || node_id.get() > NodeID::Max)
+ {
+ handleFatalError("NodeStatusMonitor NodeID");
+ }
+ return entries_[node_id.get() - 1];
+ }
+
+ void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
+ {
+ Entry& entry = getEntry(node_id);
+ if (entry.status != new_entry_value.status)
+ {
+ NodeStatusChangeEvent event;
+ event.node_id = node_id;
+ event.old_status = entry.status;
+ event.status = new_entry_value.status;
+ event.was_known = entry.time_since_last_update_ms100 >= 0;
+
+ UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()),
+ (event.was_known ? "known" : "new"),
+ event.old_status.toString().c_str(), event.status.toString().c_str());
+
+ handleNodeStatusChange(event);
+ }
+ entry = new_entry_value;
+ }
+
+ void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+ {
+ Entry new_entry;
+ new_entry.time_since_last_update_ms100 = 0;
+ new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1);
+ new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1);
+ new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1);
+
+ changeNodeStatus(msg.getSrcNodeID(), new_entry);
+
+ handleNodeStatusMessage(msg);
+ }
+
+ void handleTimerEvent(const TimerEvent&)
+ {
+ const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
+
+ for (uint8_t i = 1; i <= NodeID::Max; i++)
+ {
+ Entry& entry = getEntry(i);
+ if (entry.time_since_last_update_ms100 >= 0 &&
+ entry.status.mode != protocol::NodeStatus::MODE_OFFLINE)
+ {
+ entry.time_since_last_update_ms100 =
+ int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
+
+ if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100)
+ {
+ Entry new_entry_value = entry;
+ new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
+ new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE;
+ changeNodeStatus(i, new_entry_value);
+ }
+ }
+ }
+ }
+
+protected:
+ /**
+ * Called when a node becomes online, changes status or goes offline.
+ * Refer to uavcan.protocol.NodeStatus for the offline timeout value.
+ * Overriding is not required.
+ */
+ virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
+ {
+ (void)event;
+ }
+
+ /**
+ * Called for every received message uavcan.protocol.NodeStatus after handleNodeStatusChange(), even
+ * if the status code did not change.
+ * Overriding is not required.
+ */
+ virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+ {
+ (void)msg;
+ }
+
+public:
+ explicit NodeStatusMonitor(INode& node)
+ : sub_(node)
+ , timer_(node)
+ { }
+
+ virtual ~NodeStatusMonitor() { }
+
+ /**
+ * Starts the monitor.
+ * Destroy the object to stop it.
+ * Returns negative error code.
+ */
+ int start()
+ {
+ const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
+ if (res >= 0)
+ {
+ timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent));
+ timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
+ }
+ return res;
+ }
+
+ /**
+ * Make the node unknown.
+ */
+ void forgetNode(NodeID node_id)
+ {
+ if (node_id.isValid())
+ {
+ Entry& entry = getEntry(node_id);
+ entry = Entry();
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+ }
+
+ /**
+ * Make all nodes unknown.
+ */
+ void forgetAllNodes()
+ {
+ for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++)
+ {
+ entries_[i] = Entry();
+ }
+ }
+
+ /**
+ * Returns status of a given node.
+ * Unknown nodes are considered offline.
+ * Complexity O(1).
+ */
+ NodeStatus getNodeStatus(NodeID node_id) const
+ {
+ if (!node_id.isValid())
+ {
+ UAVCAN_ASSERT(0);
+ return NodeStatus();
+ }
+
+ const Entry& entry = getEntry(node_id);
+ if (entry.time_since_last_update_ms100 >= 0)
+ {
+ return entry.status;
+ }
+ else
+ {
+ return NodeStatus();
+ }
+ }
+
+ /**
+ * Whether the class has observed this node at least once since initialization.
+ * Complexity O(1).
+ */
+ bool isNodeKnown(NodeID node_id) const
+ {
+ if (!node_id.isValid())
+ {
+ UAVCAN_ASSERT(0);
+ return false;
+ }
+ return getEntry(node_id).time_since_last_update_ms100 >= 0;
+ }
+
+ /**
+ * This helper method allows to quickly estimate the overall network health.
+ * Health of the local node is not considered.
+ * Returns an invalid Node ID value if there's no known nodes in the network.
+ */
+ NodeID findNodeWithWorstHealth() const
+ {
+ NodeID nid_with_worst_health;
+ uint8_t worst_health = protocol::NodeStatus::HEALTH_OK;
+
+ for (uint8_t i = 1; i <= NodeID::Max; i++)
+ {
+ const NodeID nid(i);
+ UAVCAN_ASSERT(nid.isUnicast());
+ const Entry& entry = getEntry(nid);
+ if (entry.time_since_last_update_ms100 >= 0)
+ {
+ if (entry.status.health > worst_health || !nid_with_worst_health.isValid())
+ {
+ nid_with_worst_health = nid;
+ worst_health = entry.status.health;
+ }
+ }
+ }
+
+ return nid_with_worst_health;
+ }
+
+ /**
+ * Calls the operator for every known node.
+ * Operator signature:
+ * void (NodeID, NodeStatus)
+ */
+ template <typename Operator>
+ void forEachNode(Operator op) const
+ {
+ for (uint8_t i = 1; i <= NodeID::Max; i++)
+ {
+ const NodeID nid(i);
+ UAVCAN_ASSERT(nid.isUnicast());
+ const Entry& entry = getEntry(nid);
+ if (entry.time_since_last_update_ms100 >= 0)
+ {
+ op(nid, entry.status);
+ }
+ }
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/node_status_provider.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,174 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED
+
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+
+namespace uavcan
+{
+/**
+ * This optional interface can be implemented by the user in order to update the node status as necessary,
+ * immediately before the next NodeStatus message is emitted by @ref NodeStatusProvider.
+ */
+class IAdHocNodeStatusUpdater
+{
+public:
+ /**
+ * This method is invoked by the library from @ref NodeStatusProvider from the library's thread immediately
+ * before the next NodeStatus message is transmitted. The application can implement this method to perform
+ * node status updates only as necessary.
+ * The application is expected to invoke the methods of @ref NodeStatusProvider to update the status
+ * from this method.
+ * Note that this method is only invoked when publication is happening by the timer event.
+ * It will NOT be invoked if the following methods are used to trigger node status publication:
+ * - @ref NodeStatusProvider::startAndPublish()
+ * - @ref NodeStatusProvider::forcePublish()
+ */
+ virtual void updateNodeStatus() = 0;
+
+ virtual ~IAdHocNodeStatusUpdater() { }
+};
+
+/**
+ * Provides the status and basic information about this node to other network participants.
+ *
+ * Usually the application does not need to deal with this class directly - it's instantiated by the node class.
+ *
+ * Default values:
+ * - health - OK
+ * - mode - INITIALIZATION
+ */
+class UAVCAN_EXPORT NodeStatusProvider : private TimerBase
+{
+ typedef MethodBinder<NodeStatusProvider*,
+ void (NodeStatusProvider::*)(const protocol::GetNodeInfo::Request&,
+ protocol::GetNodeInfo::Response&)> GetNodeInfoCallback;
+
+ const MonotonicTime creation_timestamp_;
+
+ Publisher<protocol::NodeStatus> node_status_pub_;
+ ServiceServer<protocol::GetNodeInfo, GetNodeInfoCallback> gni_srv_;
+
+ protocol::GetNodeInfo::Response node_info_;
+
+ IAdHocNodeStatusUpdater* ad_hoc_status_updater_;
+
+ INode& getNode() { return node_status_pub_.getNode(); }
+
+ bool isNodeInfoInitialized() const;
+
+ int publish();
+
+ virtual void handleTimerEvent(const TimerEvent&);
+ void handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, protocol::GetNodeInfo::Response& rsp);
+
+public:
+ typedef typename StorageType<typename protocol::NodeStatus::FieldTypes::vendor_specific_status_code>::Type
+ VendorSpecificStatusCode;
+
+ typedef typename StorageType<typename protocol::GetNodeInfo::Response::FieldTypes::name>::Type NodeName;
+
+ explicit NodeStatusProvider(INode& node)
+ : TimerBase(node)
+ , creation_timestamp_(node.getMonotonicTime())
+ , node_status_pub_(node)
+ , gni_srv_(node)
+ , ad_hoc_status_updater_(UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(!creation_timestamp_.isZero());
+
+ node_info_.status.mode = protocol::NodeStatus::MODE_INITIALIZATION;
+
+ node_info_.status.health = protocol::NodeStatus::HEALTH_OK;
+ }
+
+ /**
+ * Starts the provider and immediately broadcasts uavcan.protocol.NodeStatus.
+ * Returns negative error code.
+ */
+ int startAndPublish(const TransferPriority priority = TransferPriority::Default);
+
+ /**
+ * Publish the message uavcan.protocol.NodeStatus right now, out of schedule.
+ * Returns negative error code.
+ */
+ int forcePublish() { return publish(); }
+
+ /**
+ * Allows to override default publishing rate for uavcan.protocol.NodeStatus.
+ * Refer to the DSDL definition of uavcan.protocol.NodeStatus to see what is the default rate.
+ * Doesn't fail; if the value is outside of acceptable range, a closest valid value will be used instead.
+ */
+ void setStatusPublicationPeriod(uavcan::MonotonicDuration period);
+ uavcan::MonotonicDuration getStatusPublicationPeriod() const;
+
+ /**
+ * Configure the optional handler that is invoked before every node status message is emitted.
+ * By default no handler is installed.
+ * It is allowed to pass a null pointer, that will disable the ad-hoc update feature.
+ * @ref IAdHocNodeStatusUpdater
+ */
+ void setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater);
+ IAdHocNodeStatusUpdater* getAdHocNodeStatusUpdater() const { return ad_hoc_status_updater_; }
+
+ /**
+ * Local node health code control.
+ */
+ uint8_t getHealth() const { return node_info_.status.health; }
+ void setHealth(uint8_t code);
+ void setHealthOk() { setHealth(protocol::NodeStatus::HEALTH_OK); }
+ void setHealthWarning() { setHealth(protocol::NodeStatus::HEALTH_WARNING); }
+ void setHealthError() { setHealth(protocol::NodeStatus::HEALTH_ERROR); }
+ void setHealthCritical() { setHealth(protocol::NodeStatus::HEALTH_CRITICAL); }
+
+ /**
+ * Local node mode code control.
+ */
+ uint8_t getMode() const { return node_info_.status.mode; }
+ void setMode(uint8_t code);
+ void setModeOperational() { setMode(protocol::NodeStatus::MODE_OPERATIONAL); }
+ void setModeInitialization() { setMode(protocol::NodeStatus::MODE_INITIALIZATION); }
+ void setModeMaintenance() { setMode(protocol::NodeStatus::MODE_MAINTENANCE); }
+ void setModeSoftwareUpdate() { setMode(protocol::NodeStatus::MODE_SOFTWARE_UPDATE); }
+ void setModeOffline() { setMode(protocol::NodeStatus::MODE_OFFLINE); }
+
+ /**
+ * Local node vendor-specific status code control.
+ */
+ void setVendorSpecificStatusCode(VendorSpecificStatusCode code);
+ VendorSpecificStatusCode getVendorSpecificStatusCode() const
+ {
+ return node_info_.status.vendor_specific_status_code;
+ }
+
+ /**
+ * Local node name control.
+ * Can be set only once before the provider is started.
+ * The provider will refuse to start if the node name is not set.
+ */
+ const NodeName& getName() const { return node_info_.name; }
+ void setName(const NodeName& name);
+
+ /**
+ * Node version information.
+ * Can be set only once before the provider is started.
+ */
+ const protocol::SoftwareVersion& getSoftwareVersion() const { return node_info_.software_version; }
+ const protocol::HardwareVersion& getHardwareVersion() const { return node_info_.hardware_version; }
+ void setSoftwareVersion(const protocol::SoftwareVersion& version);
+ void setHardwareVersion(const protocol::HardwareVersion& version);
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED
+
+#include <uavcan/debug.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/protocol/Panic.hpp>
+
+namespace uavcan
+{
+/**
+ * Helper for broadcasting the message uavcan.protocol.Panic.
+ */
+class UAVCAN_EXPORT PanicBroadcaster : private TimerBase
+{
+ Publisher<protocol::Panic> pub_;
+ protocol::Panic msg_;
+
+ void publishOnce()
+ {
+ const int res = pub_.broadcast(msg_);
+ if (res < 0)
+ {
+ pub_.getNode().registerInternalFailure("Panic pub failed");
+ }
+ }
+
+ virtual void handleTimerEvent(const TimerEvent&)
+ {
+ publishOnce();
+ }
+
+public:
+ explicit PanicBroadcaster(INode& node)
+ : TimerBase(node)
+ , pub_(node)
+ { }
+
+ /**
+ * This method does not block and can't fail.
+ * @param short_reason Short ASCII string that describes the reason of the panic, 7 characters max.
+ * If the string exceeds 7 characters, it will be truncated.
+ * @param broadcasting_period Broadcasting period. Optional.
+ * @param priority Transfer priority. Optional.
+ */
+ void panic(const char* short_reason_description,
+ MonotonicDuration broadcasting_period = MonotonicDuration::fromMSec(100),
+ const TransferPriority priority = TransferPriority::Default)
+ {
+ msg_.reason_text.clear();
+ const char* p = short_reason_description;
+ while (p && *p)
+ {
+ if (msg_.reason_text.size() == msg_.reason_text.capacity())
+ {
+ break;
+ }
+ msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p));
+ p++;
+ }
+
+ UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str());
+
+ pub_.setTxTimeout(broadcasting_period);
+ pub_.setPriority(priority);
+
+ publishOnce();
+ startPeriodic(broadcasting_period);
+ }
+
+ /**
+ * Stop broadcasting immediately.
+ */
+ void dontPanic() // Where's my towel
+ {
+ stop();
+ msg_.reason_text.clear();
+ }
+
+ bool isPanicking() const { return isRunning(); }
+
+ const typename protocol::Panic::FieldTypes::reason_text& getReason() const
+ {
+ return msg_.reason_text;
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/panic_listener.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/debug.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/Panic.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# include <functional>
+#endif
+
+namespace uavcan
+{
+/**
+ * This class implements proper panic detector.
+ * Refer to uavcan.protocol.Panic for details.
+ * The listener can be stopped from the callback.
+ *
+ * @tparam Callback Possible callback prototypes:
+ * void (const ReceivedDataStructure<protocol::Panic>&)
+ * void (const protocol::Panic&)
+ */
+template <
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ typename Callback = std::function<void (const ReceivedDataStructure<protocol::Panic>&)>
+#else
+ typename Callback = void (*)(const ReceivedDataStructure<protocol::Panic>&)
+#endif
+ >
+class UAVCAN_EXPORT PanicListener : Noncopyable
+{
+ typedef MethodBinder<PanicListener*, void (PanicListener::*)(const ReceivedDataStructure<protocol::Panic>&)>
+ PanicMsgCallback;
+
+ Subscriber<protocol::Panic, PanicMsgCallback> sub_;
+ MonotonicTime prev_msg_timestamp_;
+ Callback callback_;
+ uint8_t num_subsequent_msgs_;
+
+ void invokeCallback(const ReceivedDataStructure<protocol::Panic>& msg)
+ {
+ if (coerceOrFallback<bool>(callback_, true))
+ {
+ callback_(msg);
+ }
+ else
+ {
+ UAVCAN_ASSERT(0); // This is a logic error because normally we shouldn't start with an invalid callback
+ sub_.getNode().registerInternalFailure("PanicListener invalid callback");
+ }
+ }
+
+ void handleMsg(const ReceivedDataStructure<protocol::Panic>& msg)
+ {
+ UAVCAN_TRACE("PanicListener", "Received panic from snid=%i reason=%s",
+ int(msg.getSrcNodeID().get()), msg.reason_text.c_str());
+ if (prev_msg_timestamp_.isZero())
+ {
+ num_subsequent_msgs_ = 1;
+ prev_msg_timestamp_ = msg.getMonotonicTimestamp();
+ }
+ else
+ {
+ const MonotonicDuration diff = msg.getMonotonicTimestamp() - prev_msg_timestamp_;
+ UAVCAN_ASSERT(diff.isPositive() || diff.isZero());
+ if (diff.toMSec() > protocol::Panic::MAX_INTERVAL_MS)
+ {
+ num_subsequent_msgs_ = 1;
+ }
+ else
+ {
+ num_subsequent_msgs_++;
+ }
+ prev_msg_timestamp_ = msg.getMonotonicTimestamp();
+ if (num_subsequent_msgs_ >= protocol::Panic::MIN_MESSAGES)
+ {
+ num_subsequent_msgs_ = protocol::Panic::MIN_MESSAGES;
+ invokeCallback(msg); // The application can stop us from the callback
+ }
+ }
+ }
+
+public:
+ explicit PanicListener(INode& node)
+ : sub_(node)
+ , callback_()
+ , num_subsequent_msgs_(0)
+ { }
+
+ /**
+ * Start the listener.
+ * Once started it does not require further attention.
+ * Returns negative error code.
+ */
+ int start(const Callback& callback)
+ {
+ stop();
+ if (!coerceOrFallback<bool>(callback, true))
+ {
+ UAVCAN_TRACE("PanicListener", "Invalid callback");
+ return -ErrInvalidParam;
+ }
+ callback_ = callback;
+ return sub_.start(PanicMsgCallback(this, &PanicListener::handleMsg));
+ }
+
+ void stop()
+ {
+ sub_.stop();
+ num_subsequent_msgs_ = 0;
+ prev_msg_timestamp_ = MonotonicTime();
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/param_server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,192 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED
+
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/ExecuteOpcode.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <cassert>
+#include <cstdlib>
+
+namespace uavcan
+{
+/**
+ * Implement this interface in the application to support the standard remote reconfiguration services.
+ * Refer to @ref ParamServer.
+ */
+class UAVCAN_EXPORT IParamManager
+{
+public:
+ typedef typename StorageType<typename protocol::param::GetSet::Response::FieldTypes::name>::Type Name;
+ typedef typename StorageType<typename protocol::param::GetSet::Request::FieldTypes::index>::Type Index;
+ typedef protocol::param::Value Value;
+ typedef protocol::param::NumericValue NumericValue;
+
+ virtual ~IParamManager() { }
+
+ /**
+ * Copy the parameter name to @ref out_name if it exists, otherwise do nothing.
+ */
+ virtual void getParamNameByIndex(Index index, Name& out_name) const = 0;
+
+ /**
+ * Assign by name if exists.
+ */
+ virtual void assignParamValue(const Name& name, const Value& value) = 0;
+
+ /**
+ * Read by name if exists, otherwise do nothing.
+ */
+ virtual void readParamValue(const Name& name, Value& out_value) const = 0;
+
+ /**
+ * Read param's default/max/min if available.
+ * Note that min/max are only applicable to numeric params.
+ * Implementation is optional.
+ */
+ virtual void readParamDefaultMaxMin(const Name& name, Value& out_default,
+ NumericValue& out_max, NumericValue& out_min) const
+ {
+ (void)name;
+ (void)out_default;
+ (void)out_max;
+ (void)out_min;
+ }
+
+ /**
+ * Save all params to non-volatile storage.
+ * @return Negative if failed.
+ */
+ virtual int saveAllParams() = 0;
+
+ /**
+ * Clear the non-volatile storage.
+ * @return Negative if failed.
+ */
+ virtual int eraseAllParams() = 0;
+};
+
+/**
+ * Convenience class for supporting the standard configuration services.
+ * Highly recommended to use.
+ */
+class UAVCAN_EXPORT ParamServer
+{
+ typedef MethodBinder<ParamServer*, void (ParamServer::*)(const protocol::param::GetSet::Request&,
+ protocol::param::GetSet::Response&)> GetSetCallback;
+
+ typedef MethodBinder<ParamServer*,
+ void (ParamServer::*)(const protocol::param::ExecuteOpcode::Request&,
+ protocol::param::ExecuteOpcode::Response&)> ExecuteOpcodeCallback;
+
+ ServiceServer<protocol::param::GetSet, GetSetCallback> get_set_srv_;
+ ServiceServer<protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> save_erase_srv_;
+ IParamManager* manager_;
+
+ void handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out)
+ {
+ UAVCAN_ASSERT(manager_ != UAVCAN_NULLPTR);
+
+ // Recover the name from index
+ if (in.name.empty())
+ {
+ manager_->getParamNameByIndex(in.index, out.name);
+ UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str());
+ }
+ else
+ {
+ out.name = in.name;
+ }
+
+ if (out.name.empty())
+ {
+ UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index));
+ return;
+ }
+
+ // Assign if needed, read back
+ if (!in.value.is(protocol::param::Value::Tag::empty))
+ {
+ manager_->assignParamValue(out.name, in.value);
+ }
+ manager_->readParamValue(out.name, out.value);
+
+ // Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about
+ if (!out.value.is(protocol::param::Value::Tag::empty))
+ {
+ manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value);
+ }
+ else
+ {
+ UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str());
+ out.name.clear();
+ }
+ }
+
+ void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in,
+ protocol::param::ExecuteOpcode::Response& out)
+ {
+ UAVCAN_ASSERT(manager_ != UAVCAN_NULLPTR);
+
+ if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE)
+ {
+ out.ok = manager_->saveAllParams() >= 0;
+ }
+ else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE)
+ {
+ out.ok = manager_->eraseAllParams() >= 0;
+ }
+ else
+ {
+ UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode));
+ out.ok = false;
+ }
+ }
+
+public:
+ explicit ParamServer(INode& node)
+ : get_set_srv_(node)
+ , save_erase_srv_(node)
+ , manager_(UAVCAN_NULLPTR)
+ { }
+
+ /**
+ * Starts the parameter server with given param manager instance.
+ * Returns negative error code.
+ */
+ int start(IParamManager* manager)
+ {
+ if (manager == UAVCAN_NULLPTR)
+ {
+ return -ErrInvalidParam;
+ }
+ manager_ = manager;
+
+ int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet));
+ if (res < 0)
+ {
+ return res;
+ }
+
+ res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode));
+ if (res < 0)
+ {
+ get_set_srv_.stop();
+ }
+ return res;
+ }
+
+ /**
+ * @ref IParamManager
+ */
+ IParamManager* getParamManager() const { return manager_; }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/restart_request_server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED
+
+#include <uavcan/debug.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/RestartNode.hpp>
+
+namespace uavcan
+{
+/**
+ * Implement this interface in the application to support the standard node restart service.
+ */
+class UAVCAN_EXPORT IRestartRequestHandler
+{
+public:
+ virtual ~IRestartRequestHandler() { }
+
+ /**
+ * This method shall do either:
+ * - restart the local node immediately;
+ * - initiate the restart procedure to complete it asynchronously;
+ * - reject the restart request and return false.
+ *
+ * If the restart requets was accepted, this method shall either return true or don't return at all.
+ */
+ virtual bool handleRestartRequest(NodeID request_source) = 0;
+};
+
+/**
+ * Convenience class for supporting the standard node restart service.
+ * Highly recommended to use.
+ */
+class UAVCAN_EXPORT RestartRequestServer : Noncopyable
+{
+ typedef MethodBinder<const RestartRequestServer*,
+ void (RestartRequestServer::*)(const ReceivedDataStructure<protocol::RestartNode::Request>&,
+ protocol::RestartNode::Response&) const> RestartNodeCallback;
+
+ ServiceServer<protocol::RestartNode, RestartNodeCallback> srv_;
+ IRestartRequestHandler* handler_;
+
+ void handleRestartNode(const ReceivedDataStructure<protocol::RestartNode::Request>& request,
+ protocol::RestartNode::Response& response) const
+ {
+ UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get()));
+ response.ok = false;
+ if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER)
+ {
+ if (handler_)
+ {
+ response.ok = handler_->handleRestartRequest(request.getSrcNodeID());
+ }
+ UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected"));
+ }
+ else
+ {
+ UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx",
+ static_cast<unsigned long long>(request.magic_number));
+ }
+ }
+
+public:
+ explicit RestartRequestServer(INode& node)
+ : srv_(node)
+ , handler_(UAVCAN_NULLPTR)
+ { }
+
+ /**
+ * Restart request handler configuration.
+ * All restart requests will be explicitly rejected if there's no handler installed.
+ */
+ IRestartRequestHandler* getHandler() const { return handler_; }
+ void setHandler(IRestartRequestHandler* handler) { handler_ = handler; }
+
+ /**
+ * Starts the server.
+ * Returns negative error code.
+ */
+ int start()
+ {
+ return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode));
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/GetTransportStats.hpp>
+
+namespace uavcan
+{
+/**
+ * This class provides statistics about the transport layer performance on the local node.
+ * The user's application does not deal with this class directly because it's instantiated by the node class.
+ */
+class UAVCAN_EXPORT TransportStatsProvider : Noncopyable
+{
+ typedef MethodBinder<const TransportStatsProvider*,
+ void (TransportStatsProvider::*)(const protocol::GetTransportStats::Request&,
+ protocol::GetTransportStats::Response&) const>
+ GetTransportStatsCallback;
+
+ ServiceServer<protocol::GetTransportStats, GetTransportStatsCallback> srv_;
+
+ void handleGetTransportStats(const protocol::GetTransportStats::Request&,
+ protocol::GetTransportStats::Response& resp) const
+ {
+ const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter();
+ resp.transfer_errors = perf.getErrorCount();
+ resp.transfers_tx = perf.getTxTransferCount();
+ resp.transfers_rx = perf.getRxTransferCount();
+
+ const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager();
+ for (uint8_t i = 0; i < canio.getNumIfaces(); i++)
+ {
+ const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i);
+ protocol::CANIfaceStats stats;
+ stats.errors = can_perf.errors;
+ stats.frames_tx = can_perf.frames_tx;
+ stats.frames_rx = can_perf.frames_rx;
+ resp.can_iface_stats.push_back(stats);
+ }
+ }
+
+public:
+ explicit TransportStatsProvider(INode& node)
+ : srv_(node)
+ { }
+
+ /**
+ * Once started, this class requires no further attention.
+ * Returns negative error code.
+ */
+ int start()
+ {
+ return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats));
+ }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/std.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_STD_HPP_INCLUDED
+#define UAVCAN_STD_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <cstdarg>
+#include <cstddef>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+# include <cstdint>
+# include <cstdio>
+
+namespace uavcan
+{
+
+typedef std::uint8_t uint8_t;
+typedef std::uint16_t uint16_t;
+typedef std::uint32_t uint32_t;
+typedef std::uint64_t uint64_t;
+
+typedef std::int8_t int8_t;
+typedef std::int16_t int16_t;
+typedef std::int32_t int32_t;
+typedef std::int64_t int64_t;
+
+}
+
+#else
+
+# include <stdint.h> // Standard integer types from C library
+# include <stdio.h> // vsnprintf() from the C library
+
+namespace uavcan
+{
+
+typedef ::uint8_t uint8_t;
+typedef ::uint16_t uint16_t;
+typedef ::uint32_t uint32_t;
+typedef ::uint64_t uint64_t;
+
+typedef ::int8_t int8_t;
+typedef ::int16_t int16_t;
+typedef ::int32_t int32_t;
+typedef ::int64_t int64_t;
+
+}
+
+#endif
+
+namespace uavcan
+{
+/**
+ * Wrapper over the standard snprintf(). This wrapper is needed because different standards and different
+ * implementations of C++ do not agree whether snprintf() should be defined in std:: or in ::. The solution
+ * is to use 'using namespace std' hack inside the wrapper, so the compiler will be able to pick whatever
+ * definition is available in the standard library. Alternatively, the user's application can provide a
+ * custom implementation of uavcan::snprintf().
+ */
+#if __GNUC__
+__attribute__ ((format(printf, 3, 4)))
+#endif
+extern int snprintf(char* out, std::size_t maxlen, const char* format, ...);
+
+#if !UAVCAN_USE_EXTERNAL_SNPRINTF
+inline int snprintf(char* out, std::size_t maxlen, const char* format, ...)
+{
+ using namespace std; // This way we can pull vsnprintf() either from std:: or from ::.
+ va_list args;
+ va_start(args, format);
+ const int return_value = vsnprintf(out, maxlen, format, args);
+ va_end(args);
+ return return_value;
+}
+#endif
+
+}
+
+#endif // UAVCAN_STD_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/time.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TIME_HPP_INCLUDED
+#define UAVCAN_TIME_HPP_INCLUDED
+
+#include <cstdio>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/Timestamp.hpp>
+
+
+namespace uavcan
+{
+
+template <typename D>
+class DurationBase
+{
+ int64_t usec_;
+
+protected:
+ ~DurationBase() { }
+
+ DurationBase()
+ : usec_(0)
+ {
+ StaticAssert<(sizeof(D) == 8)>::check();
+ }
+
+public:
+ static D getInfinite() { return fromUSec(NumericTraits<int64_t>::max()); }
+
+ static D fromUSec(int64_t us)
+ {
+ D d;
+ d.usec_ = us;
+ return d;
+ }
+ static D fromMSec(int64_t ms) { return fromUSec(ms * 1000); }
+
+ int64_t toUSec() const { return usec_; }
+ int64_t toMSec() const { return usec_ / 1000; }
+
+ D getAbs() const { return D::fromUSec((usec_ < 0) ? (-usec_) : usec_); }
+
+ bool isPositive() const { return usec_ > 0; }
+ bool isNegative() const { return usec_ < 0; }
+ bool isZero() const { return usec_ == 0; }
+
+ bool operator==(const D& r) const { return usec_ == r.usec_; }
+ bool operator!=(const D& r) const { return !operator==(r); }
+
+ bool operator<(const D& r) const { return usec_ < r.usec_; }
+ bool operator>(const D& r) const { return usec_ > r.usec_; }
+ bool operator<=(const D& r) const { return usec_ <= r.usec_; }
+ bool operator>=(const D& r) const { return usec_ >= r.usec_; }
+
+ D operator+(const D& r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check
+ D operator-(const D& r) const { return fromUSec(usec_ - r.usec_); } // ditto
+
+ D operator-() const { return fromUSec(-usec_); }
+
+ D& operator+=(const D& r)
+ {
+ *this = *this + r;
+ return *static_cast<D*>(this);
+ }
+ D& operator-=(const D& r)
+ {
+ *this = *this - r;
+ return *static_cast<D*>(this);
+ }
+
+ template <typename Scale>
+ D operator*(Scale scale) const { return fromUSec(usec_ * scale); }
+
+ template <typename Scale>
+ D& operator*=(Scale scale)
+ {
+ *this = *this * scale;
+ return *static_cast<D*>(this);
+ }
+
+ static const unsigned StringBufSize = 32;
+ void toString(char buf[StringBufSize]) const; ///< Prints time in seconds with microsecond resolution
+#if UAVCAN_TOSTRING
+ std::string toString() const; ///< Prints time in seconds with microsecond resolution
+#endif
+};
+
+
+template <typename T, typename D>
+class TimeBase
+{
+ uint64_t usec_;
+
+protected:
+ ~TimeBase() { }
+
+ TimeBase()
+ : usec_(0)
+ {
+ StaticAssert<(sizeof(T) == 8)>::check();
+ StaticAssert<(sizeof(D) == 8)>::check();
+ }
+
+public:
+ static T getMax() { return fromUSec(NumericTraits<uint64_t>::max()); }
+
+ static T fromUSec(uint64_t us)
+ {
+ T d;
+ d.usec_ = us;
+ return d;
+ }
+ static T fromMSec(uint64_t ms) { return fromUSec(ms * 1000); }
+
+ uint64_t toUSec() const { return usec_; }
+ uint64_t toMSec() const { return usec_ / 1000; }
+
+ bool isZero() const { return usec_ == 0; }
+
+ bool operator==(const T& r) const { return usec_ == r.usec_; }
+ bool operator!=(const T& r) const { return !operator==(r); }
+
+ bool operator<(const T& r) const { return usec_ < r.usec_; }
+ bool operator>(const T& r) const { return usec_ > r.usec_; }
+ bool operator<=(const T& r) const { return usec_ <= r.usec_; }
+ bool operator>=(const T& r) const { return usec_ >= r.usec_; }
+
+ T operator+(const D& r) const
+ {
+ if (r.isNegative())
+ {
+ if (uint64_t(r.getAbs().toUSec()) > usec_)
+ {
+ return fromUSec(0);
+ }
+ }
+ else
+ {
+ if (uint64_t(int64_t(usec_) + r.toUSec()) < usec_)
+ {
+ return fromUSec(NumericTraits<uint64_t>::max());
+ }
+ }
+ return fromUSec(uint64_t(int64_t(usec_) + r.toUSec()));
+ }
+
+ T operator-(const D& r) const
+ {
+ return *static_cast<const T*>(this) + (-r);
+ }
+ D operator-(const T& r) const
+ {
+ return D::fromUSec((usec_ > r.usec_) ? int64_t(usec_ - r.usec_) : -int64_t(r.usec_ - usec_));
+ }
+
+ T& operator+=(const D& r)
+ {
+ *this = *this + r;
+ return *static_cast<T*>(this);
+ }
+ T& operator-=(const D& r)
+ {
+ *this = *this - r;
+ return *static_cast<T*>(this);
+ }
+
+ static const unsigned StringBufSize = 32;
+ void toString(char buf[StringBufSize]) const; ///< Prints time in seconds with microsecond resolution
+#if UAVCAN_TOSTRING
+ std::string toString() const; ///< Prints time in seconds with microsecond resolution
+#endif
+};
+
+/*
+ * Monotonic
+ */
+class UAVCAN_EXPORT MonotonicDuration : public DurationBase<MonotonicDuration> { };
+
+class UAVCAN_EXPORT MonotonicTime : public TimeBase<MonotonicTime, MonotonicDuration> { };
+
+/*
+ * UTC
+ */
+class UAVCAN_EXPORT UtcDuration : public DurationBase<UtcDuration> { };
+
+class UAVCAN_EXPORT UtcTime : public TimeBase<UtcTime, UtcDuration> /// Implicitly convertible to/from uavcan.Timestamp
+{
+public:
+ UtcTime() { }
+
+ UtcTime(const Timestamp& ts) // Implicit
+ {
+ operator=(ts);
+ }
+
+ UtcTime& operator=(const Timestamp& ts)
+ {
+ *this = fromUSec(ts.usec);
+ return *this;
+ }
+
+ operator Timestamp() const
+ {
+ Timestamp ts;
+ ts.usec = toUSec();
+ return ts;
+ }
+};
+
+// ----------------------------------------------------------------------------
+
+template <typename D>
+const unsigned DurationBase<D>::StringBufSize;
+
+template <typename T, typename D>
+const unsigned TimeBase<T, D>::StringBufSize;
+
+template <typename D>
+void DurationBase<D>::toString(char buf[StringBufSize]) const
+{
+ char* ptr = buf;
+ if (isNegative())
+ {
+ *ptr++ = '-';
+ }
+ (void)snprintf(ptr, StringBufSize - 1, "%llu.%06lu",
+ static_cast<unsigned long long>(getAbs().toUSec() / 1000000L),
+ static_cast<unsigned long>(getAbs().toUSec() % 1000000L));
+}
+
+
+template <typename T, typename D>
+void TimeBase<T, D>::toString(char buf[StringBufSize]) const
+{
+ (void)snprintf(buf, StringBufSize, "%llu.%06lu",
+ static_cast<unsigned long long>(toUSec() / 1000000UL),
+ static_cast<unsigned long>(toUSec() % 1000000UL));
+}
+
+
+#if UAVCAN_TOSTRING
+
+template <typename D>
+std::string DurationBase<D>::toString() const
+{
+ char buf[StringBufSize];
+ toString(buf);
+ return std::string(buf);
+}
+
+template <typename T, typename D>
+std::string TimeBase<T, D>::toString() const
+{
+ char buf[StringBufSize];
+ toString(buf);
+ return std::string(buf);
+}
+
+#endif
+
+
+template <typename Stream, typename D>
+UAVCAN_EXPORT
+Stream& operator<<(Stream& s, const DurationBase<D>& d)
+{
+ char buf[DurationBase<D>::StringBufSize];
+ d.toString(buf);
+ s << buf;
+ return s;
+}
+
+template <typename Stream, typename T, typename D>
+UAVCAN_EXPORT
+Stream& operator<<(Stream& s, const TimeBase<T, D>& t)
+{
+ char buf[TimeBase<T, D>::StringBufSize];
+ t.toString(buf);
+ s << buf;
+ return s;
+}
+
+}
+
+#endif // UAVCAN_TIME_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/std.hpp>
+
+namespace uavcan
+{
+/**
+ * API for transfer buffer users.
+ */
+class UAVCAN_EXPORT ITransferBuffer
+{
+public:
+ virtual ~ITransferBuffer() { }
+
+ virtual int read(unsigned offset, uint8_t* data, unsigned len) const = 0;
+ virtual int write(unsigned offset, const uint8_t* data, unsigned len) = 0;
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ * Ilia Sheremet <illia.sheremet@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/data_type.hpp>
+#include <uavcan/error.hpp>
+#include <uavcan/transport/dispatcher.hpp>
+#include <uavcan/node/abstract_node.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/multiset.hpp>
+
+namespace uavcan
+{
+/**
+ * This class configures hardware acceptance filters (if this feature is present on the particular CAN driver) to
+ * preclude reception of irrelevant CAN frames on the hardware level.
+ *
+ * Configuration starts by creating an object of class @ref CanAcceptanceFilterConfigurator on the stack.
+ * By means of computeConfiguration() method the class determines the number of available HW filters and the number
+ * of listeners. In case if custom configuration required, it is possible to add it through addFilterConfig().
+ * Subsequently obtained configurations are then loaded into the CAN driver by calling the applyConfiguration() method.
+ * If the cumulative number of configurations obtained by computeConfiguration() and addFilterConfig() is higher than
+ * the number of available HW filters, configurations will be merged automatically in the most efficient way.
+ *
+ * Note that if the application adds additional server or subscriber objects after the filters have been configured,
+ * the configuration procedure will have to be performed again.
+ *
+ * The maximum number of CAN acceptance filters is predefined in uavcan/build_config.hpp through a constant
+ * @ref MaxCanAcceptanceFilters. The algorithm doesn't allow to have higher number of HW filters configurations than
+ * defined by MaxCanAcceptanceFilters. You can change this value according to the number specified in your CAN driver
+ * datasheet.
+ */
+class CanAcceptanceFilterConfigurator
+{
+public:
+ /**
+ * These arguments defines whether acceptance filter configuration has anonymous messages or not
+ */
+ enum AnonymousMessages
+ {
+ AcceptAnonymousMessages,
+ IgnoreAnonymousMessages
+ };
+
+private:
+ /**
+ * Below constants based on UAVCAN transport layer specification. Masks and ID's depends on message
+ * TypeID, TransferID (RequestNotResponse - for service types, ServiceNotMessage - for all types of messages).
+ * For more details refer to uavcan.org/CAN_bus_transport_layer_specification.
+ * For clarity let's represent "i" as Data Type ID and "d" as Destination Node Id
+ * DefaultFilterMsgMask = 00000 11111111 11111111 10000000
+ * DefaultFilterMsgID = 00000 iiiiiiii iiiiiiii 00000000, no need to explicitly define, since MsgID initialized
+ * as 0.
+ * DefaultFilterServiceMask = 00000 00000000 01111111 10000000
+ * DefaultFilterServiceID = 00000 00000000 0ddddddd 10000000, all Service Response Frames are accepted by
+ * HW filters.
+ * DefaultAnonMsgMask = 00000 00000000 00000000 11111111
+ * DefaultAnonMsgID = 00000 00000000 00000000 00000000, by default the config is added to accept all anonymous
+ * frames. In case there are no anonymous messages, invoke computeConfiguration(IgnoreAnonymousMessages).
+ */
+ static const unsigned DefaultFilterMsgMask = 0xFFFF80;
+ static const unsigned DefaultFilterServiceMask = 0x7F80;
+ static const unsigned DefaultFilterServiceID = 0x80;
+ static const unsigned DefaultAnonMsgMask = 0xFF;
+ static const unsigned DefaultAnonMsgID = 0x0;
+
+ typedef uavcan::Multiset<CanFilterConfig> MultisetConfigContainer;
+
+ static CanFilterConfig mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_);
+ static uint8_t countBits(uint32_t n_);
+ uint16_t getNumFilters() const;
+
+ /**
+ * Fills the multiset_configs_ to proceed it with mergeConfigurations()
+ */
+ int16_t loadInputConfiguration(AnonymousMessages load_mode);
+
+ /**
+ * This method merges several listeners's filter configurations by predetermined algorithm
+ * if number of available hardware acceptance filters less than number of listeners
+ */
+ int16_t mergeConfigurations();
+
+ INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher
+ MultisetConfigContainer multiset_configs_;
+ uint16_t filters_number_;
+
+public:
+ /**
+ * @param node Libuavcan node whose subscribers/servers/etc will be used to configure the filters.
+ *
+ * @param filters_number Allows to override the maximum number of hardware filters to use.
+ * If set to zero (which is default), the class will obtain the number of available
+ * filters from the CAN driver via @ref ICanIface::getNumFilters().
+ */
+ explicit CanAcceptanceFilterConfigurator(INode& node, uint16_t filters_number = 0)
+ : node_(node)
+ , multiset_configs_(node.getAllocator())
+ , filters_number_(filters_number)
+ { }
+
+ /**
+ * This method invokes loadInputConfiguration() and mergeConfigurations() consequently
+ * in order to comute optimal filter configurations for the current hardware.
+ *
+ * It can only be invoked when all of the subscriber and server objects are initialized.
+ * If new subscriber or server objects are added later, the filters will have to be reconfigured again.
+ *
+ * @param mode Either: AcceptAnonymousMessages - the filters will accept all anonymous messages (this is default)
+ * IgnoreAnonymousMessages - anonymous messages will be ignored
+ * @return 0 = success, negative for error.
+ */
+ int computeConfiguration(AnonymousMessages mode = AcceptAnonymousMessages);
+
+ /**
+ * Add an additional filter configuration.
+ * This method must not be invoked after @ref computeConfiguration().
+ * @return 0 = success, negative for error.
+ */
+ int addFilterConfig(const CanFilterConfig& config);
+
+ /**
+ * This method loads the configuration computed with mergeConfigurations() or explicitly added by addFilterConfig()
+ * to the CAN driver. Must be called after computeConfiguration() and addFilterConfig().
+ * @return 0 = success, negative for error.
+ */
+ int applyConfiguration();
+
+ /**
+ * Returns the configuration computed with mergeConfigurations() or added by addFilterConfig().
+ * If mergeConfigurations() or addFilterConfig() have not been called yet, an empty configuration will be returned.
+ */
+ const MultisetConfigContainer& getConfiguration() const
+ {
+ return multiset_configs_;
+ }
+};
+
+/**
+ * This function is a shortcut for @ref CanAcceptanceFilterConfigurator.
+ * It allows to compute filter configuration and then apply it in just one step.
+ * It implements only the most common use case; if you have special requirements,
+ * use @ref CanAcceptanceFilterConfigurator directly.
+ *
+ * @param node Refer to @ref CanAcceptanceFilterConfigurator constructor for explanation.
+ * @param mode Refer to @ref CanAcceptanceFilterConfigurator::computeConfiguration() for explanation.
+ * @return non-negative on success, negative error code on error.
+ */
+static inline int configureCanAcceptanceFilters(INode& node,
+ CanAcceptanceFilterConfigurator::AnonymousMessages mode
+ = CanAcceptanceFilterConfigurator::AcceptAnonymousMessages)
+{
+ CanAcceptanceFilterConfigurator cfger(node);
+
+ const int compute_res = cfger.computeConfiguration(mode);
+ if (compute_res < 0)
+ {
+ return compute_res;
+ }
+
+ return cfger.applyConfiguration();
+}
+
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/can_io.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,186 @@
+/*
+ * CAN bus IO logic.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/error.hpp>
+#include <uavcan/std.hpp>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/driver/can.hpp>
+#include <uavcan/driver/system_clock.hpp>
+#include <uavcan/time.hpp>
+
+namespace uavcan
+{
+
+struct UAVCAN_EXPORT CanRxFrame : public CanFrame
+{
+ MonotonicTime ts_mono;
+ UtcTime ts_utc;
+ uint8_t iface_index;
+
+ CanRxFrame()
+ : iface_index(0)
+ { }
+
+#if UAVCAN_TOSTRING
+ std::string toString(StringRepresentation mode = StrTight) const;
+#endif
+};
+
+
+class UAVCAN_EXPORT CanTxQueue : Noncopyable
+{
+public:
+ enum Qos { Volatile, Persistent };
+
+ struct Entry : public LinkedListNode<Entry> // Not required to be packed - fits the block in any case
+ {
+ MonotonicTime deadline;
+ CanFrame frame;
+ uint8_t qos;
+ CanIOFlags flags;
+
+ Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags)
+ : deadline(arg_deadline)
+ , frame(arg_frame)
+ , qos(uint8_t(arg_qos))
+ , flags(arg_flags)
+ {
+ UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent));
+ IsDynamicallyAllocatable<Entry>::check();
+ }
+
+ static void destroy(Entry*& obj, IPoolAllocator& allocator);
+
+ bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; }
+
+ bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
+ bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
+ bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); }
+ bool qosLowerThan(const Entry& rhs) const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); }
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+ };
+
+private:
+ class PriorityInsertionComparator
+ {
+ const CanFrame& frm_;
+ public:
+ explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { }
+ bool operator()(const Entry* entry)
+ {
+ UAVCAN_ASSERT(entry);
+ return frm_.priorityHigherThan(entry->frame);
+ }
+ };
+
+ LinkedListRoot<Entry> queue_;
+ LimitedPoolAllocator allocator_;
+ ISystemClock& sysclock_;
+ uint32_t rejected_frames_cnt_;
+
+ void registerRejectedFrame();
+
+public:
+ CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota)
+ : allocator_(allocator, allocator_quota)
+ , sysclock_(sysclock)
+ , rejected_frames_cnt_(0)
+ { }
+
+ ~CanTxQueue();
+
+ void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags);
+
+ Entry* peek(); // Modifier
+ void remove(Entry*& entry);
+ const CanFrame* getTopPriorityPendingFrame() const;
+
+ /// The 'or equal' condition is necessary to avoid frame reordering.
+ bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const;
+
+ uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }
+
+ bool isEmpty() const { return queue_.isEmpty(); }
+};
+
+
+struct UAVCAN_EXPORT CanIfacePerfCounters
+{
+ uint64_t frames_tx;
+ uint64_t frames_rx;
+ uint64_t errors;
+
+ CanIfacePerfCounters()
+ : frames_tx(0)
+ , frames_rx(0)
+ , errors(0)
+ { }
+};
+
+
+class UAVCAN_EXPORT CanIOManager : Noncopyable
+{
+ struct IfaceFrameCounters
+ {
+ uint64_t frames_tx;
+ uint64_t frames_rx;
+
+ IfaceFrameCounters()
+ : frames_tx(0)
+ , frames_rx(0)
+ { }
+ };
+
+ ICanDriver& driver_;
+ ISystemClock& sysclock_;
+
+ LazyConstructor<CanTxQueue> tx_queues_[MaxCanIfaces];
+ IfaceFrameCounters counters_[MaxCanIfaces];
+
+ const uint8_t num_ifaces_;
+
+ int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
+ int sendFromTxQueue(uint8_t iface_index);
+ int callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces],
+ MonotonicTime blocking_deadline);
+
+public:
+ CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
+ std::size_t mem_blocks_per_iface = 0);
+
+ uint8_t getNumIfaces() const { return num_ifaces_; }
+
+ CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const;
+
+ const ICanDriver& getCanDriver() const { return driver_; }
+ ICanDriver& getCanDriver() { return driver_; }
+
+ uint8_t makePendingTxMask() const;
+
+ /**
+ * Returns:
+ * 0 - rejected/timedout/enqueued
+ * 1+ - sent/received
+ * negative - failure
+ */
+ int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
+ uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
+ int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags);
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/crc.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_CRC_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_CRC_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+
+namespace uavcan
+{
+
+/**
+ * CRC-16-CCITT
+ * Initial value: 0xFFFF
+ * Poly: 0x1021
+ * Reverse: no
+ * Output xor: 0
+ *
+ * import crcmod
+ * crc = crcmod.predefined.Crc('crc-ccitt-false')
+ * crc.update('123456789')
+ * crc.hexdigest()
+ * '29B1'
+ */
+class UAVCAN_EXPORT TransferCRC
+{
+#if !UAVCAN_TINY
+ static const uint16_t Table[256];
+#endif
+
+ uint16_t value_;
+
+public:
+ enum { NumBytes = 2 };
+
+ TransferCRC()
+ : value_(0xFFFFU)
+ { }
+
+#if UAVCAN_TINY
+ void add(uint8_t byte)
+ {
+ value_ ^= uint16_t(uint16_t(byte) << 8);
+ for (uint8_t bit = 8; bit > 0; --bit)
+ {
+ if (value_ & 0x8000U)
+ {
+ value_ = uint16_t(uint16_t(value_ << 1) ^ 0x1021U);
+ }
+ else
+ {
+ value_ = uint16_t(value_ << 1);
+ }
+ }
+ }
+#else
+ void add(uint8_t byte)
+ {
+ value_ = uint16_t(uint16_t((value_ << 8) ^ Table[uint16_t((value_ >> 8) ^ byte) & 0xFFU]) & 0xFFFFU);
+ }
+#endif
+
+ void add(const uint8_t* bytes, unsigned len)
+ {
+ UAVCAN_ASSERT(bytes);
+ while (len--)
+ {
+ add(*bytes++);
+ }
+ }
+
+ uint16_t get() const { return value_; }
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_CRC_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/dispatcher.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,242 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/error.hpp>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/transport/perf_counter.hpp>
+#include <uavcan/transport/transfer_listener.hpp>
+#include <uavcan/transport/outgoing_transfer_registry.hpp>
+#include <uavcan/transport/can_io.hpp>
+#include <uavcan/util/linked_list.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT Dispatcher;
+
+#if !UAVCAN_TINY
+/**
+ * Inherit this class to receive notifications about all TX CAN frames that were transmitted with the loopback flag.
+ */
+class UAVCAN_EXPORT LoopbackFrameListenerBase : public LinkedListNode<LoopbackFrameListenerBase>
+{
+ Dispatcher& dispatcher_;
+
+protected:
+ explicit LoopbackFrameListenerBase(Dispatcher& dispatcher)
+ : dispatcher_(dispatcher)
+ { }
+
+ virtual ~LoopbackFrameListenerBase() { stopListening(); }
+
+ void startListening();
+ void stopListening();
+ bool isListening() const;
+
+ Dispatcher& getDispatcher() { return dispatcher_; }
+
+public:
+ virtual void handleLoopbackFrame(const RxFrame& frame) = 0;
+};
+
+
+class UAVCAN_EXPORT LoopbackFrameListenerRegistry : Noncopyable
+{
+ LinkedListRoot<LoopbackFrameListenerBase> listeners_;
+
+public:
+ void add(LoopbackFrameListenerBase* listener);
+ void remove(LoopbackFrameListenerBase* listener);
+ bool doesExist(const LoopbackFrameListenerBase* listener) const;
+ unsigned getNumListeners() const { return listeners_.getLength(); }
+
+ void invokeListeners(RxFrame& frame);
+};
+
+/**
+ * Implement this interface to receive notifications about all incoming CAN frames, including loopback.
+ */
+class UAVCAN_EXPORT IRxFrameListener
+{
+public:
+ virtual ~IRxFrameListener() { }
+
+ /**
+ * Make sure to filter out loopback frames if they are not wanted.
+ */
+ virtual void handleRxFrame(const CanRxFrame& frame, CanIOFlags flags) = 0;
+};
+#endif
+
+/**
+ * This class performs low-level CAN frame routing.
+ */
+class UAVCAN_EXPORT Dispatcher : Noncopyable
+{
+ CanIOManager canio_;
+ ISystemClock& sysclock_;
+ OutgoingTransferRegistry outgoing_transfer_reg_;
+ TransferPerfCounter perf_;
+
+ class ListenerRegistry
+ {
+ LinkedListRoot<TransferListener> list_;
+
+ class DataTypeIDInsertionComparator
+ {
+ const DataTypeID id_;
+ public:
+ explicit DataTypeIDInsertionComparator(DataTypeID id) : id_(id) { }
+ bool operator()(const TransferListener* listener) const
+ {
+ UAVCAN_ASSERT(listener);
+ return id_ > listener->getDataTypeDescriptor().getID();
+ }
+ };
+
+ public:
+ enum Mode { UniqueListener, ManyListeners };
+
+ bool add(TransferListener* listener, Mode mode);
+ void remove(TransferListener* listener);
+ bool exists(DataTypeID dtid) const;
+ void cleanup(MonotonicTime ts);
+ void handleFrame(const RxFrame& frame);
+
+ unsigned getNumEntries() const { return list_.getLength(); }
+
+ const LinkedListRoot<TransferListener>& getList() const { return list_; }
+ };
+
+ ListenerRegistry lmsg_;
+ ListenerRegistry lsrv_req_;
+ ListenerRegistry lsrv_resp_;
+
+#if !UAVCAN_TINY
+ LoopbackFrameListenerRegistry loopback_listeners_;
+ IRxFrameListener* rx_listener_;
+#endif
+
+ NodeID self_node_id_;
+ bool self_node_id_is_set_;
+
+ void handleFrame(const CanRxFrame& can_frame);
+
+ void handleLoopbackFrame(const CanRxFrame& can_frame);
+
+ void notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags);
+
+public:
+ Dispatcher(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock)
+ : canio_(driver, allocator, sysclock)
+ , sysclock_(sysclock)
+ , outgoing_transfer_reg_(allocator)
+#if !UAVCAN_TINY
+ , rx_listener_(UAVCAN_NULLPTR)
+#endif
+ , self_node_id_(NodeID::Broadcast) // Default
+ , self_node_id_is_set_(false)
+ { }
+
+ /**
+ * This version returns strictly when the deadline is reached.
+ */
+ int spin(MonotonicTime deadline);
+
+ /**
+ * This version does not return until all available frames are processed.
+ */
+ int spinOnce();
+
+ /**
+ * Refer to CanIOManager::send() for the parameter description
+ */
+ int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos,
+ CanIOFlags flags, uint8_t iface_mask);
+
+ void cleanup(MonotonicTime ts);
+
+ bool registerMessageListener(TransferListener* listener);
+ bool registerServiceRequestListener(TransferListener* listener);
+ bool registerServiceResponseListener(TransferListener* listener);
+
+ void unregisterMessageListener(TransferListener* listener);
+ void unregisterServiceRequestListener(TransferListener* listener);
+ void unregisterServiceResponseListener(TransferListener* listener);
+
+ bool hasSubscriber(DataTypeID dtid) const;
+ bool hasPublisher(DataTypeID dtid) const;
+ bool hasServer(DataTypeID dtid) const;
+
+ unsigned getNumMessageListeners() const { return lmsg_.getNumEntries(); }
+ unsigned getNumServiceRequestListeners() const { return lsrv_req_.getNumEntries(); }
+ unsigned getNumServiceResponseListeners() const { return lsrv_resp_.getNumEntries(); }
+
+ /**
+ * These methods can be used to retreive lists of messages, service requests and service responses the
+ * dispatcher is currently listening to.
+ * Note that the list of service response listeners is very volatile, because a response listener will be
+ * removed from this list as soon as the corresponding service call is complete.
+ * @{
+ */
+ const LinkedListRoot<TransferListener>& getListOfMessageListeners() const
+ {
+ return lmsg_.getList();
+ }
+ const LinkedListRoot<TransferListener>& getListOfServiceRequestListeners() const
+ {
+ return lsrv_req_.getList();
+ }
+ const LinkedListRoot<TransferListener>& getListOfServiceResponseListeners() const
+ {
+ return lsrv_resp_.getList();
+ }
+ /**
+ * @}
+ */
+
+ OutgoingTransferRegistry& getOutgoingTransferRegistry() { return outgoing_transfer_reg_; }
+
+#if !UAVCAN_TINY
+ LoopbackFrameListenerRegistry& getLoopbackFrameListenerRegistry() { return loopback_listeners_; }
+
+ IRxFrameListener* getRxFrameListener() const { return rx_listener_; }
+ void removeRxFrameListener() { rx_listener_ = UAVCAN_NULLPTR; }
+ void installRxFrameListener(IRxFrameListener* listener)
+ {
+ UAVCAN_ASSERT(listener != UAVCAN_NULLPTR);
+ rx_listener_ = listener;
+ }
+#endif
+
+ /**
+ * Node ID can be set only once.
+ * Non-unicast Node ID puts the node into passive mode.
+ */
+ NodeID getNodeID() const { return self_node_id_; }
+ bool setNodeID(NodeID nid);
+
+ /**
+ * Refer to the specs to learn more about passive mode.
+ */
+ bool isPassiveMode() const { return !getNodeID().isUnicast(); }
+
+ const ISystemClock& getSystemClock() const { return sysclock_; }
+ ISystemClock& getSystemClock() { return sysclock_; }
+
+ const CanIOManager& getCanIOManager() const { return canio_; }
+ CanIOManager& getCanIOManager() { return canio_; }
+
+ const TransferPerfCounter& getTransferPerfCounter() const { return perf_; }
+ TransferPerfCounter& getTransferPerfCounter() { return perf_; }
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/frame.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/transport/transfer.hpp>
+#include <uavcan/transport/can_io.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/data_type.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT Frame
+{
+ enum { PayloadCapacity = 7 }; // Will be redefined when CAN FD is available
+
+ uint8_t payload_[PayloadCapacity];
+ TransferPriority transfer_priority_;
+ TransferType transfer_type_;
+ DataTypeID data_type_id_;
+ uint_fast8_t payload_len_;
+ NodeID src_node_id_;
+ NodeID dst_node_id_;
+ TransferID transfer_id_;
+ bool start_of_transfer_;
+ bool end_of_transfer_;
+ bool toggle_;
+
+public:
+ Frame() :
+ transfer_type_(TransferType(NumTransferTypes)), // Invalid value
+ payload_len_(0),
+ start_of_transfer_(false),
+ end_of_transfer_(false),
+ toggle_(false)
+ { }
+
+ Frame(DataTypeID data_type_id,
+ TransferType transfer_type,
+ NodeID src_node_id,
+ NodeID dst_node_id,
+ TransferID transfer_id) :
+ transfer_priority_(TransferPriority::Default),
+ transfer_type_(transfer_type),
+ data_type_id_(data_type_id),
+ payload_len_(0),
+ src_node_id_(src_node_id),
+ dst_node_id_(dst_node_id),
+ transfer_id_(transfer_id),
+ start_of_transfer_(false),
+ end_of_transfer_(false),
+ toggle_(false)
+ {
+ UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast());
+ UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type)));
+ UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true);
+ }
+
+ void setPriority(TransferPriority priority) { transfer_priority_ = priority; }
+ TransferPriority getPriority() const { return transfer_priority_; }
+
+ /**
+ * Max payload length depends on the transfer type and frame index.
+ */
+ uint8_t getPayloadCapacity() const { return PayloadCapacity; }
+ uint8_t setPayload(const uint8_t* data, unsigned len);
+
+ unsigned getPayloadLen() const { return payload_len_; }
+ const uint8_t* getPayloadPtr() const { return payload_; }
+
+ TransferType getTransferType() const { return transfer_type_; }
+ DataTypeID getDataTypeID() const { return data_type_id_; }
+ NodeID getSrcNodeID() const { return src_node_id_; }
+ NodeID getDstNodeID() const { return dst_node_id_; }
+ TransferID getTransferID() const { return transfer_id_; }
+
+ void setStartOfTransfer(bool x) { start_of_transfer_ = x; }
+ void setEndOfTransfer(bool x) { end_of_transfer_ = x; }
+
+ bool isStartOfTransfer() const { return start_of_transfer_; }
+ bool isEndOfTransfer() const { return end_of_transfer_; }
+
+ void flipToggle() { toggle_ = !toggle_; }
+ bool getToggle() const { return toggle_; }
+
+ bool parse(const CanFrame& can_frame);
+ bool compile(CanFrame& can_frame) const;
+
+ bool isValid() const;
+
+ bool operator!=(const Frame& rhs) const { return !operator==(rhs); }
+ bool operator==(const Frame& rhs) const;
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+};
+
+
+class UAVCAN_EXPORT RxFrame : public Frame
+{
+ MonotonicTime ts_mono_;
+ UtcTime ts_utc_;
+ uint8_t iface_index_;
+
+public:
+ RxFrame()
+ : iface_index_(0)
+ { }
+
+ RxFrame(const Frame& frame, MonotonicTime ts_mono, UtcTime ts_utc, uint8_t iface_index)
+ : ts_mono_(ts_mono)
+ , ts_utc_(ts_utc)
+ , iface_index_(iface_index)
+ {
+ *static_cast<Frame*>(this) = frame;
+ }
+
+ bool parse(const CanRxFrame& can_frame);
+
+ /**
+ * Can't be zero.
+ */
+ MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
+
+ /**
+ * Can be zero if not supported by the platform driver.
+ */
+ UtcTime getUtcTimestamp() const { return ts_utc_; }
+
+ uint8_t getIfaceIndex() const { return iface_index_; }
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/map.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/transport/transfer.hpp>
+#include <uavcan/time.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT OutgoingTransferRegistryKey
+{
+ DataTypeID data_type_id_;
+ uint8_t transfer_type_;
+ NodeID destination_node_id_; ///< Not applicable for message broadcasting
+
+public:
+ OutgoingTransferRegistryKey()
+ : transfer_type_(0xFF)
+ { }
+
+ OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id)
+ : data_type_id_(data_type_id)
+ , transfer_type_(transfer_type)
+ , destination_node_id_(destination_node_id)
+ {
+ UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast());
+ /*
+ * Service response transfers must use the same Transfer ID as matching service request transfer,
+ * so this registry is not applicable for service response transfers at all.
+ */
+ UAVCAN_ASSERT(transfer_type != TransferTypeServiceResponse);
+ }
+
+ DataTypeID getDataTypeID() const { return data_type_id_; }
+ TransferType getTransferType() const { return TransferType(transfer_type_); }
+
+ bool operator==(const OutgoingTransferRegistryKey& rhs) const
+ {
+ return
+ (data_type_id_ == rhs.data_type_id_) &&
+ (transfer_type_ == rhs.transfer_type_) &&
+ (destination_node_id_ == rhs.destination_node_id_);
+ }
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+};
+
+/**
+ * Outgoing transfer registry keeps track of Transfer ID values for all currently existing local transfer senders.
+ * If a local transfer sender was inactive for a sufficiently long time, the outgoing transfer registry will
+ * remove the respective Transfer ID tracking object.
+ */
+class UAVCAN_EXPORT OutgoingTransferRegistry : Noncopyable
+{
+ struct Value
+ {
+ MonotonicTime deadline;
+ TransferID tid;
+ };
+
+ class DeadlineExpiredPredicate
+ {
+ const MonotonicTime ts_;
+
+ public:
+ explicit DeadlineExpiredPredicate(MonotonicTime ts)
+ : ts_(ts)
+ { }
+
+ bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const
+ {
+ (void)key;
+ UAVCAN_ASSERT(!value.deadline.isZero());
+ const bool expired = value.deadline <= ts_;
+ if (expired)
+ {
+ UAVCAN_TRACE("OutgoingTransferRegistry", "Expired %s tid=%i",
+ key.toString().c_str(), int(value.tid.get()));
+ }
+ return expired;
+ }
+ };
+
+ class ExistenceCheckingPredicate
+ {
+ const DataTypeID dtid_;
+ const TransferType tt_;
+
+ public:
+ ExistenceCheckingPredicate(DataTypeID dtid, TransferType tt)
+ : dtid_(dtid)
+ , tt_(tt)
+ { }
+
+ bool operator()(const OutgoingTransferRegistryKey& key, const Value&) const
+ {
+ return dtid_ == key.getDataTypeID() && tt_ == key.getTransferType();
+ }
+ };
+
+ Map<OutgoingTransferRegistryKey, Value> map_;
+
+public:
+ static const MonotonicDuration MinEntryLifetime;
+
+ explicit OutgoingTransferRegistry(IPoolAllocator& allocator)
+ : map_(allocator)
+ { }
+
+ TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline);
+
+ bool exists(DataTypeID dtid, TransferType tt) const;
+
+ void cleanup(MonotonicTime ts);
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/perf_counter.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED
+
+#include <uavcan/std.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+
+namespace uavcan
+{
+
+#if UAVCAN_TINY
+
+class UAVCAN_EXPORT TransferPerfCounter : Noncopyable
+{
+public:
+ void addTxTransfer() { }
+ void addRxTransfer() { }
+ void addError() { }
+ void addErrors(unsigned) { }
+ uint64_t getTxTransferCount() const { return 0; }
+ uint64_t getRxTransferCount() const { return 0; }
+ uint64_t getErrorCount() const { return 0; }
+};
+
+#else
+
+/**
+ * The class is declared noncopyable for two reasons:
+ * - to prevent accidental pass-by-value into a mutator
+ * - to make the addresses of the counters fixed and exposable to the user of the library
+ */
+class UAVCAN_EXPORT TransferPerfCounter : Noncopyable
+{
+ uint64_t transfers_tx_;
+ uint64_t transfers_rx_;
+ uint64_t errors_;
+
+public:
+ TransferPerfCounter()
+ : transfers_tx_(0)
+ , transfers_rx_(0)
+ , errors_(0)
+ { }
+
+ void addTxTransfer() { transfers_tx_++; }
+ void addRxTransfer() { transfers_rx_++; }
+
+ void addError() { errors_++; }
+
+ void addErrors(unsigned errors)
+ {
+ errors_ += errors;
+ }
+
+ /**
+ * Returned references are guaranteed to be valid as long as this instance of Node exists.
+ * This is enforced by virtue of the class being Noncopyable.
+ */
+ const uint64_t& getTxTransferCount() const { return transfers_tx_; }
+ const uint64_t& getRxTransferCount() const { return transfers_rx_; }
+ const uint64_t& getErrorCount() const { return errors_; }
+};
+
+#endif
+
+}
+
+#endif // UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/transfer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/std.hpp>
+
+namespace uavcan
+{
+
+static const unsigned GuaranteedPayloadLenPerFrame = 7; ///< Guaranteed for all transfers, all CAN standards
+
+enum TransferType
+{
+ TransferTypeServiceResponse = 0,
+ TransferTypeServiceRequest = 1,
+ TransferTypeMessageBroadcast = 2
+};
+
+static const uint8_t NumTransferTypes = 3;
+
+
+class UAVCAN_EXPORT TransferPriority
+{
+ uint8_t value_;
+
+public:
+ static const uint8_t BitLen = 5U;
+ static const uint8_t NumericallyMax = (1U << BitLen) - 1;
+ static const uint8_t NumericallyMin = 0;
+
+ /// This priority is used by default
+ static const TransferPriority Default;
+ static const TransferPriority MiddleLower;
+ static const TransferPriority OneHigherThanLowest;
+ static const TransferPriority OneLowerThanHighest;
+ static const TransferPriority Lowest;
+
+ TransferPriority() : value_(0xFF) { }
+
+ TransferPriority(uint8_t value) // Implicit
+ : value_(value)
+ {
+ UAVCAN_ASSERT(isValid());
+ }
+
+ template <uint8_t Percent>
+ static TransferPriority fromPercent()
+ {
+ StaticAssert<(Percent <= 100)>::check();
+ enum { Result = ((100U - Percent) * NumericallyMax) / 100U };
+ StaticAssert<(Result <= NumericallyMax)>::check();
+ StaticAssert<(Result >= NumericallyMin)>::check();
+ return TransferPriority(Result);
+ }
+
+ uint8_t get() const { return value_; }
+
+ bool isValid() const { return value_ < (1U << BitLen); }
+
+ bool operator!=(TransferPriority rhs) const { return value_ != rhs.value_; }
+ bool operator==(TransferPriority rhs) const { return value_ == rhs.value_; }
+};
+
+
+class UAVCAN_EXPORT TransferID
+{
+ uint8_t value_;
+
+public:
+ static const uint8_t BitLen = 5U;
+ static const uint8_t Max = (1U << BitLen) - 1U;
+ static const uint8_t Half = (1U << BitLen) / 2U;
+
+ TransferID()
+ : value_(0)
+ { }
+
+ TransferID(uint8_t value) // implicit
+ : value_(value)
+ {
+ value_ &= Max;
+ UAVCAN_ASSERT(value == value_);
+ }
+
+ bool operator!=(TransferID rhs) const { return !operator==(rhs); }
+ bool operator==(TransferID rhs) const { return get() == rhs.get(); }
+
+ void increment()
+ {
+ value_ = (value_ + 1) & Max;
+ }
+
+ uint8_t get() const
+ {
+ UAVCAN_ASSERT(value_ <= Max);
+ return value_;
+ }
+
+ /**
+ * Amount of increment() calls to reach rhs value.
+ */
+ int computeForwardDistance(TransferID rhs) const;
+};
+
+
+class UAVCAN_EXPORT NodeID
+{
+ static const uint8_t ValueBroadcast = 0;
+ static const uint8_t ValueInvalid = 0xFF;
+ uint8_t value_;
+
+public:
+ static const uint8_t BitLen = 7U;
+ static const uint8_t Max = (1U << BitLen) - 1U;
+ static const uint8_t MaxRecommendedForRegularNodes = Max - 2;
+ static const NodeID Broadcast;
+
+ NodeID() : value_(ValueInvalid) { }
+
+ NodeID(uint8_t value) // Implicit
+ : value_(value)
+ {
+ UAVCAN_ASSERT(isValid());
+ }
+
+ uint8_t get() const { return value_; }
+
+ bool isValid() const { return value_ <= Max; }
+ bool isBroadcast() const { return value_ == ValueBroadcast; }
+ bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); }
+
+ bool operator!=(NodeID rhs) const { return !operator==(rhs); }
+ bool operator==(NodeID rhs) const { return value_ == rhs.value_; }
+
+ bool operator<(NodeID rhs) const { return value_ < rhs.value_; }
+ bool operator>(NodeID rhs) const { return value_ > rhs.value_; }
+ bool operator<=(NodeID rhs) const { return value_ <= rhs.value_; }
+ bool operator>=(NodeID rhs) const { return value_ >= rhs.value_; }
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/transfer_buffer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,199 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED
+
+#include <uavcan/std.hpp>
+#include <uavcan/error.hpp>
+#include <uavcan/transport/frame.hpp>
+#include <uavcan/transport/abstract_transfer_buffer.hpp>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+
+namespace uavcan
+{
+/**
+ * Standalone static buffer
+ */
+class StaticTransferBufferImpl : public ITransferBuffer
+{
+ uint8_t* const data_;
+ const uint16_t size_;
+ uint16_t max_write_pos_;
+
+public:
+ StaticTransferBufferImpl(uint8_t* buf, uint16_t buf_size) :
+ data_(buf),
+ size_(buf_size),
+ max_write_pos_(0)
+ { }
+
+ virtual int read(unsigned offset, uint8_t* data, unsigned len) const;
+ virtual int write(unsigned offset, const uint8_t* data, unsigned len);
+
+ void reset();
+
+ uint16_t getSize() const { return size_; }
+
+ uint8_t* getRawPtr() { return data_; }
+ const uint8_t* getRawPtr() const { return data_; }
+
+ uint16_t getMaxWritePos() const { return max_write_pos_; }
+ void setMaxWritePos(uint16_t value) { max_write_pos_ = value; }
+};
+
+template <uint16_t Size>
+class UAVCAN_EXPORT StaticTransferBuffer : public StaticTransferBufferImpl
+{
+ uint8_t buffer_[Size];
+public:
+ StaticTransferBuffer() : StaticTransferBufferImpl(buffer_, Size)
+ {
+ StaticAssert<(Size > 0)>::check();
+ }
+};
+
+/**
+ * Internal for TransferBufferManager
+ */
+class UAVCAN_EXPORT TransferBufferManagerKey
+{
+ NodeID node_id_;
+ uint8_t transfer_type_;
+
+public:
+ TransferBufferManagerKey()
+ : transfer_type_(TransferType(0))
+ {
+ UAVCAN_ASSERT(isEmpty());
+ }
+
+ TransferBufferManagerKey(NodeID node_id, TransferType ttype)
+ : node_id_(node_id)
+ , transfer_type_(ttype)
+ {
+ UAVCAN_ASSERT(!isEmpty());
+ }
+
+ bool operator==(const TransferBufferManagerKey& rhs) const
+ {
+ return node_id_ == rhs.node_id_ && transfer_type_ == rhs.transfer_type_;
+ }
+
+ bool isEmpty() const { return !node_id_.isValid(); }
+
+ NodeID getNodeID() const { return node_id_; }
+ TransferType getTransferType() const { return TransferType(transfer_type_); }
+
+#if UAVCAN_TOSTRING
+ std::string toString() const;
+#endif
+};
+
+/**
+ * Resizable gather/scatter storage.
+ * reset() call releases all memory blocks.
+ * Supports unordered write operations - from higher to lower offsets
+ */
+class UAVCAN_EXPORT TransferBufferManagerEntry : public ITransferBuffer
+ , public LinkedListNode<TransferBufferManagerEntry>
+{
+ struct Block : LinkedListNode<Block>
+ {
+ enum { Size = MemPoolBlockSize - sizeof(LinkedListNode<Block>) };
+ uint8_t data[static_cast<unsigned>(Size)];
+
+ static Block* instantiate(IPoolAllocator& allocator);
+ static void destroy(Block*& obj, IPoolAllocator& allocator);
+
+ void read(uint8_t*& outptr, unsigned target_offset,
+ unsigned& total_offset, unsigned& left_to_read);
+ void write(const uint8_t*& inptr, unsigned target_offset,
+ unsigned& total_offset, unsigned& left_to_write);
+ };
+
+ IPoolAllocator& allocator_;
+ LinkedListRoot<Block> blocks_; // Blocks are ordered from lower to higher buffer offset
+ uint16_t max_write_pos_;
+ const uint16_t max_size_;
+ TransferBufferManagerKey key_;
+
+public:
+ TransferBufferManagerEntry(IPoolAllocator& allocator, uint16_t max_size) :
+ allocator_(allocator),
+ max_write_pos_(0),
+ max_size_(max_size)
+ {
+ StaticAssert<(Block::Size > 8)>::check();
+ IsDynamicallyAllocatable<Block>::check();
+ IsDynamicallyAllocatable<TransferBufferManagerEntry>::check();
+ }
+
+ virtual ~TransferBufferManagerEntry() { reset(); }
+
+ static TransferBufferManagerEntry* instantiate(IPoolAllocator& allocator, uint16_t max_size);
+ static void destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator);
+
+ virtual int read(unsigned offset, uint8_t* data, unsigned len) const;
+ virtual int write(unsigned offset, const uint8_t* data, unsigned len);
+
+ void reset(const TransferBufferManagerKey& key = TransferBufferManagerKey());
+
+ const TransferBufferManagerKey& getKey() const { return key_; }
+ bool isEmpty() const { return key_.isEmpty(); }
+};
+
+/**
+ * Buffer manager implementation.
+ */
+class TransferBufferManager : public Noncopyable
+{
+ LinkedListRoot<TransferBufferManagerEntry> buffers_;
+ IPoolAllocator& allocator_;
+ const uint16_t max_buf_size_;
+
+ TransferBufferManagerEntry* findFirst(const TransferBufferManagerKey& key);
+
+public:
+ TransferBufferManager(uint16_t max_buf_size, IPoolAllocator& allocator) :
+ allocator_(allocator),
+ max_buf_size_(max_buf_size)
+ { }
+
+ ~TransferBufferManager();
+
+ ITransferBuffer* access(const TransferBufferManagerKey& key);
+ ITransferBuffer* create(const TransferBufferManagerKey& key);
+ void remove(const TransferBufferManagerKey& key);
+ bool isEmpty() const;
+
+ unsigned getNumBuffers() const;
+};
+
+/**
+ * Convinience class.
+ */
+class UAVCAN_EXPORT TransferBufferAccessor
+{
+ TransferBufferManager& bufmgr_;
+ const TransferBufferManagerKey key_;
+
+public:
+ TransferBufferAccessor(TransferBufferManager& bufmgr, TransferBufferManagerKey key) :
+ bufmgr_(bufmgr),
+ key_(key)
+ {
+ UAVCAN_ASSERT(!key.isEmpty());
+ }
+ ITransferBuffer* access() { return bufmgr_.access(key_); }
+ ITransferBuffer* create() { return bufmgr_.create(key_); }
+ void remove() { bufmgr_.remove(key_); }
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,194 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED
+
+#include <cassert>
+#include <uavcan/error.hpp>
+#include <uavcan/std.hpp>
+#include <uavcan/transport/transfer_receiver.hpp>
+#include <uavcan/transport/perf_counter.hpp>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/util/map.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/transport/crc.hpp>
+#include <uavcan/data_type.hpp>
+
+namespace uavcan
+{
+/**
+ * Container for received transfer.
+ */
+class UAVCAN_EXPORT IncomingTransfer : public ITransferBuffer
+{
+ MonotonicTime ts_mono_;
+ UtcTime ts_utc_;
+ TransferPriority transfer_priority_;
+ TransferType transfer_type_;
+ TransferID transfer_id_;
+ NodeID src_node_id_;
+ uint8_t iface_index_;
+
+ /// That's a no-op, asserts in debug builds
+ virtual int write(unsigned offset, const uint8_t* data, unsigned len);
+
+protected:
+ IncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, TransferPriority transfer_priority,
+ TransferType transfer_type, TransferID transfer_id, NodeID source_node_id,
+ uint8_t iface_index)
+ : ts_mono_(ts_mono)
+ , ts_utc_(ts_utc)
+ , transfer_priority_(transfer_priority)
+ , transfer_type_(transfer_type)
+ , transfer_id_(transfer_id)
+ , src_node_id_(source_node_id)
+ , iface_index_(iface_index)
+ { }
+
+public:
+ /**
+ * Dispose the payload buffer. Further calls to read() will not be possible.
+ */
+ virtual void release() { }
+
+ /**
+ * Whether this is a anonymous transfer
+ */
+ virtual bool isAnonymousTransfer() const { return false; }
+
+ MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
+ UtcTime getUtcTimestamp() const { return ts_utc_; }
+ TransferPriority getPriority() const { return transfer_priority_; }
+ TransferType getTransferType() const { return transfer_type_; }
+ TransferID getTransferID() const { return transfer_id_; }
+ NodeID getSrcNodeID() const { return src_node_id_; }
+ uint8_t getIfaceIndex() const { return iface_index_; }
+};
+
+/**
+ * Internal.
+ */
+class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer
+{
+ const uint8_t* const payload_;
+ const uint8_t payload_len_;
+public:
+ explicit SingleFrameIncomingTransfer(const RxFrame& frm);
+ virtual int read(unsigned offset, uint8_t* data, unsigned len) const;
+ virtual bool isAnonymousTransfer() const;
+};
+
+/**
+ * Internal.
+ */
+class UAVCAN_EXPORT MultiFrameIncomingTransfer : public IncomingTransfer, Noncopyable
+{
+ TransferBufferAccessor& buf_acc_;
+public:
+ MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, const RxFrame& last_frame,
+ TransferBufferAccessor& tba);
+ virtual int read(unsigned offset, uint8_t* data, unsigned len) const;
+ virtual void release() { buf_acc_.remove(); }
+};
+
+/**
+ * Internal, refer to the transport dispatcher class.
+ */
+class UAVCAN_EXPORT TransferListener : public LinkedListNode<TransferListener>
+{
+ const DataTypeDescriptor& data_type_;
+ TransferBufferManager bufmgr_;
+ Map<TransferBufferManagerKey, TransferReceiver> receivers_;
+ TransferPerfCounter& perf_;
+ const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant
+ bool allow_anonymous_transfers_;
+
+ class TimedOutReceiverPredicate
+ {
+ const MonotonicTime ts_;
+ TransferBufferManager& parent_bufmgr_;
+
+ public:
+ TimedOutReceiverPredicate(MonotonicTime arg_ts, TransferBufferManager& arg_bufmgr)
+ : ts_(arg_ts)
+ , parent_bufmgr_(arg_bufmgr)
+ { }
+
+ bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const;
+ };
+
+ bool checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const;
+
+protected:
+ void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba);
+ void handleAnonymousTransferReception(const RxFrame& frame);
+
+ virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0;
+
+public:
+ TransferListener(TransferPerfCounter& perf, const DataTypeDescriptor& data_type,
+ uint16_t max_buffer_size, IPoolAllocator& allocator)
+ : data_type_(data_type)
+ , bufmgr_(max_buffer_size, allocator)
+ , receivers_(allocator)
+ , perf_(perf)
+ , crc_base_(data_type.getSignature().toTransferCRC())
+ , allow_anonymous_transfers_(false)
+ { }
+
+ virtual ~TransferListener();
+
+ const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; }
+
+ /**
+ * By default, anonymous transfers will be ignored.
+ * This option allows to enable reception of anonymous transfers.
+ */
+ void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; }
+
+ void cleanup(MonotonicTime ts);
+
+ virtual void handleFrame(const RxFrame& frame);
+};
+
+/**
+ * This class is used by transfer listener to decide if the frame should be accepted or ignored.
+ */
+class ITransferAcceptanceFilter
+{
+public:
+ /**
+ * If it returns false, the frame will be ignored, otherwise accepted.
+ */
+ virtual bool shouldAcceptFrame(const RxFrame& frame) const = 0;
+
+ virtual ~ITransferAcceptanceFilter() { }
+};
+
+/**
+ * This class should be derived by callers.
+ */
+class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener
+{
+ const ITransferAcceptanceFilter* filter_;
+
+ virtual void handleFrame(const RxFrame& frame);
+
+public:
+ TransferListenerWithFilter(TransferPerfCounter& perf, const DataTypeDescriptor& data_type,
+ uint16_t max_buffer_size, IPoolAllocator& allocator)
+ : TransferListener(perf, data_type, max_buffer_size, allocator)
+ , filter_(UAVCAN_NULLPTR)
+ { }
+
+ void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter)
+ {
+ filter_ = acceptance_filter;
+ }
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/transfer_receiver.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED
+
+#include <cstdlib>
+#include <uavcan/build_config.hpp>
+#include <uavcan/transport/frame.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT TransferReceiver
+{
+public:
+ enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame };
+
+ static const uint16_t MinTransferIntervalMSec = 1;
+ static const uint16_t MaxTransferIntervalMSec = 0xFFFF;
+ static const uint16_t DefaultTransferIntervalMSec = 1000;
+ static const uint16_t DefaultTidTimeoutMSec = 1000;
+
+ static MonotonicDuration getDefaultTransferInterval()
+ {
+ return MonotonicDuration::fromMSec(DefaultTransferIntervalMSec);
+ }
+ static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromMSec(MinTransferIntervalMSec); }
+ static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromMSec(MaxTransferIntervalMSec); }
+
+private:
+ enum { IfaceIndexNotSet = MaxCanIfaces };
+
+ enum { ErrorCntMask = 31 };
+ enum { IfaceIndexMask = MaxCanIfaces };
+
+ MonotonicTime prev_transfer_ts_;
+ MonotonicTime this_transfer_ts_;
+ UtcTime first_frame_ts_;
+ uint16_t transfer_interval_msec_;
+ uint16_t this_transfer_crc_;
+
+ uint16_t buffer_write_pos_;
+
+ TransferID tid_; // 1 byte field
+
+ // 1 byte aligned bitfields:
+ uint8_t next_toggle_ : 1;
+ uint8_t iface_index_ : 2;
+ mutable uint8_t error_cnt_ : 5;
+
+ bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; }
+
+ bool isMidTransfer() const { return buffer_write_pos_ > 0; }
+
+ MonotonicDuration getIfaceSwitchDelay() const;
+ MonotonicDuration getTidTimeout() const;
+
+ void registerError() const;
+
+ void updateTransferTimings();
+ void prepareForNextTransfer();
+
+ bool validate(const RxFrame& frame) const;
+ bool writePayload(const RxFrame& frame, ITransferBuffer& buf);
+ ResultCode receive(const RxFrame& frame, TransferBufferAccessor& tba);
+
+public:
+ TransferReceiver() :
+ transfer_interval_msec_(DefaultTransferIntervalMSec),
+ this_transfer_crc_(0),
+ buffer_write_pos_(0),
+ next_toggle_(false),
+ iface_index_(IfaceIndexNotSet),
+ error_cnt_(0)
+ { }
+
+ bool isTimedOut(MonotonicTime current_ts) const;
+
+ ResultCode addFrame(const RxFrame& frame, TransferBufferAccessor& tba);
+
+ uint8_t yieldErrorCount();
+
+ MonotonicTime getLastTransferTimestampMonotonic() const { return prev_transfer_ts_; }
+ UtcTime getLastTransferTimestampUtc() const { return first_frame_ts_; }
+
+ uint16_t getLastTransferCrc() const { return this_transfer_crc_; }
+
+ MonotonicDuration getInterval() const { return MonotonicDuration::fromMSec(transfer_interval_msec_); }
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED
+
+#include <cstdlib>
+#include <cassert>
+#include <uavcan/build_config.hpp>
+#include <uavcan/error.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/transport/crc.hpp>
+#include <uavcan/transport/transfer.hpp>
+#include <uavcan/transport/dispatcher.hpp>
+
+namespace uavcan
+{
+
+class UAVCAN_EXPORT TransferSender
+{
+ const MonotonicDuration max_transfer_interval_;
+
+ Dispatcher& dispatcher_;
+
+ TransferPriority priority_;
+ CanTxQueue::Qos qos_;
+ TransferCRC crc_base_;
+ DataTypeID data_type_id_;
+ CanIOFlags flags_;
+ uint8_t iface_mask_;
+ bool allow_anonymous_transfers_;
+
+ void registerError() const;
+
+public:
+ enum { AllIfacesMask = 0xFF };
+
+ static MonotonicDuration getDefaultMaxTransferInterval()
+ {
+ return MonotonicDuration::fromMSec(60 * 1000);
+ }
+
+ TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
+ MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
+ : max_transfer_interval_(max_transfer_interval)
+ , dispatcher_(dispatcher)
+ , priority_(TransferPriority::Default)
+ , qos_(CanTxQueue::Qos())
+ , flags_(CanIOFlags(0))
+ , iface_mask_(AllIfacesMask)
+ , allow_anonymous_transfers_(false)
+ {
+ init(data_type, qos);
+ }
+
+ TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
+ : max_transfer_interval_(max_transfer_interval)
+ , dispatcher_(dispatcher)
+ , priority_(TransferPriority::Default)
+ , qos_(CanTxQueue::Qos())
+ , flags_(CanIOFlags(0))
+ , iface_mask_(AllIfacesMask)
+ , allow_anonymous_transfers_(false)
+ { }
+
+ void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos);
+
+ bool isInitialized() const { return data_type_id_ != DataTypeID(); }
+
+ CanIOFlags getCanIOFlags() const { return flags_; }
+ void setCanIOFlags(CanIOFlags flags) { flags_ = flags; }
+
+ uint8_t getIfaceMask() const { return iface_mask_; }
+ void setIfaceMask(uint8_t iface_mask)
+ {
+ UAVCAN_ASSERT(iface_mask);
+ iface_mask_ = iface_mask;
+ }
+
+ TransferPriority getPriority() const { return priority_; }
+ void setPriority(TransferPriority prio) { priority_ = prio; }
+
+ /**
+ * Anonymous transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if
+ * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID).
+ * By default, this class will return an error if it is asked to send a transfer while the
+ * node is configured in passive mode. However, if this option is enabled, it will be possible
+ * to send anonymous transfers from passive mode.
+ */
+ void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; }
+
+ /**
+ * Send with explicit Transfer ID.
+ * Should be used only for service responses, where response TID should match request TID.
+ */
+ int send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
+ MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
+ TransferID tid) const;
+
+ /**
+ * Send with automatic Transfer ID.
+ *
+ * Note that as long as the local node operates in passive mode, the
+ * flag @ref CanIOFlagAbortOnError will be set implicitly for all outgoing frames.
+ *
+ * TID is managed by OutgoingTransferRegistry.
+ */
+ int send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
+ MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const;
+};
+
+}
+
+#endif // UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/include/uavcan/uavcan.hpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + * + * This header should be included by the user application. + */ + +#ifndef UAVCAN_UAVCAN_HPP_INCLUDED +#define UAVCAN_UAVCAN_HPP_INCLUDED + +#include <uavcan/build_config.hpp> +#include <uavcan/time.hpp> + +// High-level node logic +#include <uavcan/node/node.hpp> +#include <uavcan/node/timer.hpp> +#include <uavcan/node/publisher.hpp> +#include <uavcan/node/subscriber.hpp> +#include <uavcan/node/service_server.hpp> +#include <uavcan/node/service_client.hpp> +#include <uavcan/node/global_data_type_registry.hpp> + +// Util +#include <uavcan/util/templates.hpp> +#include <uavcan/util/lazy_constructor.hpp> +#include <uavcan/util/method_binder.hpp> + +#endif // UAVCAN_UAVCAN_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/bitset.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_BITSET_HPP_INCLUDED
+#define UAVCAN_UTIL_BITSET_HPP_INCLUDED
+
+#include <cassert>
+#include <cstddef>
+#include <cstring>
+#include <uavcan/build_config.hpp>
+
+namespace uavcan
+{
+/**
+ * STL-like bitset
+ */
+template <std::size_t NumBits>
+class UAVCAN_EXPORT BitSet
+{
+ enum { NumBytes = (NumBits + 7) / 8 };
+
+ static std::size_t getByteNum(std::size_t bit_num) { return bit_num / 8; }
+
+ static std::size_t getBitNum(const std::size_t bit_num) { return bit_num % 8; }
+
+ static void validatePos(std::size_t& inout_pos)
+ {
+ if (inout_pos >= NumBits)
+ {
+ UAVCAN_ASSERT(0);
+ inout_pos = NumBits - 1;
+ }
+ }
+
+ char data_[NumBytes];
+
+public:
+ class Reference
+ {
+ friend class BitSet;
+
+ BitSet* const parent_;
+ const std::size_t bitpos_;
+
+ Reference(BitSet* arg_parent, std::size_t arg_bitpos)
+ : parent_(arg_parent)
+ , bitpos_(arg_bitpos)
+ { }
+
+ public:
+ Reference& operator=(bool x)
+ {
+ parent_->set(bitpos_, x);
+ return *this;
+ }
+
+ Reference& operator=(const Reference& x)
+ {
+ parent_->set(bitpos_, x);
+ return *this;
+ }
+
+ bool operator~() const
+ {
+ return !parent_->test(bitpos_);
+ }
+
+ operator bool() const
+ {
+ return parent_->test(bitpos_);
+ }
+ };
+
+ BitSet()
+ : data_()
+ {
+ reset();
+ }
+
+ BitSet<NumBits>& reset()
+ {
+ std::memset(data_, 0, NumBytes);
+ return *this;
+ }
+
+ BitSet<NumBits>& set()
+ {
+ std::memset(data_, 0xFF, NumBytes);
+ return *this;
+ }
+
+ BitSet<NumBits>& set(std::size_t pos, bool val = true)
+ {
+ validatePos(pos);
+ if (val)
+ {
+ data_[getByteNum(pos)] = char(data_[getByteNum(pos)] | (1 << getBitNum(pos)));
+ }
+ else
+ {
+ data_[getByteNum(pos)] = char(data_[getByteNum(pos)] & ~(1 << getBitNum(pos)));
+ }
+ return *this;
+ }
+
+ bool test(std::size_t pos) const
+ {
+ return (data_[getByteNum(pos)] & (1 << getBitNum(pos))) != 0;
+ }
+
+ bool any() const
+ {
+ for (std::size_t i = 0; i < NumBits; ++i)
+ {
+ if (test(i))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ std::size_t count() const
+ {
+ std::size_t retval = 0;
+ for (std::size_t i = 0; i < NumBits; ++i)
+ {
+ retval += test(i) ? 1U : 0U;
+ }
+ return retval;
+ }
+
+ std::size_t size() const { return NumBits; }
+
+ bool operator[](std::size_t pos) const
+ {
+ return test(pos);
+ }
+
+ Reference operator[](std::size_t pos)
+ {
+ validatePos(pos);
+ return Reference(this, pos);
+ }
+
+ BitSet<NumBits>& operator=(const BitSet<NumBits> & rhs)
+ {
+ if (&rhs == this)
+ {
+ return *this;
+ }
+ for (std::size_t i = 0; i < NumBytes; ++i)
+ {
+ data_[i] = rhs.data_[i];
+ }
+ return *this;
+ }
+
+ bool operator!=(const BitSet<NumBits>& rhs) const { return !operator==(rhs); }
+ bool operator==(const BitSet<NumBits>& rhs) const
+ {
+ for (std::size_t i = 0; i < NumBits; ++i)
+ {
+ if (test(i) != rhs.test(i))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+};
+
+template <> class BitSet<0>; ///< Invalid instantiation
+
+
+template <typename Stream, std::size_t NumBits>
+Stream& operator<<(Stream& s, const BitSet<NumBits>& x)
+{
+ for (std::size_t i = NumBits; i > 0; --i)
+ {
+ s << (x.test(i-1) ? "1" : "0");
+ }
+ return s;
+}
+
+}
+
+#endif // UAVCAN_UTIL_BITSET_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/comparison.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,271 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_COMPARISON_HPP_INCLUDED
+#define UAVCAN_UTIL_COMPARISON_HPP_INCLUDED
+
+#include <uavcan/util/templates.hpp>
+#include <uavcan/build_config.hpp>
+
+namespace uavcan
+{
+/**
+ * Exact comparison of two floats that suppresses the compiler warnings.
+ */
+template <typename T>
+UAVCAN_EXPORT
+inline bool areFloatsExactlyEqual(const T& left, const T& right)
+{
+ return (left <= right) && (left >= right);
+}
+
+/**
+ * This function performs fuzzy comparison of two floating point numbers.
+ * Type of T can be either float, double or long double.
+ * For details refer to http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ * See also: @ref UAVCAN_FLOAT_COMPARISON_EPSILON_MULT.
+ */
+template <typename T>
+UAVCAN_EXPORT
+inline bool areFloatsClose(T a, T b, const T& absolute_epsilon, const T& relative_epsilon)
+{
+ // NAN
+ if (isNaN(a) || isNaN(b))
+ {
+ return false;
+ }
+
+ // Infinity
+ if (isInfinity(a) || isInfinity(b))
+ {
+ return areFloatsExactlyEqual(a, b);
+ }
+
+ // Close numbers near zero
+ const T diff = std::fabs(a - b);
+ if (diff <= absolute_epsilon)
+ {
+ return true;
+ }
+
+ // General case
+ a = std::fabs(a);
+ b = std::fabs(b);
+ const T largest = (b > a) ? b : a;
+ return (diff <= largest * relative_epsilon);
+}
+
+/**
+ * This namespace contains implementation details for areClose().
+ * Don't try this at home.
+ */
+namespace are_close_impl_
+{
+
+struct Applicable { char foo[1]; };
+struct NotApplicable { long long foo[16]; };
+
+template <typename This, typename Rhs>
+struct HasIsCloseMethod
+{
+ template <typename U, typename R, bool (U::*)(const R&) const> struct ConstRef { };
+ template <typename U, typename R, bool (U::*)(R) const> struct ByValue { };
+
+ template <typename U, typename R> static Applicable test(ConstRef<U, R, &U::isClose>*);
+ template <typename U, typename R> static Applicable test(ByValue<U, R, &U::isClose>*);
+
+ template <typename U, typename R> static NotApplicable test(...);
+
+ enum { Result = sizeof(test<This, Rhs>(UAVCAN_NULLPTR)) };
+};
+
+/// First stage: bool L::isClose(R)
+template <typename L, typename R>
+UAVCAN_EXPORT
+inline bool areCloseImplFirst(const L& left, const R& right, IntToType<sizeof(Applicable)>)
+{
+ return left.isClose(right);
+}
+
+/// Second stage: bool R::isClose(L)
+template <typename L, typename R>
+UAVCAN_EXPORT
+inline bool areCloseImplSecond(const L& left, const R& right, IntToType<sizeof(Applicable)>)
+{
+ return right.isClose(left);
+}
+
+/// Second stage: L == R
+template <typename L, typename R>
+UAVCAN_EXPORT
+inline bool areCloseImplSecond(const L& left, const R& right, IntToType<sizeof(NotApplicable)>)
+{
+ return left == right;
+}
+
+/// First stage: select either L == R or bool R::isClose(L)
+template <typename L, typename R>
+UAVCAN_EXPORT
+inline bool areCloseImplFirst(const L& left, const R& right, IntToType<sizeof(NotApplicable)>)
+{
+ return are_close_impl_::areCloseImplSecond(left, right,
+ IntToType<are_close_impl_::HasIsCloseMethod<R, L>::Result>());
+}
+
+} // namespace are_close_impl_
+
+/**
+ * Generic fuzzy comparison function.
+ *
+ * This function properly handles floating point comparison, including mixed floating point type comparison,
+ * e.g. float with long double.
+ *
+ * Two objects of types A and B will be fuzzy comparable if either method is defined:
+ * - bool A::isClose(const B&) const
+ * - bool A::isClose(B) const
+ * - bool B::isClose(const A&) const
+ * - bool B::isClose(A) const
+ *
+ * Call areClose(A, B) will be dispatched as follows:
+ *
+ * - If A and B are both floating point types (float, double, long double) - possibly different - the call will be
+ * dispatched to @ref areFloatsClose(). If A and B are different types, value of the larger type will be coerced
+ * to the smaller type, e.g. areClose(long double, float) --> areClose(float, float).
+ *
+ * - If A defines isClose() that accepts B, the call will be dispatched there.
+ *
+ * - If B defines isClose() that accepts A, the call will be dispatched there (A/B swapped).
+ *
+ * - Last resort is A == B.
+ *
+ * Alternatively, a custom behavior can be implemented via template specialization.
+ *
+ * See also: @ref UAVCAN_FLOAT_COMPARISON_EPSILON_MULT.
+ *
+ * Examples:
+ * areClose(1.0, 1.0F) --> true
+ * areClose(1.0, 1.0F + std::numeric_limits<float>::epsilon()) --> true
+ * areClose(1.0, 1.1) --> false
+ * areClose("123", std::string("123")) --> true (using std::string's operator ==)
+ * areClose(inf, inf) --> true
+ * areClose(inf, -inf) --> false
+ * areClose(nan, nan) --> false (any comparison with nan returns false)
+ * areClose(123, "123") --> compilation error: operator == is not defined
+ */
+template <typename L, typename R>
+UAVCAN_EXPORT
+inline bool areClose(const L& left, const R& right)
+{
+ return are_close_impl_::areCloseImplFirst(left, right,
+ IntToType<are_close_impl_::HasIsCloseMethod<L, R>::Result>());
+}
+
+/*
+ * Float comparison specializations
+ */
+template <>
+UAVCAN_EXPORT
+inline bool areClose<float, float>(const float& left, const float& right)
+{
+ return areFloatsClose(left, right, NumericTraits<float>::epsilon(),
+ NumericTraits<float>::epsilon() * FloatComparisonEpsilonMult);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<double, double>(const double& left, const double& right)
+{
+ return areFloatsClose(left, right, NumericTraits<double>::epsilon(),
+ NumericTraits<double>::epsilon() * FloatComparisonEpsilonMult);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<long double, long double>(const long double& left, const long double& right)
+{
+ return areFloatsClose(left, right, NumericTraits<long double>::epsilon(),
+ NumericTraits<long double>::epsilon() * FloatComparisonEpsilonMult);
+}
+
+/*
+ * Mixed floating type comparison - coercing larger type to smaller type
+ */
+template <>
+UAVCAN_EXPORT
+inline bool areClose<float, double>(const float& left, const double& right)
+{
+ return areClose(left, static_cast<float>(right));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<double, float>(const double& left, const float& right)
+{
+ return areClose(static_cast<float>(left), right);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<float, long double>(const float& left, const long double& right)
+{
+ return areClose(left, static_cast<float>(right));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<long double, float>(const long double& left, const float& right)
+{
+ return areClose(static_cast<float>(left), right);
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<double, long double>(const double& left, const long double& right)
+{
+ return areClose(left, static_cast<double>(right));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool areClose<long double, double>(const long double& left, const double& right)
+{
+ return areClose(static_cast<double>(left), right);
+}
+
+/**
+ * Comparison against zero.
+ * Helps to compare a floating point number against zero if the exact type is unknown.
+ * For non-floating point types performs exact comparison against integer zero.
+ */
+template <typename T>
+UAVCAN_EXPORT
+inline bool isCloseToZero(const T& x)
+{
+ return x == 0;
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool isCloseToZero<float>(const float& x)
+{
+ return areClose(x, static_cast<float>(0.0F));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool isCloseToZero<double>(const double& x)
+{
+ return areClose(x, static_cast<double>(0.0));
+}
+
+template <>
+UAVCAN_EXPORT
+inline bool isCloseToZero<long double>(const long double& x)
+{
+ return areClose(x, static_cast<long double>(0.0L));
+}
+
+}
+
+#endif // UAVCAN_UTIL_COMPARISON_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/lazy_constructor.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED
+#define UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED
+
+#include <cstdlib>
+#include <uavcan/error.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+
+namespace uavcan
+{
+/**
+ * This class allows to postpone the object contruction.
+ * It statically allocates a pool of memory of just enough size to fit the object being constructed.
+ * Later call to construct<>() calls the constructor of the object.
+ * The object will be destroyed automatically when the container class destructor is called.
+ * The memory pool is aligned at the size of the largest primitive type (long double or long long int).
+ */
+template <typename T>
+class UAVCAN_EXPORT LazyConstructor
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ struct
+ {
+ alignas(T) unsigned char pool[sizeof(T)];
+ } data_;
+#else
+ union
+ {
+ unsigned char pool[sizeof(T)];
+ long double _aligner1;
+ long long _aligner2;
+ } data_;
+#endif
+
+ T* ptr_;
+
+ void ensureConstructed() const
+ {
+ if (!ptr_)
+ {
+ handleFatalError("LazyConstructor<T>");
+ }
+ }
+
+ void ensureNotConstructed() const
+ {
+ if (ptr_)
+ {
+ handleFatalError("LazyConstructor<T>");
+ }
+ }
+
+public:
+ LazyConstructor()
+ : ptr_(UAVCAN_NULLPTR)
+ {
+ fill(data_.pool, data_.pool + sizeof(T), uint8_t(0));
+ }
+
+ LazyConstructor(const LazyConstructor<T>& rhs) // Implicit
+ : ptr_(UAVCAN_NULLPTR)
+ {
+ fill(data_.pool, data_.pool + sizeof(T), uint8_t(0));
+ if (rhs)
+ {
+ construct<const T&>(*rhs); // Invoke copy constructor
+ }
+ }
+
+ ~LazyConstructor() { destroy(); }
+
+ LazyConstructor<T>& operator=(const LazyConstructor<T>& rhs)
+ {
+ destroy();
+ if (rhs)
+ {
+ construct<const T&>(*rhs); // Invoke copy constructor
+ }
+ return *this;
+ }
+
+ bool isConstructed() const { return ptr_ != UAVCAN_NULLPTR; }
+
+ operator T*() const { return ptr_; }
+
+ const T* operator->() const { ensureConstructed(); return ptr_; }
+ T* operator->() { ensureConstructed(); return ptr_; }
+
+ const T& operator*() const { ensureConstructed(); return *ptr_; }
+ T& operator*() { ensureConstructed(); return *ptr_; }
+
+ void destroy()
+ {
+ if (ptr_)
+ {
+ ptr_->~T();
+ }
+ ptr_ = UAVCAN_NULLPTR;
+ fill(data_.pool, data_.pool + sizeof(T), uint8_t(0));
+ }
+
+ void construct()
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T();
+ }
+
+// MAX_ARGS = 6
+// TEMPLATE = '''
+// template <%s>
+// void construct(%s)
+// {
+// ensureNotConstructed();
+// ptr_ = new (static_cast<void*>(data_.pool)) T(%s);
+// }'''
+// for nargs in range(1, MAX_ARGS + 1):
+// nums = [(x + 1) for x in range(nargs)]
+// l1 = ['typename P%d' % x for x in nums]
+// l2 = ['typename ParameterType<P%d>::Type p%d' % (x, x) for x in nums]
+// l3 = ['p%d' % x for x in nums]
+// print(TEMPLATE % (', '.join(l1), ', '.join(l2), ', '.join(l3)))
+
+ template <typename P1>
+ void construct(typename ParameterType<P1>::Type p1)
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T(p1);
+ }
+
+ template<typename P1, typename P2>
+ void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2)
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2);
+ }
+
+ template<typename P1, typename P2, typename P3>
+ void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+ typename ParameterType<P3>::Type p3)
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3);
+ }
+
+ template<typename P1, typename P2, typename P3, typename P4>
+ void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+ typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4)
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4);
+ }
+
+ template<typename P1, typename P2, typename P3, typename P4, typename P5>
+ void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+ typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4,
+ typename ParameterType<P5>::Type p5)
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4, p5);
+ }
+
+ template<typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
+ void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+ typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4,
+ typename ParameterType<P5>::Type p5, typename ParameterType<P6>::Type p6)
+ {
+ ensureNotConstructed();
+ ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4, p5, p6);
+ }
+};
+
+}
+
+#endif // UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/linked_list.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,176 @@
+/*
+ * Singly-linked list.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED
+#define UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED
+
+#include <cstdlib>
+#include <cassert>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+
+namespace uavcan
+{
+/**
+ * Classes that are supposed to be linked-listed should derive this.
+ */
+template <typename T>
+class UAVCAN_EXPORT LinkedListNode : Noncopyable
+{
+ T* next_;
+
+protected:
+ LinkedListNode()
+ : next_(UAVCAN_NULLPTR)
+ { }
+
+ ~LinkedListNode() { }
+
+public:
+ T* getNextListNode() const { return next_; }
+
+ void setNextListNode(T* node)
+ {
+ next_ = node;
+ }
+};
+
+/**
+ * Linked list root.
+ */
+template <typename T>
+class UAVCAN_EXPORT LinkedListRoot : Noncopyable
+{
+ T* root_;
+
+public:
+ LinkedListRoot()
+ : root_(UAVCAN_NULLPTR)
+ { }
+
+ T* get() const { return root_; }
+ bool isEmpty() const { return get() == UAVCAN_NULLPTR; }
+
+ /**
+ * Complexity: O(N)
+ */
+ unsigned getLength() const;
+
+ /**
+ * Inserts the node to the beginning of the list.
+ * If the node is already present in the list, it will be relocated to the beginning.
+ * Complexity: O(N)
+ */
+ void insert(T* node);
+
+ /**
+ * Inserts the node immediately before the node X where predicate(X) returns true.
+ * If the node is already present in the list, it can be relocated to a new position.
+ * Complexity: O(2N) (calls remove())
+ */
+ template <typename Predicate>
+ void insertBefore(T* node, Predicate predicate);
+
+ /**
+ * Removes only the first occurence of the node.
+ * Complexity: O(N)
+ */
+ void remove(const T* node);
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * LinkedListRoot<>
+ */
+template <typename T>
+unsigned LinkedListRoot<T>::getLength() const
+{
+ T* node = root_;
+ unsigned cnt = 0;
+ while (node)
+ {
+ cnt++;
+ node = node->getNextListNode();
+ }
+ return cnt;
+}
+
+template <typename T>
+void LinkedListRoot<T>::insert(T* node)
+{
+ if (node == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ return;
+ }
+ remove(node); // Making sure there will be no loops
+ node->setNextListNode(root_);
+ root_ = node;
+}
+
+template <typename T>
+template <typename Predicate>
+void LinkedListRoot<T>::insertBefore(T* node, Predicate predicate)
+{
+ if (node == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ return;
+ }
+
+ remove(node);
+
+ if (root_ == UAVCAN_NULLPTR || predicate(root_))
+ {
+ node->setNextListNode(root_);
+ root_ = node;
+ }
+ else
+ {
+ T* p = root_;
+ while (p->getNextListNode())
+ {
+ if (predicate(p->getNextListNode()))
+ {
+ break;
+ }
+ p = p->getNextListNode();
+ }
+ node->setNextListNode(p->getNextListNode());
+ p->setNextListNode(node);
+ }
+}
+
+template <typename T>
+void LinkedListRoot<T>::remove(const T* node)
+{
+ if (root_ == UAVCAN_NULLPTR || node == UAVCAN_NULLPTR)
+ {
+ return;
+ }
+
+ if (root_ == node)
+ {
+ root_ = root_->getNextListNode();
+ }
+ else
+ {
+ T* p = root_;
+ while (p->getNextListNode())
+ {
+ if (p->getNextListNode() == node)
+ {
+ p->setNextListNode(p->getNextListNode()->getNextListNode());
+ break;
+ }
+ p = p->getNextListNode();
+ }
+ }
+}
+
+}
+
+#endif // UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/map.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,388 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_MAP_HPP_INCLUDED
+#define UAVCAN_UTIL_MAP_HPP_INCLUDED
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/placement_new.hpp>
+
+namespace uavcan
+{
+/**
+ * Slow but memory efficient KV container.
+ *
+ * KV pairs will be allocated in the node's memory pool.
+ *
+ * Please be aware that this container does not perform any speed optimizations to minimize memory footprint,
+ * so the complexity of most operations is O(N).
+ *
+ * Type requirements:
+ * Both key and value must be copyable, assignable and default constructible.
+ * Key must implement a comparison operator.
+ * Key's default constructor must initialize the object into invalid state.
+ * Size of Key + Value + padding must not exceed MemPoolBlockSize.
+ */
+template <typename Key, typename Value>
+class UAVCAN_EXPORT Map : Noncopyable
+{
+public:
+ struct KVPair
+ {
+ Value value; // Key and value are swapped because this may allow to reduce padding (depending on types)
+ Key key;
+
+ KVPair() :
+ value(),
+ key()
+ { }
+
+ KVPair(const Key& arg_key, const Value& arg_value) :
+ value(arg_value),
+ key(arg_key)
+ { }
+
+ bool match(const Key& rhs) const { return rhs == key; }
+ };
+
+private:
+ struct KVGroup : LinkedListNode<KVGroup>
+ {
+ enum { NumKV = (MemPoolBlockSize - sizeof(LinkedListNode<KVGroup>)) / sizeof(KVPair) };
+ KVPair kvs[NumKV];
+
+ KVGroup()
+ {
+ StaticAssert<(static_cast<unsigned>(NumKV) > 0)>::check();
+ IsDynamicallyAllocatable<KVGroup>::check();
+ }
+
+ static KVGroup* instantiate(IPoolAllocator& allocator)
+ {
+ void* const praw = allocator.allocate(sizeof(KVGroup));
+ if (praw == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ return new (praw) KVGroup();
+ }
+
+ static void destroy(KVGroup*& obj, IPoolAllocator& allocator)
+ {
+ if (obj != UAVCAN_NULLPTR)
+ {
+ obj->~KVGroup();
+ allocator.deallocate(obj);
+ obj = UAVCAN_NULLPTR;
+ }
+ }
+
+ KVPair* find(const Key& key)
+ {
+ for (unsigned i = 0; i < static_cast<unsigned>(NumKV); i++)
+ {
+ if (kvs[i].match(key))
+ {
+ return kvs + i;
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+ };
+
+ LinkedListRoot<KVGroup> list_;
+ IPoolAllocator& allocator_;
+
+ KVPair* findKey(const Key& key);
+
+ void compact();
+
+ struct YesPredicate
+ {
+ bool operator()(const Key&, const Value&) const { return true; }
+ };
+
+public:
+ Map(IPoolAllocator& allocator) :
+ allocator_(allocator)
+ {
+ UAVCAN_ASSERT(Key() == Key());
+ }
+
+ ~Map()
+ {
+ clear();
+ }
+
+ /**
+ * Returns null pointer if there's no such entry.
+ */
+ Value* access(const Key& key);
+
+ /**
+ * If entry with the same key already exists, it will be replaced
+ */
+ Value* insert(const Key& key, const Value& value);
+
+ /**
+ * Does nothing if there's no such entry.
+ */
+ void remove(const Key& key);
+
+ /**
+ * Removes entries where the predicate returns true.
+ * Predicate prototype:
+ * bool (Key& key, Value& value)
+ */
+ template <typename Predicate>
+ void removeAllWhere(Predicate predicate);
+
+ /**
+ * Returns first entry where the predicate returns true.
+ * Predicate prototype:
+ * bool (const Key& key, const Value& value)
+ */
+ template <typename Predicate>
+ const Key* find(Predicate predicate) const;
+
+ /**
+ * Removes all items.
+ */
+ void clear();
+
+ /**
+ * Returns a key-value pair located at the specified position from the beginning.
+ * Note that any insertion or deletion may greatly disturb internal ordering, so use with care.
+ * If index is greater than or equal the number of pairs, null pointer will be returned.
+ */
+ KVPair* getByIndex(unsigned index);
+ const KVPair* getByIndex(unsigned index) const;
+
+ /**
+ * Complexity is O(1).
+ */
+ bool isEmpty() const { return find(YesPredicate()) == UAVCAN_NULLPTR; }
+
+ /**
+ * Complexity is O(N).
+ */
+ unsigned getSize() const;
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * Map<>
+ */
+template <typename Key, typename Value>
+typename Map<Key, Value>::KVPair* Map<Key, Value>::findKey(const Key& key)
+{
+ KVGroup* p = list_.get();
+ while (p)
+ {
+ KVPair* const kv = p->find(key);
+ if (kv)
+ {
+ return kv;
+ }
+ p = p->getNextListNode();
+ }
+ return UAVCAN_NULLPTR;
+}
+
+template <typename Key, typename Value>
+void Map<Key, Value>::compact()
+{
+ KVGroup* p = list_.get();
+ while (p)
+ {
+ KVGroup* const next = p->getNextListNode();
+ bool remove_this = true;
+ for (int i = 0; i < KVGroup::NumKV; i++)
+ {
+ if (!p->kvs[i].match(Key()))
+ {
+ remove_this = false;
+ break;
+ }
+ }
+ if (remove_this)
+ {
+ list_.remove(p);
+ KVGroup::destroy(p, allocator_);
+ }
+ p = next;
+ }
+}
+
+template <typename Key, typename Value>
+Value* Map<Key, Value>::access(const Key& key)
+{
+ UAVCAN_ASSERT(!(key == Key()));
+ KVPair* const kv = findKey(key);
+ return kv ? &kv->value : UAVCAN_NULLPTR;
+}
+
+template <typename Key, typename Value>
+Value* Map<Key, Value>::insert(const Key& key, const Value& value)
+{
+ UAVCAN_ASSERT(!(key == Key()));
+ remove(key);
+
+ KVPair* const kv = findKey(Key());
+ if (kv)
+ {
+ *kv = KVPair(key, value);
+ return &kv->value;
+ }
+
+ KVGroup* const kvg = KVGroup::instantiate(allocator_);
+ if (kvg == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ list_.insert(kvg);
+ kvg->kvs[0] = KVPair(key, value);
+ return &kvg->kvs[0].value;
+}
+
+template <typename Key, typename Value>
+void Map<Key, Value>::remove(const Key& key)
+{
+ UAVCAN_ASSERT(!(key == Key()));
+ KVPair* const kv = findKey(key);
+ if (kv)
+ {
+ *kv = KVPair();
+ compact();
+ }
+}
+
+template <typename Key, typename Value>
+template <typename Predicate>
+void Map<Key, Value>::removeAllWhere(Predicate predicate)
+{
+ unsigned num_removed = 0;
+
+ KVGroup* p = list_.get();
+ while (p != UAVCAN_NULLPTR)
+ {
+ KVGroup* const next_group = p->getNextListNode();
+
+ for (int i = 0; i < KVGroup::NumKV; i++)
+ {
+ const KVPair* const kv = p->kvs + i;
+ if (!kv->match(Key()))
+ {
+ if (predicate(kv->key, kv->value))
+ {
+ num_removed++;
+ p->kvs[i] = KVPair();
+ }
+ }
+ }
+
+ p = next_group;
+ }
+
+ if (num_removed > 0)
+ {
+ compact();
+ }
+}
+
+template <typename Key, typename Value>
+template <typename Predicate>
+const Key* Map<Key, Value>::find(Predicate predicate) const
+{
+ KVGroup* p = list_.get();
+ while (p != UAVCAN_NULLPTR)
+ {
+ KVGroup* const next_group = p->getNextListNode();
+
+ for (int i = 0; i < KVGroup::NumKV; i++)
+ {
+ const KVPair* const kv = p->kvs + i;
+ if (!kv->match(Key()))
+ {
+ if (predicate(kv->key, kv->value))
+ {
+ return &p->kvs[i].key;
+ }
+ }
+ }
+
+ p = next_group;
+ }
+ return UAVCAN_NULLPTR;
+}
+
+template <typename Key, typename Value>
+void Map<Key, Value>::clear()
+{
+ removeAllWhere(YesPredicate());
+}
+
+template <typename Key, typename Value>
+typename Map<Key, Value>::KVPair* Map<Key, Value>::getByIndex(unsigned index)
+{
+ // Slowly crawling through the dynamic storage
+ KVGroup* p = list_.get();
+ while (p != UAVCAN_NULLPTR)
+ {
+ KVGroup* const next_group = p->getNextListNode();
+
+ for (int i = 0; i < KVGroup::NumKV; i++)
+ {
+ KVPair* const kv = p->kvs + i;
+ if (!kv->match(Key()))
+ {
+ if (index == 0)
+ {
+ return kv;
+ }
+ index--;
+ }
+ }
+
+ p = next_group;
+ }
+
+ return UAVCAN_NULLPTR;
+}
+
+template <typename Key, typename Value>
+const typename Map<Key, Value>::KVPair* Map<Key, Value>::getByIndex(unsigned index) const
+{
+ return const_cast<Map<Key, Value>*>(this)->getByIndex(index);
+}
+
+template <typename Key, typename Value>
+unsigned Map<Key, Value>::getSize() const
+{
+ unsigned num = 0;
+ KVGroup* p = list_.get();
+ while (p)
+ {
+ for (int i = 0; i < KVGroup::NumKV; i++)
+ {
+ const KVPair* const kv = p->kvs + i;
+ if (!kv->match(Key()))
+ {
+ num++;
+ }
+ }
+ p = p->getNextListNode();
+ }
+ return num;
+}
+
+}
+
+#endif // UAVCAN_UTIL_MAP_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/method_binder.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED
+#define UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED
+
+#include <uavcan/error.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/util/templates.hpp>
+
+namespace uavcan
+{
+/**
+ * Use this to call member functions as callbacks in C++03 mode.
+ *
+ * In C++11 or newer you don't need it because you can use std::function<>/std::bind<> instead.
+ */
+template <typename ObjectPtr, typename MemFunPtr>
+class UAVCAN_EXPORT MethodBinder
+{
+ ObjectPtr obj_;
+ MemFunPtr fun_;
+
+ void validateBeforeCall() const
+ {
+ if (!operator bool())
+ {
+ handleFatalError("Null binder");
+ }
+ }
+
+public:
+ MethodBinder()
+ : obj_()
+ , fun_()
+ { }
+
+ MethodBinder(ObjectPtr o, MemFunPtr f)
+ : obj_(o)
+ , fun_(f)
+ { }
+
+ /**
+ * Returns true if the binder is initialized (doesn't contain null pointers).
+ */
+ operator bool() const
+ {
+ return coerceOrFallback<bool>(obj_, true) && coerceOrFallback<bool>(fun_, true);
+ }
+
+ /**
+ * Will raise a fatal error if either method pointer or object pointer are null.
+ */
+ void operator()()
+ {
+ validateBeforeCall();
+ (obj_->*fun_)();
+ }
+
+ /**
+ * Will raise a fatal error if either method pointer or object pointer are null.
+ */
+ template <typename Par1>
+ void operator()(Par1& p1)
+ {
+ validateBeforeCall();
+ (obj_->*fun_)(p1);
+ }
+
+ /**
+ * Will raise a fatal error if either method pointer or object pointer are null.
+ */
+ template <typename Par1, typename Par2>
+ void operator()(Par1& p1, Par2& p2)
+ {
+ validateBeforeCall();
+ (obj_->*fun_)(p1, p2);
+ }
+};
+
+}
+
+#endif // UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/multiset.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,478 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED
+#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/placement_new.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+namespace uavcan
+{
+/**
+ * Slow but memory efficient unordered multiset. Unlike Map<>, this container does not move objects, so
+ * they don't have to be copyable.
+ *
+ * Items will be allocated in the node's memory pool.
+ */
+template <typename T>
+class UAVCAN_EXPORT Multiset : Noncopyable
+{
+ struct Item : ::uavcan::Noncopyable
+ {
+ T* ptr;
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ alignas(T) unsigned char pool[sizeof(T)]; ///< Memory efficient version
+#else
+ union
+ {
+ unsigned char pool[sizeof(T)];
+ /*
+ * Such alignment does not guarantee safety for all types (only for libuavcan internal ones);
+ * however, increasing it is too memory inefficient. So it is recommended to use C++11, where
+ * this issue is resolved with alignas() (see above).
+ */
+ void* _aligner1_;
+ long long _aligner2_;
+ };
+#endif
+
+ Item()
+ : ptr(UAVCAN_NULLPTR)
+ {
+ fill_n(pool, sizeof(pool), static_cast<unsigned char>(0));
+ }
+
+ ~Item() { destroy(); }
+
+ bool isConstructed() const { return ptr != UAVCAN_NULLPTR; }
+
+ void destroy()
+ {
+ if (ptr != UAVCAN_NULLPTR)
+ {
+ ptr->~T();
+ ptr = UAVCAN_NULLPTR;
+ fill_n(pool, sizeof(pool), static_cast<unsigned char>(0));
+ }
+ }
+ };
+
+private:
+ struct Chunk : LinkedListNode<Chunk>
+ {
+ enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode<Chunk>)) / sizeof(Item) };
+ Item items[NumItems];
+
+ Chunk()
+ {
+ StaticAssert<(static_cast<unsigned>(NumItems) > 0)>::check();
+ IsDynamicallyAllocatable<Chunk>::check();
+ UAVCAN_ASSERT(!items[0].isConstructed());
+ }
+
+ static Chunk* instantiate(IPoolAllocator& allocator)
+ {
+ void* const praw = allocator.allocate(sizeof(Chunk));
+ if (praw == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ return new (praw) Chunk();
+ }
+
+ static void destroy(Chunk*& obj, IPoolAllocator& allocator)
+ {
+ if (obj != UAVCAN_NULLPTR)
+ {
+ obj->~Chunk();
+ allocator.deallocate(obj);
+ obj = UAVCAN_NULLPTR;
+ }
+ }
+
+ Item* findFreeSlot()
+ {
+ for (unsigned i = 0; i < static_cast<unsigned>(NumItems); i++)
+ {
+ if (!items[i].isConstructed())
+ {
+ return items + i;
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+ };
+
+ /*
+ * Data
+ */
+ LinkedListRoot<Chunk> list_;
+ IPoolAllocator& allocator_;
+
+ /*
+ * Methods
+ */
+ Item* findOrCreateFreeSlot();
+
+ void compact();
+
+ enum RemoveStrategy { RemoveOne, RemoveAll };
+
+ template <typename Predicate>
+ void removeWhere(Predicate predicate, RemoveStrategy strategy);
+
+ struct YesPredicate
+ {
+ bool operator()(const T&) const { return true; }
+ };
+
+ struct IndexPredicate : ::uavcan::Noncopyable
+ {
+ unsigned index;
+ IndexPredicate(unsigned target_index)
+ : index(target_index)
+ { }
+
+ bool operator()(const T&)
+ {
+ return index-- == 0;
+ }
+ };
+
+ struct ComparingPredicate
+ {
+ const T& reference;
+
+ ComparingPredicate(const T& ref)
+ : reference(ref)
+ { }
+
+ bool operator()(const T& sample)
+ {
+ return reference == sample;
+ }
+ };
+
+ template<typename Operator>
+ struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable
+ {
+ Operator oper;
+
+ OperatorToFalsePredicateAdapter(Operator o)
+ : oper(o)
+ { }
+
+ bool operator()(T& item)
+ {
+ oper(item);
+ return false;
+ }
+
+ bool operator()(const T& item) const
+ {
+ oper(item);
+ return false;
+ }
+ };
+
+public:
+ Multiset(IPoolAllocator& allocator)
+ : allocator_(allocator)
+ { }
+
+ ~Multiset()
+ {
+ clear();
+ }
+
+ /**
+ * Creates one item in-place and returns a pointer to it.
+ * If creation fails due to lack of memory, UAVCAN_NULLPTR will be returned.
+ * Complexity is O(N).
+ */
+ T* emplace()
+ {
+ Item* const item = findOrCreateFreeSlot();
+ if (item == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR);
+ item->ptr = new (item->pool) T();
+ return item->ptr;
+ }
+
+ template <typename P1>
+ T* emplace(P1 p1)
+ {
+ Item* const item = findOrCreateFreeSlot();
+ if (item == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR);
+ item->ptr = new (item->pool) T(p1);
+ return item->ptr;
+ }
+
+ template <typename P1, typename P2>
+ T* emplace(P1 p1, P2 p2)
+ {
+ Item* const item = findOrCreateFreeSlot();
+ if (item == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR);
+ item->ptr = new (item->pool) T(p1, p2);
+ return item->ptr;
+ }
+
+ template <typename P1, typename P2, typename P3>
+ T* emplace(P1 p1, P2 p2, P3 p3)
+ {
+ Item* const item = findOrCreateFreeSlot();
+ if (item == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR);
+ item->ptr = new (item->pool) T(p1, p2, p3);
+ return item->ptr;
+ }
+
+ /**
+ * Removes entries where the predicate returns true.
+ * Predicate prototype:
+ * bool (T& item)
+ */
+ template <typename Predicate>
+ void removeAllWhere(Predicate predicate) { removeWhere<Predicate>(predicate, RemoveAll); }
+
+ template <typename Predicate>
+ void removeFirstWhere(Predicate predicate) { removeWhere<Predicate>(predicate, RemoveOne); }
+
+ void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); }
+
+ void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); }
+
+ void clear() { removeAllWhere(YesPredicate()); }
+
+ /**
+ * Returns first entry where the predicate returns true.
+ * Predicate prototype:
+ * bool (const T& item)
+ */
+ template <typename Predicate>
+ T* find(Predicate predicate);
+
+ template <typename Predicate>
+ const T* find(Predicate predicate) const
+ {
+ return const_cast<Multiset*>(this)->find<Predicate>(predicate);
+ }
+
+ /**
+ * Calls Operator for each item of the set.
+ * Operator prototype:
+ * void (T& item)
+ * void (const T& item) - const overload
+ */
+ template <typename Operator>
+ void forEach(Operator oper)
+ {
+ OperatorToFalsePredicateAdapter<Operator> adapter(oper);
+ (void)find<OperatorToFalsePredicateAdapter<Operator>&>(adapter);
+ }
+
+ template <typename Operator>
+ void forEach(Operator oper) const
+ {
+ const OperatorToFalsePredicateAdapter<Operator> adapter(oper);
+ (void)find<const OperatorToFalsePredicateAdapter<Operator>&>(adapter);
+ }
+
+ /**
+ * Returns an item located at the specified position from the beginning.
+ * Note that addition and removal operations invalidate indices.
+ * If index is greater than or equal the number of items, null pointer will be returned.
+ * Complexity is O(N).
+ */
+ T* getByIndex(unsigned index)
+ {
+ IndexPredicate predicate(index);
+ return find<IndexPredicate&>(predicate);
+ }
+
+ const T* getByIndex(unsigned index) const
+ {
+ return const_cast<Multiset*>(this)->getByIndex(index);
+ }
+
+ /**
+ * Complexity is O(1).
+ */
+ bool isEmpty() const { return find(YesPredicate()) == UAVCAN_NULLPTR; }
+
+ /**
+ * Counts number of items stored.
+ * Best case complexity is O(N).
+ */
+ unsigned getSize() const;
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * Multiset<>
+ */
+template <typename T>
+typename Multiset<T>::Item* Multiset<T>::findOrCreateFreeSlot()
+{
+ // Search
+ {
+ Chunk* p = list_.get();
+ while (p)
+ {
+ Item* const dyn = p->findFreeSlot();
+ if (dyn != UAVCAN_NULLPTR)
+ {
+ return dyn;
+ }
+ p = p->getNextListNode();
+ }
+ }
+
+ // Create new chunk
+ Chunk* const chunk = Chunk::instantiate(allocator_);
+ if (chunk == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ list_.insert(chunk);
+ return &chunk->items[0];
+}
+
+template <typename T>
+void Multiset<T>::compact()
+{
+ Chunk* p = list_.get();
+ while (p)
+ {
+ Chunk* const next = p->getNextListNode();
+ bool remove_this = true;
+ for (int i = 0; i < Chunk::NumItems; i++)
+ {
+ if (p->items[i].isConstructed())
+ {
+ remove_this = false;
+ break;
+ }
+ }
+ if (remove_this)
+ {
+ list_.remove(p);
+ Chunk::destroy(p, allocator_);
+ }
+ p = next;
+ }
+}
+
+template <typename T>
+template <typename Predicate>
+void Multiset<T>::removeWhere(Predicate predicate, const RemoveStrategy strategy)
+{
+ unsigned num_removed = 0;
+
+ Chunk* p = list_.get();
+ while (p != UAVCAN_NULLPTR)
+ {
+ Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified
+
+ if ((num_removed > 0) && (strategy == RemoveOne))
+ {
+ break;
+ }
+
+ for (int i = 0; i < Chunk::NumItems; i++)
+ {
+ Item& item = p->items[i];
+ if (item.isConstructed())
+ {
+ if (predicate(*item.ptr))
+ {
+ num_removed++;
+ item.destroy();
+ if (strategy == RemoveOne)
+ {
+ break;
+ }
+ }
+ }
+ }
+
+ p = next_chunk;
+ }
+
+ if (num_removed > 0)
+ {
+ compact();
+ }
+}
+
+template <typename T>
+template <typename Predicate>
+T* Multiset<T>::find(Predicate predicate)
+{
+ Chunk* p = list_.get();
+ while (p != UAVCAN_NULLPTR)
+ {
+ Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified
+
+ for (int i = 0; i < Chunk::NumItems; i++)
+ {
+ if (p->items[i].isConstructed())
+ {
+ if (predicate(*p->items[i].ptr))
+ {
+ return p->items[i].ptr;
+ }
+ }
+ }
+
+ p = next_chunk;
+ }
+ return UAVCAN_NULLPTR;
+}
+
+template <typename T>
+unsigned Multiset<T>::getSize() const
+{
+ unsigned num = 0;
+ Chunk* p = list_.get();
+ while (p)
+ {
+ for (int i = 0; i < Chunk::NumItems; i++)
+ {
+ num += p->items[i].isConstructed() ? 1U : 0U;
+ }
+ p = p->getNextListNode();
+ }
+ return num;
+}
+
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/placement_new.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED
+#define UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED
+
+#include <cstddef>
+#include <uavcan/build_config.hpp>
+
+/*
+ * Some embedded C++ implementations don't implement the placement new operator.
+ * Define UAVCAN_IMPLEMENT_PLACEMENT_NEW to apply this workaround.
+ */
+
+#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW
+# error UAVCAN_IMPLEMENT_PLACEMENT_NEW
+#endif
+
+#if UAVCAN_IMPLEMENT_PLACEMENT_NEW
+
+inline void* operator new (std::size_t, void* ptr) throw()
+{
+ return ptr;
+}
+inline void* operator new[](std::size_t, void* ptr) throw()
+{
+ return ptr;
+}
+
+inline void operator delete (void*, void*) throw() { }
+inline void operator delete[](void*, void*) throw() { }
+
+#else
+# include <new>
+#endif
+
+#endif // UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/include/uavcan/util/templates.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,557 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED
+#define UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED
+
+#include <climits>
+#include <cstddef>
+#include <cmath>
+#include <uavcan/build_config.hpp>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+# include <float.h> // cfloat may not be available
+#else
+# include <cfloat> // C++11 mode assumes that all standard headers are available
+#endif
+
+namespace uavcan
+{
+/**
+ * Usage:
+ * StaticAssert<expression>::check();
+ */
+template <bool Value>
+struct UAVCAN_EXPORT StaticAssert;
+
+template <>
+struct UAVCAN_EXPORT StaticAssert<true>
+{
+ static void check() { }
+};
+
+/**
+ * Usage:
+ * ShowIntegerAsError<integer_expression>::foobar();
+ */
+template <long N> struct ShowIntegerAsError;
+
+/**
+ * Prevents copying when inherited
+ */
+class UAVCAN_EXPORT Noncopyable
+{
+ Noncopyable(const Noncopyable&);
+ Noncopyable& operator=(const Noncopyable&);
+protected:
+ Noncopyable() { }
+ ~Noncopyable() { }
+};
+
+/**
+ * Compile time conditions
+ */
+template <bool B, typename T = void>
+struct UAVCAN_EXPORT EnableIf { };
+
+template <typename T>
+struct UAVCAN_EXPORT EnableIf<true, T>
+{
+ typedef T Type;
+};
+
+/**
+ * Lightweight type categorization.
+ */
+template <typename T, typename R = void>
+struct UAVCAN_EXPORT EnableIfType
+{
+ typedef R Type;
+};
+
+/**
+ * Compile-time type selection (Alexandrescu)
+ */
+template <bool Condition, typename TrueType, typename FalseType>
+struct UAVCAN_EXPORT Select;
+
+template <typename TrueType, typename FalseType>
+struct UAVCAN_EXPORT Select<true, TrueType, FalseType>
+{
+ typedef TrueType Result;
+};
+
+template <typename TrueType, typename FalseType>
+struct UAVCAN_EXPORT Select<false, TrueType, FalseType>
+{
+ typedef FalseType Result;
+};
+
+/**
+ * Checks if two identifiers refer to the same type.
+ */
+template<class T, class U>
+struct IsSameType
+{
+ enum { Result = 0 };
+};
+
+template <typename T>
+struct IsSameType<T, T>
+{
+ enum { Result = 1 };
+};
+
+/**
+ * Remove reference as in <type_traits>
+ */
+template <typename T> struct RemoveReference { typedef T Type; };
+template <typename T> struct RemoveReference<T&> { typedef T Type; };
+#if UAVCAN_CPP_VERSION > UAVCAN_CPP03
+template <typename T> struct RemoveReference<T&&> { typedef T Type; };
+#endif
+
+/**
+ * Parameter types
+ */
+template <typename U> struct ParameterType { typedef const U& Type; };
+template <typename U> struct ParameterType<U&> { typedef U& Type; };
+#if UAVCAN_CPP_VERSION > UAVCAN_CPP03
+template <typename U> struct ParameterType<U&&> { typedef U&& Type; };
+#endif
+
+/**
+ * Value types
+ */
+template <bool> struct UAVCAN_EXPORT BooleanType { };
+typedef BooleanType<true> TrueType;
+typedef BooleanType<false> FalseType;
+
+template <int N> struct IntToType { };
+
+/**
+ * Relations
+ */
+template <typename T1, typename T2>
+class UAVCAN_EXPORT IsImplicitlyConvertibleFromTo
+{
+ template <typename U> static U returner();
+
+ struct True_ { char x[2]; };
+ struct False_ { };
+
+ static True_ test(const T2 &);
+ static False_ test(...);
+
+public:
+ enum { Result = sizeof(True_) == sizeof(IsImplicitlyConvertibleFromTo<T1, T2>::test(returner<T1>())) };
+};
+
+/**
+ * coerceOrFallback<To>(From)
+ * coerceOrFallback<To>(From, To)
+ * @{
+ */
+template <typename From, typename To>
+struct UAVCAN_EXPORT CoerceOrFallbackImpl
+{
+ static To impl(const From& from, const To&, TrueType) { return To(from); }
+ static To impl(const From&, const To& default_, FalseType) { return default_; }
+};
+
+/**
+ * If possible, performs an implicit cast from the type From to the type To.
+ * If the cast is not possible, returns default_ of type To.
+ */
+template <typename To, typename From>
+UAVCAN_EXPORT
+To coerceOrFallback(const From& from, const To& default_)
+{
+ return CoerceOrFallbackImpl<From, To>::impl(from, default_,
+ BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
+}
+
+/**
+ * If possible, performs an implicit cast from the type From to the type To.
+ * If the cast is not possible, returns a default constructed object of the type To.
+ */
+template <typename To, typename From>
+UAVCAN_EXPORT
+To coerceOrFallback(const From& from)
+{
+ return CoerceOrFallbackImpl<From, To>::impl(from, To(),
+ BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
+}
+/**
+ * @}
+ */
+
+/**
+ * Selects smaller value
+ */
+template <long A, long B>
+struct EnumMin
+{
+ enum { Result = (A < B) ? A : B };
+};
+
+/**
+ * Selects larger value
+ */
+template <long A, long B>
+struct EnumMax
+{
+ enum { Result = (A > B) ? A : B };
+};
+
+/**
+ * Compile time square root for integers.
+ * Useful for operations on square matrices.
+ */
+template <unsigned Value> struct UAVCAN_EXPORT CompileTimeIntSqrt;
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<1> { enum { Result = 1 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<4> { enum { Result = 2 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<9> { enum { Result = 3 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<16> { enum { Result = 4 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<25> { enum { Result = 5 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<36> { enum { Result = 6 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<49> { enum { Result = 7 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<64> { enum { Result = 8 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<81> { enum { Result = 9 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<100> { enum { Result = 10 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<121> { enum { Result = 11 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<144> { enum { Result = 12 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<169> { enum { Result = 13 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<196> { enum { Result = 14 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<225> { enum { Result = 15 }; };
+template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<256> { enum { Result = 16 }; };
+
+/**
+ * Replacement for std::copy(..)
+ */
+template <typename InputIt, typename OutputIt>
+UAVCAN_EXPORT
+OutputIt copy(InputIt first, InputIt last, OutputIt result)
+{
+ while (first != last)
+ {
+ *result = *first;
+ ++first;
+ ++result;
+ }
+ return result;
+}
+
+/**
+ * Replacement for std::fill(..)
+ */
+template <typename ForwardIt, typename T>
+UAVCAN_EXPORT
+void fill(ForwardIt first, ForwardIt last, const T& value)
+{
+ while (first != last)
+ {
+ *first = value;
+ ++first;
+ }
+}
+
+/**
+ * Replacement for std::fill_n(..)
+ */
+template<typename OutputIt, typename T>
+UAVCAN_EXPORT
+void fill_n(OutputIt first, std::size_t n, const T& value)
+{
+ while (n--)
+ {
+ *first++ = value;
+ }
+}
+
+/**
+ * Replacement for std::min(..)
+ */
+template <typename T>
+UAVCAN_EXPORT
+const T& min(const T& a, const T& b)
+{
+ return (b < a) ? b : a;
+}
+
+/**
+ * Replacement for std::max(..)
+ */
+template <typename T>
+UAVCAN_EXPORT
+const T& max(const T& a, const T& b)
+{
+ return (a < b) ? b : a;
+}
+
+/**
+ * Replacement for std::lexicographical_compare(..)
+ */
+template<typename InputIt1, typename InputIt2>
+UAVCAN_EXPORT
+bool lexicographical_compare(InputIt1 first1, InputIt1 last1, InputIt2 first2, InputIt2 last2)
+{
+ while ((first1 != last1) && (first2 != last2))
+ {
+ if (*first1 < *first2)
+ {
+ return true;
+ }
+ if (*first2 < *first1)
+ {
+ return false;
+ }
+ ++first1;
+ ++first2;
+ }
+ return (first1 == last1) && (first2 != last2);
+}
+
+/**
+ * Replacement for std::equal(..)
+ */
+template<typename InputIt1, typename InputIt2>
+UAVCAN_EXPORT
+bool equal(InputIt1 first1, InputIt1 last1, InputIt2 first2)
+{
+ while (first1 != last1)
+ {
+ if (*first1 != *first2)
+ {
+ return false;
+ }
+ ++first1;
+ ++first2;
+ }
+ return true;
+}
+
+/**
+ * Numeric traits, like std::numeric_limits<>
+ */
+template <typename T>
+struct UAVCAN_EXPORT NumericTraits;
+
+/// bool
+template <>
+struct UAVCAN_EXPORT NumericTraits<bool>
+{
+ enum { IsSigned = 0 };
+ enum { IsInteger = 1 };
+ static bool max() { return true; }
+ static bool min() { return false; }
+};
+
+/// char
+template <>
+struct UAVCAN_EXPORT NumericTraits<char>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 1 };
+ static char max() { return CHAR_MAX; }
+ static char min() { return CHAR_MIN; }
+};
+template <>
+struct UAVCAN_EXPORT NumericTraits<signed char>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 1 };
+ static signed char max() { return SCHAR_MAX; }
+ static signed char min() { return SCHAR_MIN; }
+};
+template <>
+struct UAVCAN_EXPORT NumericTraits<unsigned char>
+{
+ enum { IsSigned = 0 };
+ enum { IsInteger = 1 };
+ static unsigned char max() { return UCHAR_MAX; }
+ static unsigned char min() { return 0; }
+};
+
+/// short
+template <>
+struct UAVCAN_EXPORT NumericTraits<short>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 1 };
+ static short max() { return SHRT_MAX; }
+ static short min() { return SHRT_MIN; }
+};
+template <>
+struct UAVCAN_EXPORT NumericTraits<unsigned short>
+{
+ enum { IsSigned = 0 };
+ enum { IsInteger = 1 };
+ static unsigned short max() { return USHRT_MAX; }
+ static unsigned short min() { return 0; }
+};
+
+/// int
+template <>
+struct UAVCAN_EXPORT NumericTraits<int>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 1 };
+ static int max() { return INT_MAX; }
+ static int min() { return INT_MIN; }
+};
+template <>
+struct UAVCAN_EXPORT NumericTraits<unsigned int>
+{
+ enum { IsSigned = 0 };
+ enum { IsInteger = 1 };
+ static unsigned int max() { return UINT_MAX; }
+ static unsigned int min() { return 0; }
+};
+
+/// long
+template <>
+struct UAVCAN_EXPORT NumericTraits<long>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 1 };
+ static long max() { return LONG_MAX; }
+ static long min() { return LONG_MIN; }
+};
+template <>
+struct UAVCAN_EXPORT NumericTraits<unsigned long>
+{
+ enum { IsSigned = 0 };
+ enum { IsInteger = 1 };
+ static unsigned long max() { return ULONG_MAX; }
+ static unsigned long min() { return 0; }
+};
+
+/// long long
+template <>
+struct UAVCAN_EXPORT NumericTraits<long long>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 1 };
+ static long long max() { return LLONG_MAX; }
+ static long long min() { return LLONG_MIN; }
+};
+template <>
+struct UAVCAN_EXPORT NumericTraits<unsigned long long>
+{
+ enum { IsSigned = 0 };
+ enum { IsInteger = 1 };
+ static unsigned long long max() { return ULLONG_MAX; }
+ static unsigned long long min() { return 0; }
+};
+
+/// float
+template <>
+struct UAVCAN_EXPORT NumericTraits<float>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 0 };
+ static float max() { return FLT_MAX; }
+ static float min() { return FLT_MIN; }
+ static float infinity() { return INFINITY; }
+ static float epsilon() { return FLT_EPSILON; }
+};
+
+/// double
+template <>
+struct UAVCAN_EXPORT NumericTraits<double>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 0 };
+ static double max() { return DBL_MAX; }
+ static double min() { return DBL_MIN; }
+ static double infinity() { return static_cast<double>(INFINITY) * static_cast<double>(INFINITY); }
+ static double epsilon() { return DBL_EPSILON; }
+};
+
+#if defined(LDBL_MAX) && defined(LDBL_MIN) && defined(LDBL_EPSILON)
+/// long double
+template <>
+struct UAVCAN_EXPORT NumericTraits<long double>
+{
+ enum { IsSigned = 1 };
+ enum { IsInteger = 0 };
+ static long double max() { return LDBL_MAX; }
+ static long double min() { return LDBL_MIN; }
+ static long double infinity() { return static_cast<long double>(INFINITY) * static_cast<long double>(INFINITY); }
+ static long double epsilon() { return LDBL_EPSILON; }
+};
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+# undef isnan
+# undef isinf
+# undef signbit
+#endif
+
+/**
+ * Replacement for std::isnan().
+ * Note that direct float comparison (==, !=) is intentionally avoided.
+ */
+template <typename T>
+inline bool isNaN(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ return std::isnan(arg);
+#else
+ // coverity[same_on_both_sides : FALSE]
+ // cppcheck-suppress duplicateExpression
+ return !(arg <= arg);
+#endif
+}
+
+/**
+ * Replacement for std::isinf().
+ * Note that direct float comparison (==, !=) is intentionally avoided.
+ */
+template <typename T>
+inline bool isInfinity(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ return std::isinf(arg);
+#else
+ return (arg >= NumericTraits<T>::infinity()) || (arg <= -NumericTraits<T>::infinity());
+#endif
+}
+
+/**
+ * Replacement for std::isfinite().
+ * Note that direct float comparison (==, !=) is intentionally avoided.
+ */
+template <typename T>
+inline bool isFinite(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ return std::isfinite(arg);
+#else
+ return !isNaN(arg) && !isInfinity(arg);
+#endif
+}
+
+/**
+ * Replacement for std::signbit().
+ * Note that direct float comparison (==, !=) is intentionally avoided.
+ */
+template <typename T>
+inline bool getSignBit(T arg)
+{
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+ return std::signbit(arg);
+#else
+ // coverity[divide_by_zero : FALSE]
+ return arg < T(0) || (((arg <= T(0)) && (arg >= T(0))) && (T(1) / arg < T(0)));
+#endif
+}
+
+}
+
+#endif // UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/driver/uc_can.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,118 @@
+/*
+ * CAN bus driver interface.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/driver/can.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+
+const uint32_t CanFrame::MaskStdID;
+const uint32_t CanFrame::MaskExtID;
+const uint32_t CanFrame::FlagEFF;
+const uint32_t CanFrame::FlagRTR;
+const uint32_t CanFrame::FlagERR;
+const uint8_t CanFrame::MaxDataLen;
+
+bool CanFrame::priorityHigherThan(const CanFrame& rhs) const
+{
+ const uint32_t clean_id = id & MaskExtID;
+ const uint32_t rhs_clean_id = rhs.id & MaskExtID;
+
+ /*
+ * STD vs EXT - if 11 most significant bits are the same, EXT loses.
+ */
+ const bool ext = id & FlagEFF;
+ const bool rhs_ext = rhs.id & FlagEFF;
+ if (ext != rhs_ext)
+ {
+ const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id;
+ const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
+ if (arb11 != rhs_arb11)
+ {
+ return arb11 < rhs_arb11;
+ }
+ else
+ {
+ return rhs_ext;
+ }
+ }
+
+ /*
+ * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
+ */
+ const bool rtr = id & FlagRTR;
+ const bool rhs_rtr = rhs.id & FlagRTR;
+ if (clean_id == rhs_clean_id && rtr != rhs_rtr)
+ {
+ return rhs_rtr;
+ }
+
+ /*
+ * Plain ID arbitration - greater value loses.
+ */
+ return clean_id < rhs_clean_id;
+}
+
+#if UAVCAN_TOSTRING
+std::string CanFrame::toString(StringRepresentation mode) const
+{
+ UAVCAN_ASSERT(mode == StrTight || mode == StrAligned);
+
+ static const unsigned AsciiColumnOffset = 36U;
+
+ char buf[50];
+ char* wpos = buf;
+ char* const epos = buf + sizeof(buf);
+ fill(buf, buf + sizeof(buf), '\0');
+
+ if (id & FlagEFF)
+ {
+ wpos += snprintf(wpos, unsigned(epos - wpos), "0x%08x ", unsigned(id & MaskExtID));
+ }
+ else
+ {
+ const char* const fmt = (mode == StrAligned) ? " 0x%03x " : "0x%03x ";
+ wpos += snprintf(wpos, unsigned(epos - wpos), fmt, unsigned(id & MaskStdID));
+ }
+
+ if (id & FlagRTR)
+ {
+ wpos += snprintf(wpos, unsigned(epos - wpos), " RTR");
+ }
+ else if (id & FlagERR)
+ {
+ wpos += snprintf(wpos, unsigned(epos - wpos), " ERR");
+ }
+ else
+ {
+ for (int dlen = 0; dlen < dlc; dlen++) // hex bytes
+ {
+ wpos += snprintf(wpos, unsigned(epos - wpos), " %02x", unsigned(data[dlen]));
+ }
+
+ while ((mode == StrAligned) && (wpos < buf + AsciiColumnOffset)) // alignment
+ {
+ *wpos++ = ' ';
+ }
+
+ wpos += snprintf(wpos, unsigned(epos - wpos), " \'"); // ascii
+ for (int dlen = 0; dlen < dlc; dlen++)
+ {
+ uint8_t ch = data[dlen];
+ if (ch < 0x20 || ch > 0x7E)
+ {
+ ch = '.';
+ }
+ wpos += snprintf(wpos, unsigned(epos - wpos), "%c", ch);
+ }
+ wpos += snprintf(wpos, unsigned(epos - wpos), "\'");
+ }
+ (void)wpos;
+ return std::string(buf);
+}
+#endif
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/marshal/uc_bit_array_copy.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/marshal/bit_stream.hpp>
+#include <climits>
+#include <cstring>
+#include <cstddef>
+
+namespace uavcan
+{
+void bitarrayCopy(const unsigned char* src, std::size_t src_offset, std::size_t src_len,
+ unsigned char* dst, std::size_t dst_offset)
+{
+ /*
+ * Should never be called on a zero-length buffer. The caller will also ensure that the bit
+ * offsets never exceed one byte.
+ */
+
+ UAVCAN_ASSERT(src_len > 0U);
+ UAVCAN_ASSERT(src_offset < 8U && dst_offset < 8U);
+
+ const std::size_t last_bit = src_offset + src_len;
+ while (last_bit - src_offset)
+ {
+ const uint8_t src_bit_offset = src_offset % 8U;
+ const uint8_t dst_bit_offset = dst_offset % 8U;
+
+ // The number of bits to copy
+ const uint8_t max_offset = uavcan::max(src_bit_offset, dst_bit_offset);
+ const std::size_t copy_bits = uavcan::min(last_bit - src_offset, std::size_t(8U - max_offset));
+
+ /*
+ * The mask indicating which bits of dest to update:
+ * dst_byte_offset copy_bits write_mask
+ * 0 8 11111111
+ * 0 7 11111110
+ * ...
+ * 0 1 10000000
+ * ...
+ * 4 4 00001111
+ * 4 3 00001110
+ * 4 2 00001100
+ * 4 1 00001000
+ * ...
+ * 7 1 00000001
+ */
+ const uint8_t write_mask = uint8_t(uint8_t(0xFF00U >> copy_bits) >> dst_bit_offset);
+
+ // The value to be extracted from src, shifted into the dst location
+ const uint8_t src_data = uint8_t((src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset);
+
+ dst[dst_offset / 8U] = uint8_t((dst[dst_offset / 8U] & ~write_mask) | (src_data & write_mask));
+
+ src_offset += copy_bits;
+ dst_offset += copy_bits;
+ }
+}
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/marshal/uc_bit_stream.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/marshal/bit_stream.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+
+const unsigned BitStream::MaxBytesPerRW;
+const unsigned BitStream::MaxBitsPerRW;
+
+int BitStream::write(const uint8_t* bytes, const unsigned bitlen)
+{
+ // Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_)
+ uint8_t tmp[MaxBytesPerRW + 1];
+
+ // Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write()
+ const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
+ UAVCAN_ASSERT(MaxBytesPerRW >= bytelen);
+ tmp[0] = tmp[bytelen - 1] = 0;
+
+ fill(tmp, tmp + bytelen, uint8_t(0));
+ copyBitArrayAlignedToUnaligned(bytes, bitlen, tmp, bit_offset_ % 8);
+
+ const unsigned new_bit_offset = bit_offset_ + bitlen;
+
+ // Bitcopy algorithm resets skipped bits in the first byte. Restore them back.
+ tmp[0] |= byte_cache_;
+
+ // (new_bit_offset % 8 == 0) means that this write was perfectly aligned.
+ byte_cache_ = uint8_t((new_bit_offset % 8) ? tmp[bytelen - 1] : 0);
+
+ /*
+ * Dump the data into the destination buffer.
+ * Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value
+ * within the next write() operation.
+ */
+ const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
+ if (write_res < 0)
+ {
+ return write_res;
+ }
+ if (static_cast<unsigned>(write_res) < bytelen)
+ {
+ return ResultOutOfBuffer;
+ }
+
+ bit_offset_ = new_bit_offset;
+ return ResultOk;
+}
+
+int BitStream::read(uint8_t* bytes, const unsigned bitlen)
+{
+ uint8_t tmp[MaxBytesPerRW + 1];
+
+ const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
+ UAVCAN_ASSERT(MaxBytesPerRW >= bytelen);
+
+ const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen);
+ if (read_res < 0)
+ {
+ return read_res;
+ }
+ if (static_cast<unsigned>(read_res) < bytelen)
+ {
+ return ResultOutOfBuffer;
+ }
+
+ fill(bytes, bytes + bitlenToBytelen(bitlen), uint8_t(0));
+ copyBitArrayUnalignedToAligned(tmp, bit_offset_ % 8, bitlen, bytes);
+ bit_offset_ += bitlen;
+ return ResultOk;
+}
+
+#if UAVCAN_TOSTRING
+std::string BitStream::toString() const
+{
+ std::string out;
+ out.reserve(128);
+
+ for (unsigned offset = 0; true; offset++)
+ {
+ uint8_t byte = 0;
+ if (1 != buf_.read(offset, &byte, 1U))
+ {
+ break;
+ }
+ for (int i = 7; i >= 0; i--) // Most significant goes first
+ {
+ out += (byte & (1 << i)) ? '1' : '0';
+ }
+ out += ' ';
+ }
+ if (out.length() > 0)
+ {
+ (void)out.erase(out.length() - 1, 1);
+ }
+ return out;
+}
+#endif
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/marshal/uc_float_spec.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/marshal/float_spec.hpp>
+#include <uavcan/build_config.hpp>
+#include <cmath>
+
+namespace uavcan
+{
+
+#if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
+
+union Fp32
+{
+ uint32_t u;
+ float f;
+};
+
+/*
+ * IEEE754Converter
+ */
+uint16_t IEEE754Converter::nativeIeeeToHalf(float value)
+{
+ /*
+ * https://gist.github.com/rygorous/2156668
+ * Public domain, by Fabian "ryg" Giesen
+ */
+ const Fp32 f32infty = { 255U << 23 };
+ const Fp32 f16infty = { 31U << 23 };
+ const Fp32 magic = { 15U << 23 };
+ const uint32_t sign_mask = 0x80000000U;
+ const uint32_t round_mask = ~0xFFFU;
+
+ Fp32 in;
+ uint16_t out;
+
+ in.f = value;
+
+ uint32_t sign = in.u & sign_mask;
+ in.u ^= sign;
+
+ if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */
+ {
+ /* NaN->sNaN and Inf->Inf */
+ out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U;
+ }
+ else /* (De)normalized number or zero */
+ {
+ in.u &= round_mask;
+ in.f *= magic.f;
+ in.u -= round_mask;
+ if (in.u > f16infty.u)
+ {
+ in.u = f16infty.u; /* Clamp to signed infinity if overflowed */
+ }
+
+ out = uint16_t(in.u >> 13); /* Take the bits! */
+ }
+
+ out = uint16_t(out | (sign >> 16));
+
+ return out;
+}
+
+float IEEE754Converter::halfToNativeIeee(uint16_t value)
+{
+ /*
+ * https://gist.github.com/rygorous/2144712
+ * Public domain, by Fabian "ryg" Giesen
+ */
+ const Fp32 magic = { (254U - 15U) << 23 };
+ const Fp32 was_infnan = { (127U + 16U) << 23 };
+ Fp32 out;
+
+ out.u = (value & 0x7FFFU) << 13; /* exponent/mantissa bits */
+ out.f *= magic.f; /* exponent adjust */
+ if (out.f >= was_infnan.f) /* make sure Inf/NaN survive */
+ {
+ out.u |= 255U << 23;
+ }
+ out.u |= (value & 0x8000U) << 16; /* sign bit */
+
+ return out.f;
+}
+
+#endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/marshal/uc_scalar_codec.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/marshal/scalar_codec.hpp>
+
+namespace uavcan
+{
+
+void ScalarCodec::swapByteOrder(uint8_t* const bytes, const unsigned len)
+{
+ UAVCAN_ASSERT(bytes);
+ for (unsigned i = 0, j = len - 1; i < j; i++, j--)
+ {
+ const uint8_t c = bytes[i];
+ bytes[i] = bytes[j];
+ bytes[j] = c;
+ }
+}
+
+int ScalarCodec::encodeBytesImpl(uint8_t* const bytes, const unsigned bitlen)
+{
+ UAVCAN_ASSERT(bytes);
+ // Underlying stream class assumes that more significant bits have lower index, so we need to shift some.
+ if (bitlen % 8)
+ {
+ bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] << ((8 - (bitlen % 8)) & 7));
+ }
+ return stream_.write(bytes, bitlen);
+}
+
+int ScalarCodec::decodeBytesImpl(uint8_t* const bytes, const unsigned bitlen)
+{
+ UAVCAN_ASSERT(bytes);
+ const int read_res = stream_.read(bytes, bitlen);
+ if (read_res > 0)
+ {
+ if (bitlen % 8)
+ {
+ bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] >> ((8 - (bitlen % 8)) & 7)); // As in encode(), vice versa
+ }
+ }
+ return read_res;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/node/uc_generic_publisher.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/node/generic_publisher.hpp>
+
+namespace uavcan
+{
+
+bool GenericPublisherBase::isInited() const
+{
+ return sender_.isInitialized();
+}
+
+int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos)
+{
+ if (isInited())
+ {
+ return 0;
+ }
+
+ GlobalDataTypeRegistry::instance().freeze();
+
+ const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname);
+ if (descr == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname);
+ return -ErrUnknownDataType;
+ }
+
+ sender_.init(*descr, qos);
+
+ return 0;
+}
+
+MonotonicTime GenericPublisherBase::getTxDeadline() const
+{
+ return node_.getMonotonicTime() + tx_timeout_;
+}
+
+int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type,
+ NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline)
+{
+ if (tid)
+ {
+ return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(),
+ blocking_deadline, transfer_type, dst_node_id, *tid);
+ }
+ else
+ {
+ return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(),
+ blocking_deadline, transfer_type, dst_node_id);
+ }
+}
+
+void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout)
+{
+ tx_timeout = max(tx_timeout, getMinTxTimeout());
+ tx_timeout = min(tx_timeout, getMaxTxTimeout());
+ tx_timeout_ = tx_timeout;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/node/uc_generic_subscriber.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/node/generic_subscriber.hpp>
+
+namespace uavcan
+{
+
+int GenericSubscriberBase::genericStart(TransferListener* listener,
+ bool (Dispatcher::*registration_method)(TransferListener*))
+{
+ if (listener == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrLogic;
+ }
+ stop(listener);
+ if (!(node_.getDispatcher().*registration_method)(listener))
+ {
+ UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener");
+ return -ErrInvalidTransferListener;
+ }
+ return 0;
+}
+
+void GenericSubscriberBase::stop(TransferListener* listener)
+{
+ if (listener != UAVCAN_NULLPTR)
+ {
+ node_.getDispatcher().unregisterMessageListener(listener);
+ node_.getDispatcher().unregisterServiceRequestListener(listener);
+ node_.getDispatcher().unregisterServiceResponseListener(listener);
+ }
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/node/uc_global_data_type_registry.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,199 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/node/global_data_type_registry.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+#include <cstdlib>
+
+namespace uavcan
+{
+
+GlobalDataTypeRegistry::List* GlobalDataTypeRegistry::selectList(DataTypeKind kind) const
+{
+ if (kind == DataTypeKindMessage)
+ {
+ return &msgs_;
+ }
+ else if (kind == DataTypeKindService)
+ {
+ return &srvs_;
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ return UAVCAN_NULLPTR;
+ }
+}
+
+GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::remove(Entry* dtd)
+{
+ if (!dtd)
+ {
+ UAVCAN_ASSERT(0);
+ return RegistrationResultInvalidParams;
+ }
+ if (isFrozen())
+ {
+ return RegistrationResultFrozen;
+ }
+
+ List* list = selectList(dtd->descriptor.getKind());
+ if (!list)
+ {
+ return RegistrationResultInvalidParams;
+ }
+
+ list->remove(dtd); // If this call came from regist<>(), that would be enough
+ Entry* p = list->get(); // But anyway
+ while (p)
+ {
+ Entry* const next = p->getNextListNode();
+ if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName()))
+ {
+ list->remove(p);
+ }
+ p = next;
+ }
+ return RegistrationResultOk;
+}
+
+GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::registImpl(Entry* dtd)
+{
+ if (!dtd || !dtd->descriptor.isValid())
+ {
+ UAVCAN_ASSERT(0);
+ return RegistrationResultInvalidParams;
+ }
+ if (isFrozen())
+ {
+ return RegistrationResultFrozen;
+ }
+
+ List* list = selectList(dtd->descriptor.getKind());
+ if (!list)
+ {
+ return RegistrationResultInvalidParams;
+ }
+
+ { // Collision check
+ Entry* p = list->get();
+ while (p)
+ {
+ if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision
+ {
+ return RegistrationResultCollision;
+ }
+ if (!std::strncmp(p->descriptor.getFullName(), dtd->descriptor.getFullName(),
+ DataTypeDescriptor::MaxFullNameLen)) // Name collision
+ {
+ return RegistrationResultCollision;
+ }
+ p = p->getNextListNode();
+ }
+ }
+#if UAVCAN_DEBUG
+ const unsigned len_before = list->getLength();
+#endif
+ list->insertBefore(dtd, EntryInsertionComparator(dtd));
+
+#if UAVCAN_DEBUG
+ { // List integrity check
+ const unsigned len_after = list->getLength();
+ if ((len_before + 1) != len_after)
+ {
+ UAVCAN_ASSERT(0);
+ std::abort();
+ }
+ }
+ { // Order check
+ Entry* p = list->get();
+ int id = -1;
+ while (p)
+ {
+ if (id >= p->descriptor.getID().get())
+ {
+ UAVCAN_ASSERT(0);
+ std::abort();
+ }
+ id = p->descriptor.getID().get();
+ p = p->getNextListNode();
+ }
+ }
+#endif
+ return RegistrationResultOk;
+}
+
+GlobalDataTypeRegistry& GlobalDataTypeRegistry::instance()
+{
+ static GlobalDataTypeRegistry singleton;
+ return singleton;
+}
+
+void GlobalDataTypeRegistry::freeze()
+{
+ if (!frozen_)
+ {
+ frozen_ = true;
+ UAVCAN_TRACE("GlobalDataTypeRegistry", "Frozen; num msgs: %u, num srvs: %u",
+ getNumMessageTypes(), getNumServiceTypes());
+ }
+}
+
+const DataTypeDescriptor* GlobalDataTypeRegistry::find(const char* name) const
+{
+ const DataTypeDescriptor* desc = find(DataTypeKindMessage, name);
+ if (desc == UAVCAN_NULLPTR)
+ {
+ desc = find(DataTypeKindService, name);
+ }
+ return desc;
+}
+
+const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const char* name) const
+{
+ if (!name)
+ {
+ UAVCAN_ASSERT(0);
+ return UAVCAN_NULLPTR;
+ }
+ const List* list = selectList(kind);
+ if (!list)
+ {
+ UAVCAN_ASSERT(0);
+ return UAVCAN_NULLPTR;
+ }
+ Entry* p = list->get();
+ while (p)
+ {
+ if (p->descriptor.match(kind, name))
+ {
+ return &p->descriptor;
+ }
+ p = p->getNextListNode();
+ }
+ return UAVCAN_NULLPTR;
+}
+
+const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, DataTypeID dtid) const
+{
+ const List* list = selectList(kind);
+ if (!list)
+ {
+ UAVCAN_ASSERT(0);
+ return UAVCAN_NULLPTR;
+ }
+ Entry* p = list->get();
+ while (p)
+ {
+ if (p->descriptor.match(kind, dtid))
+ {
+ return &p->descriptor;
+ }
+ p = p->getNextListNode();
+ }
+ return UAVCAN_NULLPTR;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/node/uc_scheduler.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,208 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/node/scheduler.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+/*
+ * MonotonicDeadlineHandler
+ */
+void DeadlineHandler::startWithDeadline(MonotonicTime deadline)
+{
+ UAVCAN_ASSERT(!deadline.isZero());
+ stop();
+ deadline_ = deadline;
+ scheduler_.getDeadlineScheduler().add(this);
+}
+
+void DeadlineHandler::startWithDelay(MonotonicDuration delay)
+{
+ startWithDeadline(scheduler_.getMonotonicTime() + delay);
+}
+
+void DeadlineHandler::stop()
+{
+ scheduler_.getDeadlineScheduler().remove(this);
+}
+
+bool DeadlineHandler::isRunning() const
+{
+ return scheduler_.getDeadlineScheduler().doesExist(this);
+}
+
+/*
+ * MonotonicDeadlineScheduler
+ */
+struct MonotonicDeadlineHandlerInsertionComparator
+{
+ const MonotonicTime ts;
+ explicit MonotonicDeadlineHandlerInsertionComparator(MonotonicTime arg_ts) : ts(arg_ts) { }
+ bool operator()(const DeadlineHandler* t) const
+ {
+ return t->getDeadline() > ts;
+ }
+};
+
+void DeadlineScheduler::add(DeadlineHandler* mdh)
+{
+ UAVCAN_ASSERT(mdh);
+ handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getDeadline()));
+}
+
+void DeadlineScheduler::remove(DeadlineHandler* mdh)
+{
+ UAVCAN_ASSERT(mdh);
+ handlers_.remove(mdh);
+}
+
+bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const
+{
+ UAVCAN_ASSERT(mdh);
+ const DeadlineHandler* p = handlers_.get();
+#if UAVCAN_DEBUG
+ MonotonicTime prev_deadline;
+#endif
+ while (p)
+ {
+#if UAVCAN_DEBUG
+ if (prev_deadline > p->getDeadline()) // Self check
+ {
+ std::abort();
+ }
+ prev_deadline = p->getDeadline();
+#endif
+ if (p == mdh)
+ {
+ return true;
+ }
+ p = p->getNextListNode();
+ }
+ return false;
+}
+
+MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock)
+{
+ while (true)
+ {
+ DeadlineHandler* const mdh = handlers_.get();
+ if (!mdh)
+ {
+ return sysclock.getMonotonic();
+ }
+#if UAVCAN_DEBUG
+ if (mdh->getNextListNode()) // Order check
+ {
+ UAVCAN_ASSERT(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline());
+ }
+#endif
+
+ const MonotonicTime ts = sysclock.getMonotonic();
+ if (ts < mdh->getDeadline())
+ {
+ return ts;
+ }
+
+ handlers_.remove(mdh);
+ mdh->handleDeadline(ts); // This handler can be re-registered immediately
+ }
+ UAVCAN_ASSERT(0);
+ return MonotonicTime();
+}
+
+MonotonicTime DeadlineScheduler::getEarliestDeadline() const
+{
+ const DeadlineHandler* const mdh = handlers_.get();
+ if (mdh)
+ {
+ return mdh->getDeadline();
+ }
+ return MonotonicTime::getMax();
+}
+
+/*
+ * Scheduler
+ */
+MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const
+{
+ const MonotonicTime earliest = min(deadline_scheduler_.getEarliestDeadline(), spin_deadline);
+ const MonotonicTime ts = getMonotonicTime();
+ if (earliest > ts)
+ {
+ if (earliest - ts > deadline_resolution_)
+ {
+ return ts + deadline_resolution_;
+ }
+ }
+ return earliest;
+}
+
+void Scheduler::pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin)
+{
+ // cleanup will be performed less frequently if the stack handles more frames per second
+ const MonotonicTime deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1);
+ if (mono_ts > deadline)
+ {
+ //UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin);
+ prev_cleanup_ts_ = mono_ts;
+ dispatcher_.cleanup(mono_ts);
+ }
+}
+
+int Scheduler::spin(MonotonicTime deadline)
+{
+ if (inside_spin_) // Preventing recursive calls
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrRecursiveCall;
+ }
+ InsideSpinSetter iss(*this);
+ UAVCAN_ASSERT(inside_spin_);
+
+ int retval = 0;
+ while (true)
+ {
+ const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
+ retval = dispatcher_.spin(dl);
+ if (retval < 0)
+ {
+ break;
+ }
+
+ const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
+ pollCleanup(ts, unsigned(retval));
+ if (ts >= deadline)
+ {
+ break;
+ }
+ }
+
+ return retval;
+}
+
+int Scheduler::spinOnce()
+{
+ if (inside_spin_) // Preventing recursive calls
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrRecursiveCall;
+ }
+ InsideSpinSetter iss(*this);
+ UAVCAN_ASSERT(inside_spin_);
+
+ const int retval = dispatcher_.spinOnce();
+ if (retval < 0)
+ {
+ return retval;
+ }
+
+ const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
+ pollCleanup(ts, unsigned(retval));
+
+ return retval;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/node/uc_service_client.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/node/service_client.hpp>
+
+namespace uavcan
+{
+/*
+ * ServiceClientBase::CallState
+ */
+void ServiceClientBase::CallState::handleDeadline(MonotonicTime)
+{
+ UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s",
+ int(id_.server_node_id.get()), int(id_.transfer_id.get()),
+ (owner_.data_type_descriptor_ == UAVCAN_NULLPTR) ? "???" : owner_.data_type_descriptor_->getFullName());
+ /*
+ * What we're doing here is relaying execution from this call stack to a different one.
+ * We need it because call registry cannot release memory from this callback, because this will destroy the
+ * object method of which we're executing now.
+ */
+ UAVCAN_ASSERT(timed_out_ == false);
+ timed_out_ = true;
+ owner_.generateDeadlineImmediately();
+ UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback");
+}
+
+/*
+ * ServiceClientBase
+ */
+int ServiceClientBase::prepareToCall(INode& node,
+ const char* dtname,
+ NodeID server_node_id,
+ ServiceCallID& out_call_id)
+{
+ /*
+ * Making sure we're not going to get transport error because of invalid input data
+ */
+ if (!server_node_id.isUnicast() || (server_node_id == node.getNodeID()))
+ {
+ UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID");
+ return -ErrInvalidParam;
+ }
+ out_call_id.server_node_id = server_node_id;
+
+ /*
+ * Determining the Data Type ID
+ */
+ if (data_type_descriptor_ == UAVCAN_NULLPTR)
+ {
+ GlobalDataTypeRegistry::instance().freeze();
+ data_type_descriptor_ = GlobalDataTypeRegistry::instance().find(DataTypeKindService, dtname);
+ if (data_type_descriptor_ == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", dtname);
+ return -ErrUnknownDataType;
+ }
+ UAVCAN_TRACE("ServiceClient", "Data type descriptor inited: %s", data_type_descriptor_->toString().c_str());
+ }
+ UAVCAN_ASSERT(data_type_descriptor_ != UAVCAN_NULLPTR);
+
+ /*
+ * Determining the Transfer ID
+ */
+ const OutgoingTransferRegistryKey otr_key(data_type_descriptor_->getID(),
+ TransferTypeServiceRequest, server_node_id);
+ const MonotonicTime otr_deadline = node.getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval();
+ TransferID* const otr_tid =
+ node.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
+ if (!otr_tid)
+ {
+ UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str());
+ return -ErrMemory;
+ }
+ out_call_id.transfer_id = *otr_tid;
+ otr_tid->increment();
+
+ return 0;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/node/uc_timer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/node/timer.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+/*
+ * TimerBase
+ */
+void TimerBase::handleDeadline(MonotonicTime current)
+{
+ UAVCAN_ASSERT(!isRunning());
+
+ const MonotonicTime scheduled_time = getDeadline();
+
+ if (period_ < MonotonicDuration::getInfinite())
+ {
+ startWithDeadline(scheduled_time + period_);
+ }
+
+ // Application can re-register the timer with different params, it's OK
+ handleTimerEvent(TimerEvent(scheduled_time, current));
+}
+
+void TimerBase::startOneShotWithDeadline(MonotonicTime deadline)
+{
+ stop();
+ period_ = MonotonicDuration::getInfinite();
+ DeadlineHandler::startWithDeadline(deadline);
+}
+
+void TimerBase::startOneShotWithDelay(MonotonicDuration delay)
+{
+ stop();
+ period_ = MonotonicDuration::getInfinite();
+ DeadlineHandler::startWithDelay(delay);
+}
+
+void TimerBase::startPeriodic(MonotonicDuration period)
+{
+ UAVCAN_ASSERT(period < MonotonicDuration::getInfinite());
+ stop();
+ period_ = period;
+ DeadlineHandler::startWithDelay(period);
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,210 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdlib>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+
+namespace uavcan
+{
+
+void DynamicNodeIDClient::terminate()
+{
+ UAVCAN_TRACE("DynamicNodeIDClient", "Client terminated");
+ stop();
+ dnida_sub_.stop();
+}
+
+MonotonicDuration DynamicNodeIDClient::getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec)
+{
+ UAVCAN_ASSERT(upper_bound_msec > lower_bound_msec);
+ // coverity[dont_call]
+ return MonotonicDuration::fromMSec(lower_bound_msec +
+ static_cast<uint32_t>(std::rand()) % (upper_bound_msec - lower_bound_msec));
+}
+
+void DynamicNodeIDClient::restartTimer(const Mode mode)
+{
+ UAVCAN_ASSERT(mode < NumModes);
+ UAVCAN_ASSERT((mode == ModeWaitingForTimeSlot) == (size_of_received_unique_id_ == 0));
+
+ const MonotonicDuration delay = (mode == ModeWaitingForTimeSlot) ?
+ getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_REQUEST_PERIOD_MS,
+ protocol::dynamic_node_id::Allocation::MAX_REQUEST_PERIOD_MS) :
+ getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_FOLLOWUP_DELAY_MS,
+ protocol::dynamic_node_id::Allocation::MAX_FOLLOWUP_DELAY_MS);
+
+ startOneShotWithDelay(delay);
+
+ UAVCAN_TRACE("DynamicNodeIDClient", "Restart mode %d, delay %d ms",
+ static_cast<int>(mode), static_cast<int>(delay.toMSec()));
+}
+
+void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&)
+{
+ UAVCAN_ASSERT(preferred_node_id_.isValid());
+ UAVCAN_ASSERT(size_of_received_unique_id_ < protocol::dynamic_node_id::Allocation::FieldTypes::unique_id::MaxSize);
+
+ if (isAllocationComplete())
+ {
+ UAVCAN_ASSERT(0);
+ terminate();
+ return;
+ }
+
+ /*
+ * Filling the message.
+ */
+ protocol::dynamic_node_id::Allocation tx;
+ tx.node_id = preferred_node_id_.get();
+ tx.first_part_of_unique_id = (size_of_received_unique_id_ == 0);
+
+ const uint8_t size_of_unique_id_in_request =
+ min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST,
+ static_cast<uint8_t>(tx.unique_id.capacity() - size_of_received_unique_id_));
+
+ tx.unique_id.resize(size_of_unique_id_in_request);
+ copy(unique_id_ + size_of_received_unique_id_,
+ unique_id_ + size_of_received_unique_id_ + size_of_unique_id_in_request,
+ tx.unique_id.begin());
+
+ UAVCAN_ASSERT(equal(tx.unique_id.begin(), tx.unique_id.end(), unique_id_ + size_of_received_unique_id_));
+
+ /*
+ * Resetting the state - this way we can continue with a first stage request on the next attempt.
+ */
+ size_of_received_unique_id_ = 0;
+ restartTimer(ModeWaitingForTimeSlot);
+
+ /*
+ * Broadcasting the message.
+ */
+ UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting; preferred ID %d, size of UID %d",
+ static_cast<int>(preferred_node_id_.get()),
+ static_cast<int>(tx.unique_id.size()));
+ const int res = dnida_pub_.broadcast(tx);
+ if (res < 0)
+ {
+ dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
+ }
+}
+
+void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
+{
+ UAVCAN_ASSERT(preferred_node_id_.isValid());
+ if (isAllocationComplete())
+ {
+ UAVCAN_ASSERT(0);
+ terminate();
+ return;
+ }
+
+ UAVCAN_TRACE("DynamicNodeIDClient", "Allocation message from %d, %d bytes of unique ID, node ID %d",
+ static_cast<int>(msg.getSrcNodeID().get()), static_cast<int>(msg.unique_id.size()),
+ static_cast<int>(msg.node_id));
+
+ /*
+ * Switching to passive state by default; will switch to active state if response matches.
+ */
+ size_of_received_unique_id_ = 0;
+ restartTimer(ModeWaitingForTimeSlot);
+
+ /*
+ * Filtering out anonymous and invalid messages.
+ */
+ const bool full_response = (msg.unique_id.size() == msg.unique_id.capacity());
+
+ if (msg.isAnonymousTransfer() ||
+ msg.unique_id.empty() ||
+ (full_response && (msg.node_id == 0)))
+ {
+ UAVCAN_TRACE("DynamicNodeIDClient", "Message from %d ignored", static_cast<int>(msg.getSrcNodeID().get()));
+ return;
+ }
+
+ /*
+ * If matches, either switch to active mode or complete the allocation.
+ */
+ if (equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_))
+ {
+ if (full_response)
+ {
+ allocated_node_id_ = msg.node_id;
+ allocator_node_id_ = msg.getSrcNodeID();
+ terminate();
+ UAVCAN_ASSERT(isAllocationComplete());
+ UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d",
+ static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get()));
+ }
+ else
+ {
+ size_of_received_unique_id_ = msg.unique_id.size();
+ restartTimer(ModeDelayBeforeFollowup);
+ }
+ }
+}
+
+int DynamicNodeIDClient::start(const UniqueID& unique_id,
+ const NodeID preferred_node_id,
+ const TransferPriority transfer_priority)
+{
+ terminate();
+
+ // Allocation is not possible if node ID is already set
+ if (dnida_pub_.getNode().getNodeID().isUnicast())
+ {
+ return -ErrLogic;
+ }
+
+ // Unique ID initialization & validation
+ copy(unique_id.begin(), unique_id.end(), unique_id_);
+ bool unique_id_is_zero = true;
+ for (uint8_t i = 0; i < sizeof(unique_id_); i++)
+ {
+ if (unique_id_[i] != 0)
+ {
+ unique_id_is_zero = false;
+ break;
+ }
+ }
+
+ if (unique_id_is_zero)
+ {
+ return -ErrInvalidParam;
+ }
+
+ if (!preferred_node_id.isValid()) // Only broadcast and unicast are allowed
+ {
+ return -ErrInvalidParam;
+ }
+
+ // Initializing the fields
+ preferred_node_id_ = preferred_node_id;
+ allocated_node_id_ = NodeID();
+ allocator_node_id_ = NodeID();
+ UAVCAN_ASSERT(preferred_node_id_.isValid());
+ UAVCAN_ASSERT(!allocated_node_id_.isValid());
+ UAVCAN_ASSERT(!allocator_node_id_.isValid());
+
+ // Initializing node objects - Rule A
+ int res = dnida_pub_.init();
+ if (res < 0)
+ {
+ return res;
+ }
+ dnida_pub_.allowAnonymousTransfers();
+ dnida_pub_.setPriority(transfer_priority);
+
+ res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation));
+ if (res < 0)
+ {
+ return res;
+ }
+ dnida_sub_.allowAnonymousTransfers();
+
+ restartTimer(ModeWaitingForTimeSlot);
+
+ return 0;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/protocol/uc_node_status_provider.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/protocol/node_status_provider.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+
+bool NodeStatusProvider::isNodeInfoInitialized() const
+{
+ // Hardware/Software versions are not required
+ return !node_info_.name.empty();
+}
+
+int NodeStatusProvider::publish()
+{
+ const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_;
+ UAVCAN_ASSERT(uptime.isPositive());
+ node_info_.status.uptime_sec = uint32_t(uptime.toMSec() / 1000);
+
+ UAVCAN_ASSERT(node_info_.status.health <= protocol::NodeStatus::FieldTypes::health::max());
+
+ return node_status_pub_.broadcast(node_info_.status);
+}
+
+void NodeStatusProvider::handleTimerEvent(const TimerEvent&)
+{
+ if (getNode().isPassiveMode())
+ {
+ UAVCAN_TRACE("NodeStatusProvider", "NodeStatus pub skipped - passive mode");
+ }
+ else
+ {
+ if (ad_hoc_status_updater_ != UAVCAN_NULLPTR)
+ {
+ ad_hoc_status_updater_->updateNodeStatus();
+ }
+
+ const int res = publish();
+ if (res < 0)
+ {
+ getNode().registerInternalFailure("NodeStatus pub failed");
+ }
+ }
+}
+
+void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&,
+ protocol::GetNodeInfo::Response& rsp)
+{
+ UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request");
+ UAVCAN_ASSERT(isNodeInfoInitialized());
+ rsp = node_info_;
+}
+
+int NodeStatusProvider::startAndPublish(const TransferPriority priority)
+{
+ if (!isNodeInfoInitialized())
+ {
+ UAVCAN_TRACE("NodeStatusProvider", "Node info was not initialized");
+ return -ErrNotInited;
+ }
+
+ int res = -1;
+
+ node_status_pub_.setPriority(priority);
+
+ if (!getNode().isPassiveMode())
+ {
+ res = publish();
+ if (res < 0) // Initial broadcast
+ {
+ goto fail;
+ }
+ }
+
+ res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest));
+ if (res < 0)
+ {
+ goto fail;
+ }
+
+ setStatusPublicationPeriod(MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS));
+
+ return res;
+
+fail:
+ UAVCAN_ASSERT(res < 0);
+ gni_srv_.stop();
+ TimerBase::stop();
+ return res;
+}
+
+void NodeStatusProvider::setStatusPublicationPeriod(uavcan::MonotonicDuration period)
+{
+ const MonotonicDuration maximum = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS);
+ const MonotonicDuration minimum = MonotonicDuration::fromMSec(protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS);
+
+ period = min(period, maximum);
+ period = max(period, minimum);
+ TimerBase::startPeriodic(period);
+
+ const MonotonicDuration tx_timeout = period - MonotonicDuration::fromUSec(period.toUSec() / 20);
+ node_status_pub_.setTxTimeout(tx_timeout);
+
+ UAVCAN_TRACE("NodeStatusProvider", "Status pub period: %s, TX timeout: %s",
+ period.toString().c_str(), node_status_pub_.getTxTimeout().toString().c_str());
+}
+
+uavcan::MonotonicDuration NodeStatusProvider::getStatusPublicationPeriod() const
+{
+ return TimerBase::getPeriod();
+}
+
+void NodeStatusProvider::setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater)
+{
+ ad_hoc_status_updater_ = updater; // Can be nullptr, that's okay
+}
+
+void NodeStatusProvider::setHealth(uint8_t code)
+{
+ node_info_.status.health = code;
+}
+
+void NodeStatusProvider::setMode(uint8_t code)
+{
+ node_info_.status.mode = code;
+}
+
+void NodeStatusProvider::setVendorSpecificStatusCode(VendorSpecificStatusCode code)
+{
+ node_info_.status.vendor_specific_status_code = code;
+}
+
+void NodeStatusProvider::setName(const NodeName& name)
+{
+ if (node_info_.name.empty())
+ {
+ node_info_.name = name;
+ }
+}
+
+void NodeStatusProvider::setSoftwareVersion(const protocol::SoftwareVersion& version)
+{
+ if (node_info_.software_version == protocol::SoftwareVersion())
+ {
+ node_info_.software_version = version;
+ }
+}
+
+void NodeStatusProvider::setHardwareVersion(const protocol::HardwareVersion& version)
+{
+ if (node_info_.hardware_version == protocol::HardwareVersion())
+ {
+ node_info_.hardware_version = version;
+ }
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,256 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ * Ilia Sheremet <illia.sheremet@gmail.com>
+ */
+
+#include <uavcan/transport/can_acceptance_filter_configurator.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask;
+const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceID;
+const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceMask;
+const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgMask;
+const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID;
+
+int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode)
+{
+ multiset_configs_.clear();
+
+ if (load_mode == AcceptAnonymousMessages)
+ {
+ CanFilterConfig anon_frame_cfg;
+ anon_frame_cfg.id = DefaultAnonMsgID | CanFrame::FlagEFF;
+ anon_frame_cfg.mask = DefaultAnonMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
+ if (multiset_configs_.emplace(anon_frame_cfg) == UAVCAN_NULLPTR)
+ {
+ return -ErrMemory;
+ }
+ }
+
+ CanFilterConfig service_cfg;
+ service_cfg.id = DefaultFilterServiceID;
+ service_cfg.id |= (static_cast<uint32_t>(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF;
+ service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
+ if (multiset_configs_.emplace(service_cfg) == UAVCAN_NULLPTR)
+ {
+ return -ErrMemory;
+ }
+
+ const TransferListener* p = node_.getDispatcher().getListOfMessageListeners().get();
+ while (p != UAVCAN_NULLPTR)
+ {
+ CanFilterConfig cfg;
+ cfg.id = (static_cast<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF;
+ cfg.mask = DefaultFilterMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
+ if (multiset_configs_.emplace(cfg) == UAVCAN_NULLPTR)
+ {
+ return -ErrMemory;
+ }
+ p = p->getNextListNode();
+ }
+
+ if (multiset_configs_.getSize() == 0)
+ {
+ return -ErrLogic;
+ }
+
+ return 0;
+}
+
+int16_t CanAcceptanceFilterConfigurator::mergeConfigurations()
+{
+ const uint16_t acceptance_filters_number = getNumFilters();
+ if (acceptance_filters_number == 0)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
+ return -ErrDriver;
+ }
+ UAVCAN_ASSERT(multiset_configs_.getSize() != 0);
+
+ while (acceptance_filters_number < multiset_configs_.getSize())
+ {
+ uint16_t i_rank = 0, j_rank = 0;
+ uint8_t best_rank = 0;
+
+ const uint16_t multiset_array_size = static_cast<uint16_t>(multiset_configs_.getSize());
+
+ for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++)
+ {
+ for (uint16_t j_ind = static_cast<uint8_t>(i_ind + 1); j_ind < multiset_array_size; j_ind++)
+ {
+ CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind),
+ *multiset_configs_.getByIndex(j_ind));
+ uint8_t rank = countBits(temp_config.mask);
+ if (rank > best_rank)
+ {
+ best_rank = rank;
+ i_rank = i_ind;
+ j_rank = j_ind;
+ }
+ }
+ }
+
+ *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank),
+ *multiset_configs_.getByIndex(j_rank));
+ multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank));
+ }
+
+ UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize());
+
+ return 0;
+}
+
+int CanAcceptanceFilterConfigurator::applyConfiguration(void)
+{
+ CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters];
+ unsigned int filter_array_size = multiset_configs_.getSize();
+
+ const uint16_t acceptance_filters_number = getNumFilters();
+ if (acceptance_filters_number == 0)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
+ return -ErrDriver;
+ }
+
+ if (filter_array_size > acceptance_filters_number)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()");
+ mergeConfigurations();
+ filter_array_size = multiset_configs_.getSize();
+ }
+
+ if (filter_array_size > MaxCanAcceptanceFilters)
+ {
+ UAVCAN_ASSERT(0);
+ UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
+ return -ErrLogic;
+ }
+
+ for (uint16_t i = 0; i < filter_array_size; i++)
+ {
+ CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i);
+
+ filter_conf_array[i] = temp_filter_config;
+ }
+
+#if UAVCAN_DEBUG
+ for (uint16_t i = 0; i < multiset_configs_.getSize(); i++)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %u", i,
+ multiset_configs_.getByIndex(i)->id);
+ UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", i,
+ multiset_configs_.getByIndex(i)->mask);
+ }
+#endif
+
+ ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver();
+ for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
+ {
+ ICanIface* iface = can_driver.getIface(i);
+ if (iface == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
+ return -ErrDriver;
+ }
+ int16_t num = iface->configureFilters(reinterpret_cast<CanFilterConfig*>(&filter_conf_array),
+ static_cast<uint16_t>(filter_array_size));
+ if (num < 0)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
+ return -ErrDriver;
+ }
+ }
+
+ return 0;
+}
+
+int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode)
+{
+ if (getNumFilters() == 0)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
+ return -ErrDriver;
+ }
+
+ int16_t fill_array_error = loadInputConfiguration(mode);
+ if (fill_array_error != 0)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()");
+ return fill_array_error;
+ }
+
+ int16_t merge_configurations_error = mergeConfigurations();
+ if (merge_configurations_error != 0)
+ {
+ UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration");
+ return merge_configurations_error;
+ }
+
+ return 0;
+}
+
+uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const
+{
+ if (filters_number_ == 0)
+ {
+ static const uint16_t InvalidOut = 0xFFFF;
+ uint16_t out = InvalidOut;
+ ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver();
+
+ for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
+ {
+ const ICanIface* iface = can_driver.getIface(i);
+ if (iface == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ out = 0;
+ break;
+ }
+ const uint16_t num = iface->getNumFilters();
+ out = min(out, num);
+ if (out > MaxCanAcceptanceFilters)
+ {
+ out = MaxCanAcceptanceFilters;
+ }
+ }
+
+ return (out == InvalidOut) ? 0 : out;
+ }
+ else
+ {
+ return filters_number_;
+ }
+}
+
+int CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config)
+{
+ if (multiset_configs_.emplace<const CanFilterConfig&>(config) == UAVCAN_NULLPTR)
+ {
+ return -ErrMemory;
+ }
+
+ return 0;
+}
+
+CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_)
+{
+ CanFilterConfig temp_arr;
+ temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id);
+ temp_arr.id = a_.id & temp_arr.mask;
+
+ return temp_arr;
+}
+
+uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_)
+{
+ uint8_t c_; // c accumulates the total bits set in v
+ for (c_ = 0; n_; c_++)
+ {
+ n_ &= n_ - 1; // clear the least significant bit set
+ }
+
+ return c_;
+}
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_can_io.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,509 @@
+/*
+ * CAN bus IO logic.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/can_io.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+/*
+ * CanRxFrame
+ */
+#if UAVCAN_TOSTRING
+std::string CanRxFrame::toString(StringRepresentation mode) const
+{
+ std::string out = CanFrame::toString(mode);
+ out.reserve(128);
+ out += " ts_m=" + ts_mono.toString();
+ out += " ts_utc=" + ts_utc.toString();
+ out += " iface=";
+ out += char('0' + iface_index);
+ return out;
+}
+#endif
+
+/*
+ * CanTxQueue::Entry
+ */
+void CanTxQueue::Entry::destroy(Entry*& obj, IPoolAllocator& allocator)
+{
+ if (obj != UAVCAN_NULLPTR)
+ {
+ obj->~Entry();
+ allocator.deallocate(obj);
+ obj = UAVCAN_NULLPTR;
+ }
+}
+
+bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const
+{
+ if (qos != rhs_qos)
+ {
+ return qos > rhs_qos;
+ }
+ return frame.priorityHigherThan(rhs_frame);
+}
+
+bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const
+{
+ if (qos != rhs_qos)
+ {
+ return qos < rhs_qos;
+ }
+ return frame.priorityLowerThan(rhs_frame);
+}
+
+#if UAVCAN_TOSTRING
+std::string CanTxQueue::Entry::toString() const
+{
+ std::string str_qos;
+ switch (qos)
+ {
+ case Volatile:
+ {
+ str_qos = "<volat> ";
+ break;
+ }
+ case Persistent:
+ {
+ str_qos = "<perst> ";
+ break;
+ }
+ default:
+ {
+ UAVCAN_ASSERT(0);
+ str_qos = "<?WTF?> ";
+ break;
+ }
+ }
+ return str_qos + frame.toString();
+}
+#endif
+
+/*
+ * CanTxQueue
+ */
+CanTxQueue::~CanTxQueue()
+{
+ Entry* p = queue_.get();
+ while (p)
+ {
+ Entry* const next = p->getNextListNode();
+ remove(p);
+ p = next;
+ }
+}
+
+void CanTxQueue::registerRejectedFrame()
+{
+ if (rejected_frames_cnt_ < NumericTraits<uint32_t>::max())
+ {
+ rejected_frames_cnt_++;
+ }
+}
+
+void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
+{
+ const MonotonicTime timestamp = sysclock_.getMonotonic();
+
+ if (timestamp >= tx_deadline)
+ {
+ UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
+ registerRejectedFrame();
+ return;
+ }
+
+ void* praw = allocator_.allocate(sizeof(Entry));
+ if (praw == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup");
+ // No memory left in the pool, so we try to remove expired frames
+ Entry* p = queue_.get();
+ while (p)
+ {
+ Entry* const next = p->getNextListNode();
+ if (p->isExpired(timestamp))
+ {
+ UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str());
+ registerRejectedFrame();
+ remove(p);
+ }
+ p = next;
+ }
+ praw = allocator_.allocate(sizeof(Entry)); // Try again
+ }
+
+ if (praw == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration");
+ registerRejectedFrame();
+
+ // Find a frame with lowest QoS
+ Entry* p = queue_.get();
+ if (p == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("CanTxQueue", "Push rejected: Nothing to replace");
+ return;
+ }
+ Entry* lowestqos = p;
+ while (p)
+ {
+ if (lowestqos->qosHigherThan(*p))
+ {
+ lowestqos = p;
+ }
+ p = p->getNextListNode();
+ }
+ // Note that frame with *equal* QoS will be replaced too.
+ if (lowestqos->qosHigherThan(frame, qos)) // Frame that we want to transmit has lowest QoS
+ {
+ UAVCAN_TRACE("CanTxQueue", "Push rejected: low QoS");
+ return; // What a loser.
+ }
+ UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str());
+ remove(lowestqos);
+ praw = allocator_.allocate(sizeof(Entry)); // Try again
+ }
+
+ if (praw == UAVCAN_NULLPTR)
+ {
+ return; // Seems that there is no memory at all.
+ }
+ Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags);
+ UAVCAN_ASSERT(entry);
+ queue_.insertBefore(entry, PriorityInsertionComparator(frame));
+}
+
+CanTxQueue::Entry* CanTxQueue::peek()
+{
+ const MonotonicTime timestamp = sysclock_.getMonotonic();
+ Entry* p = queue_.get();
+ while (p)
+ {
+ if (p->isExpired(timestamp))
+ {
+ UAVCAN_TRACE("CanTxQueue", "Peek: Expired %s", p->toString().c_str());
+ Entry* const next = p->getNextListNode();
+ registerRejectedFrame();
+ remove(p);
+ p = next;
+ }
+ else
+ {
+ return p;
+ }
+ }
+ return UAVCAN_NULLPTR;
+}
+
+void CanTxQueue::remove(Entry*& entry)
+{
+ if (entry == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ return;
+ }
+ queue_.remove(entry);
+ Entry::destroy(entry, allocator_);
+}
+
+const CanFrame* CanTxQueue::getTopPriorityPendingFrame() const
+{
+ return (queue_.get() == UAVCAN_NULLPTR) ? UAVCAN_NULLPTR : &queue_.get()->frame;
+}
+
+bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
+{
+ const Entry* entry = queue_.get();
+ if (entry == UAVCAN_NULLPTR)
+ {
+ return false;
+ }
+ return !rhs_frame.priorityHigherThan(entry->frame);
+}
+
+/*
+ * CanIOManager
+ */
+int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags)
+{
+ UAVCAN_ASSERT(iface_index < MaxCanIfaces);
+ ICanIface* const iface = driver_.getIface(iface_index);
+ if (iface == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0); // Nonexistent interface
+ return -ErrLogic;
+ }
+ const int res = iface->send(frame, tx_deadline, flags);
+ if (res != 1)
+ {
+ UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
+ res, iface_index, frame.toString().c_str());
+ }
+ if (res > 0)
+ {
+ counters_[iface_index].frames_tx += unsigned(res);
+ }
+ return res;
+}
+
+int CanIOManager::sendFromTxQueue(uint8_t iface_index)
+{
+ UAVCAN_ASSERT(iface_index < MaxCanIfaces);
+ CanTxQueue::Entry* entry = tx_queues_[iface_index]->peek();
+ if (entry == UAVCAN_NULLPTR)
+ {
+ return 0;
+ }
+ const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags);
+ if (res > 0)
+ {
+ tx_queues_[iface_index]->remove(entry);
+ }
+ return res;
+}
+
+int CanIOManager::callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces],
+ MonotonicTime blocking_deadline)
+{
+ const CanSelectMasks in_masks = inout_masks;
+
+ const int res = driver_.select(inout_masks, pending_tx, blocking_deadline);
+ if (res < 0)
+ {
+ return -ErrDriver;
+ }
+
+ inout_masks.read &= in_masks.read; // Driver is not required to clean the masks
+ inout_masks.write &= in_masks.write;
+ return res;
+}
+
+CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
+ std::size_t mem_blocks_per_iface)
+ : driver_(driver)
+ , sysclock_(sysclock)
+ , num_ifaces_(driver.getNumIfaces())
+{
+ if (num_ifaces_ < 1 || num_ifaces_ > MaxCanIfaces)
+ {
+ handleFatalError("Num ifaces");
+ }
+
+ if (mem_blocks_per_iface == 0)
+ {
+ mem_blocks_per_iface = allocator.getBlockCapacity() / (num_ifaces_ + 1U) + 1U;
+ }
+ UAVCAN_TRACE("CanIOManager", "Memory blocks per iface: %u, total: %u",
+ unsigned(mem_blocks_per_iface), unsigned(allocator.getBlockCapacity()));
+
+ for (int i = 0; i < num_ifaces_; i++)
+ {
+ tx_queues_[i].construct<IPoolAllocator&, ISystemClock&, std::size_t>
+ (allocator, sysclock, mem_blocks_per_iface);
+ }
+}
+
+uint8_t CanIOManager::makePendingTxMask() const
+{
+ uint8_t write_mask = 0;
+ for (uint8_t i = 0; i < getNumIfaces(); i++)
+ {
+ if (!tx_queues_[i]->isEmpty())
+ {
+ write_mask = uint8_t(write_mask | (1 << i));
+ }
+ }
+ return write_mask;
+}
+
+CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const
+{
+ ICanIface* const iface = driver_.getIface(iface_index);
+ if (iface == UAVCAN_NULLPTR || iface_index >= MaxCanIfaces)
+ {
+ UAVCAN_ASSERT(0);
+ return CanIfacePerfCounters();
+ }
+ CanIfacePerfCounters cnt;
+ cnt.errors = iface->getErrorCount() + tx_queues_[iface_index]->getRejectedFrameCount();
+ cnt.frames_rx = counters_[iface_index].frames_rx;
+ cnt.frames_tx = counters_[iface_index].frames_tx;
+ return cnt;
+}
+
+int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
+ uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
+{
+ const uint8_t num_ifaces = getNumIfaces();
+ const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1);
+ iface_mask &= all_ifaces_mask;
+
+ if (blocking_deadline > tx_deadline)
+ {
+ blocking_deadline = tx_deadline;
+ }
+
+ int retval = 0;
+
+ while (true) // Somebody please refactor this.
+ {
+ if (iface_mask == 0)
+ {
+ break;
+ }
+
+ CanSelectMasks masks;
+ masks.write = iface_mask | makePendingTxMask();
+ {
+ // Building the list of next pending frames per iface.
+ // The driver will give them a scrutinizing look before deciding whether he wants to accept them.
+ const CanFrame* pending_tx[MaxCanIfaces] = {};
+ for (int i = 0; i < num_ifaces; i++)
+ {
+ CanTxQueue& q = *tx_queues_[i];
+ if (iface_mask & (1 << i)) // I hate myself so much right now.
+ {
+ pending_tx[i] = q.topPriorityHigherOrEqual(frame) ? q.getTopPriorityPendingFrame() : &frame;
+ }
+ else
+ {
+ pending_tx[i] = q.getTopPriorityPendingFrame();
+ }
+ }
+
+ const int select_res = callSelect(masks, pending_tx, blocking_deadline);
+ if (select_res < 0)
+ {
+ return -ErrDriver;
+ }
+ UAVCAN_ASSERT(masks.read == 0);
+ }
+
+ // Transmission
+ for (uint8_t i = 0; i < num_ifaces; i++)
+ {
+ if (masks.write & (1 << i))
+ {
+ int res = 0;
+ if (iface_mask & (1 << i))
+ {
+ if (tx_queues_[i]->topPriorityHigherOrEqual(frame))
+ {
+ res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired)
+ }
+ if (res <= 0)
+ {
+ res = sendToIface(i, frame, tx_deadline, flags);
+ if (res > 0)
+ {
+ iface_mask &= uint8_t(~(1 << i)); // Mark transmitted
+ }
+ }
+ }
+ else
+ {
+ res = sendFromTxQueue(i);
+ }
+ if (res > 0)
+ {
+ retval++;
+ }
+ }
+ }
+
+ // Timeout. Enqueue the frame if wasn't transmitted and leave.
+ const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline;
+ if (masks.write == 0 || timed_out)
+ {
+ if (!timed_out)
+ {
+ UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
+ continue;
+ }
+ for (uint8_t i = 0; i < num_ifaces; i++)
+ {
+ if (iface_mask & (1 << i))
+ {
+ tx_queues_[i]->push(frame, tx_deadline, qos, flags);
+ }
+ }
+ break;
+ }
+ }
+ return retval;
+}
+
+int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags)
+{
+ const uint8_t num_ifaces = getNumIfaces();
+
+ while (true)
+ {
+ CanSelectMasks masks;
+ masks.write = makePendingTxMask();
+ masks.read = uint8_t((1 << num_ifaces) - 1);
+ {
+ const CanFrame* pending_tx[MaxCanIfaces] = {};
+ for (int i = 0; i < num_ifaces; i++) // Dear compiler, kindly unroll this. Thanks.
+ {
+ pending_tx[i] = tx_queues_[i]->getTopPriorityPendingFrame();
+ }
+
+ const int select_res = callSelect(masks, pending_tx, blocking_deadline);
+ if (select_res < 0)
+ {
+ return -ErrDriver;
+ }
+ }
+
+ // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
+ for (uint8_t i = 0; i < num_ifaces; i++)
+ {
+ if (masks.write & (1 << i))
+ {
+ (void)sendFromTxQueue(i); // It may fail, we don't care. Requested operation was receive, not send.
+ }
+ }
+
+ // Read
+ for (uint8_t i = 0; i < num_ifaces; i++)
+ {
+ if (masks.read & (1 << i))
+ {
+ ICanIface* const iface = driver_.getIface(i);
+ if (iface == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0); // Nonexistent interface
+ continue;
+ }
+
+ const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags);
+ if (res == 0)
+ {
+ UAVCAN_ASSERT(0); // select() reported that iface has pending RX frames, but receive() returned none
+ continue;
+ }
+ out_frame.iface_index = i;
+
+ if ((res > 0) && !(out_flags & CanIOFlagLoopback))
+ {
+ counters_[i].frames_rx += 1;
+ }
+ return (res < 0) ? -ErrDriver : res;
+ }
+ }
+
+ // Timeout checked in the last order - this way we can operate with expired deadline:
+ if (sysclock_.getMonotonic() >= blocking_deadline)
+ {
+ break;
+ }
+ }
+ return 0;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_crc.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/crc.hpp>
+
+namespace uavcan
+{
+
+#if !UAVCAN_TINY
+
+// print ', '.join(map(lambda x: '%04x' % x, map(lambda x: int(x, 0), c.crc_ccitt_tab)))
+const uint16_t TransferCRC::Table[256] =
+{
+ 0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U,
+ 0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU,
+ 0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U,
+ 0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU,
+ 0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U,
+ 0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU,
+ 0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U,
+ 0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU,
+ 0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U,
+ 0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU,
+ 0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U,
+ 0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU,
+ 0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U,
+ 0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U,
+ 0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U,
+ 0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U,
+ 0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU,
+ 0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U,
+ 0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU,
+ 0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U,
+ 0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU,
+ 0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U,
+ 0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU,
+ 0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U,
+ 0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU,
+ 0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U,
+ 0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU,
+ 0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U,
+ 0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U,
+ 0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U,
+ 0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U,
+ 0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U
+};
+
+#endif
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_dispatcher.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,385 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/dispatcher.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+#if !UAVCAN_TINY
+/*
+ * LoopbackFrameListenerBase
+ */
+void LoopbackFrameListenerBase::startListening()
+{
+ dispatcher_.getLoopbackFrameListenerRegistry().add(this);
+}
+
+void LoopbackFrameListenerBase::stopListening()
+{
+ dispatcher_.getLoopbackFrameListenerRegistry().remove(this);
+}
+
+bool LoopbackFrameListenerBase::isListening() const
+{
+ return dispatcher_.getLoopbackFrameListenerRegistry().doesExist(this);
+}
+
+/*
+ * LoopbackFrameListenerRegistry
+ */
+void LoopbackFrameListenerRegistry::add(LoopbackFrameListenerBase* listener)
+{
+ UAVCAN_ASSERT(listener);
+ listeners_.insert(listener);
+}
+
+void LoopbackFrameListenerRegistry::remove(LoopbackFrameListenerBase* listener)
+{
+ UAVCAN_ASSERT(listener);
+ listeners_.remove(listener);
+}
+
+bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* listener) const
+{
+ UAVCAN_ASSERT(listener);
+ const LoopbackFrameListenerBase* p = listeners_.get();
+ while (p)
+ {
+ if (p == listener)
+ {
+ return true;
+ }
+ p = p->getNextListNode();
+ }
+ return false;
+}
+
+void LoopbackFrameListenerRegistry::invokeListeners(RxFrame& frame)
+{
+ LoopbackFrameListenerBase* p = listeners_.get();
+ while (p)
+ {
+ LoopbackFrameListenerBase* const next = p->getNextListNode();
+ p->handleLoopbackFrame(frame); // p may be modified
+ p = next;
+ }
+}
+#endif
+
+/*
+ * Dispatcher::ListenerRegister
+ */
+bool Dispatcher::ListenerRegistry::add(TransferListener* listener, Mode mode)
+{
+ if (mode == UniqueListener)
+ {
+ TransferListener* p = list_.get();
+ while (p)
+ {
+ if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID())
+ {
+ return false;
+ }
+ p = p->getNextListNode();
+ }
+ }
+ // Objective is to arrange entries by Data Type ID in ascending order from root.
+ list_.insertBefore(listener, DataTypeIDInsertionComparator(listener->getDataTypeDescriptor().getID()));
+ return true;
+}
+
+void Dispatcher::ListenerRegistry::remove(TransferListener* listener)
+{
+ list_.remove(listener);
+}
+
+bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const
+{
+ TransferListener* p = list_.get();
+ while (p)
+ {
+ if (p->getDataTypeDescriptor().getID() == dtid)
+ {
+ return true;
+ }
+ p = p->getNextListNode();
+ }
+ return false;
+}
+
+void Dispatcher::ListenerRegistry::cleanup(MonotonicTime ts)
+{
+ TransferListener* p = list_.get();
+ while (p)
+ {
+ TransferListener* const next = p->getNextListNode();
+ p->cleanup(ts); // p may be modified
+ p = next;
+ }
+}
+
+void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame)
+{
+ TransferListener* p = list_.get();
+ while (p)
+ {
+ TransferListener* const next = p->getNextListNode();
+ if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
+ {
+ p->handleFrame(frame); // p may be modified
+ }
+ else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id!
+ {
+ break;
+ }
+ else
+ {
+ ; // Nothing to do with this one
+ }
+ p = next;
+ }
+}
+
+/*
+ * Dispatcher
+ */
+void Dispatcher::handleFrame(const CanRxFrame& can_frame)
+{
+ RxFrame frame;
+ if (!frame.parse(can_frame))
+ {
+ // This is not counted as a transport error
+ UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
+ return;
+ }
+
+ if ((frame.getDstNodeID() != NodeID::Broadcast) &&
+ (frame.getDstNodeID() != getNodeID()))
+ {
+ return;
+ }
+
+ switch (frame.getTransferType())
+ {
+ case TransferTypeMessageBroadcast:
+ {
+ lmsg_.handleFrame(frame);
+ break;
+ }
+ case TransferTypeServiceRequest:
+ {
+ lsrv_req_.handleFrame(frame);
+ break;
+ }
+ case TransferTypeServiceResponse:
+ {
+ lsrv_resp_.handleFrame(frame);
+ break;
+ }
+ default:
+ {
+ UAVCAN_ASSERT(0);
+ break;
+ }
+ }
+}
+
+#if UAVCAN_TINY
+void Dispatcher::handleLoopbackFrame(const CanRxFrame&)
+{
+}
+
+void Dispatcher::notifyRxFrameListener(const CanRxFrame&, CanIOFlags)
+{
+}
+#else
+void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame)
+{
+ RxFrame frame;
+ if (!frame.parse(can_frame))
+ {
+ UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str());
+ UAVCAN_ASSERT(0); // No way!
+ return;
+ }
+ UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID());
+ loopback_listeners_.invokeListeners(frame);
+}
+
+void Dispatcher::notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags)
+{
+ if (rx_listener_ != UAVCAN_NULLPTR)
+ {
+ rx_listener_->handleRxFrame(can_frame, flags);
+ }
+}
+#endif
+
+int Dispatcher::spin(MonotonicTime deadline)
+{
+ int num_frames_processed = 0;
+ do
+ {
+ CanIOFlags flags = 0;
+ CanRxFrame frame;
+ const int res = canio_.receive(frame, deadline, flags);
+ if (res < 0)
+ {
+ return res;
+ }
+ if (res > 0)
+ {
+ if (flags & CanIOFlagLoopback)
+ {
+ handleLoopbackFrame(frame);
+ }
+ else
+ {
+ num_frames_processed++;
+ handleFrame(frame);
+ }
+ notifyRxFrameListener(frame, flags);
+ }
+ }
+ while (sysclock_.getMonotonic() < deadline);
+
+ return num_frames_processed;
+}
+
+int Dispatcher::spinOnce()
+{
+ int num_frames_processed = 0;
+
+ while (true)
+ {
+ CanIOFlags flags = 0;
+ CanRxFrame frame;
+ const int res = canio_.receive(frame, MonotonicTime(), flags);
+ if (res < 0)
+ {
+ return res;
+ }
+ else if (res > 0)
+ {
+ if (flags & CanIOFlagLoopback)
+ {
+ handleLoopbackFrame(frame);
+ }
+ else
+ {
+ num_frames_processed++;
+ handleFrame(frame);
+ }
+ notifyRxFrameListener(frame, flags);
+ }
+ else
+ {
+ break; // No frames left
+ }
+ }
+
+ return num_frames_processed;
+}
+
+int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
+ CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask)
+{
+ if (frame.getSrcNodeID() != getNodeID())
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrLogic;
+ }
+
+ CanFrame can_frame;
+ if (!frame.compile(can_frame))
+ {
+ UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str());
+ UAVCAN_ASSERT(0);
+ return -ErrLogic;
+ }
+ return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags);
+}
+
+void Dispatcher::cleanup(MonotonicTime ts)
+{
+ outgoing_transfer_reg_.cleanup(ts);
+ lmsg_.cleanup(ts);
+ lsrv_req_.cleanup(ts);
+ lsrv_resp_.cleanup(ts);
+}
+
+bool Dispatcher::registerMessageListener(TransferListener* listener)
+{
+ if (listener->getDataTypeDescriptor().getKind() != DataTypeKindMessage)
+ {
+ UAVCAN_ASSERT(0);
+ return false;
+ }
+ return lmsg_.add(listener, ListenerRegistry::ManyListeners); // Multiple subscribers are OK
+}
+
+bool Dispatcher::registerServiceRequestListener(TransferListener* listener)
+{
+ if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService)
+ {
+ UAVCAN_ASSERT(0);
+ return false;
+ }
+ return lsrv_req_.add(listener, ListenerRegistry::UniqueListener); // Only one server per data type
+}
+
+bool Dispatcher::registerServiceResponseListener(TransferListener* listener)
+{
+ if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService)
+ {
+ UAVCAN_ASSERT(0);
+ return false;
+ }
+ return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv
+}
+
+void Dispatcher::unregisterMessageListener(TransferListener* listener)
+{
+ lmsg_.remove(listener);
+}
+
+void Dispatcher::unregisterServiceRequestListener(TransferListener* listener)
+{
+ lsrv_req_.remove(listener);
+}
+
+void Dispatcher::unregisterServiceResponseListener(TransferListener* listener)
+{
+ lsrv_resp_.remove(listener);
+}
+
+bool Dispatcher::hasSubscriber(DataTypeID dtid) const
+{
+ return lmsg_.exists(dtid);
+}
+
+bool Dispatcher::hasPublisher(DataTypeID dtid) const
+{
+ return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast);
+}
+
+bool Dispatcher::hasServer(DataTypeID dtid) const
+{
+ return lsrv_req_.exists(dtid);
+}
+
+bool Dispatcher::setNodeID(NodeID nid)
+{
+ if (!self_node_id_is_set_)
+ {
+ self_node_id_ = nid;
+ self_node_id_is_set_ = true;
+ return true;
+ }
+ return false;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_frame.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,334 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/frame.hpp>
+#include <uavcan/transport/can_io.hpp>
+#include <uavcan/transport/crc.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+namespace uavcan
+{
+/**
+ * Frame
+ */
+uint8_t Frame::setPayload(const uint8_t* data, unsigned len)
+{
+ const uint8_t maxlen = getPayloadCapacity();
+ len = min(unsigned(maxlen), len);
+ (void)copy(data, data + len, payload_);
+ payload_len_ = uint_fast8_t(len);
+ return static_cast<uint8_t>(len);
+}
+
+template <int OFFSET, int WIDTH>
+inline static uint32_t bitunpack(uint32_t val)
+{
+ StaticAssert<(OFFSET >= 0)>::check();
+ StaticAssert<(WIDTH > 0)>::check();
+ StaticAssert<((OFFSET + WIDTH) <= 29)>::check();
+ return (val >> OFFSET) & ((1UL << WIDTH) - 1);
+}
+
+bool Frame::parse(const CanFrame& can_frame)
+{
+ if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended())
+ {
+ UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__);
+ return false;
+ }
+
+ if (can_frame.dlc > sizeof(can_frame.data))
+ {
+ UAVCAN_ASSERT(0); // This is not a protocol error, so UAVCAN_ASSERT() is ok
+ return false;
+ }
+
+ if (can_frame.dlc < 1)
+ {
+ UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__);
+ return false;
+ }
+
+ /*
+ * CAN ID parsing
+ */
+ const uint32_t id = can_frame.id & CanFrame::MaskExtID;
+
+ transfer_priority_ = static_cast<uint8_t>(bitunpack<24, 5>(id));
+ src_node_id_ = static_cast<uint8_t>(bitunpack<0, 7>(id));
+
+ const bool service_not_message = bitunpack<7, 1>(id) != 0U;
+ if (service_not_message)
+ {
+ const bool request_not_response = bitunpack<15, 1>(id) != 0U;
+ transfer_type_ = request_not_response ? TransferTypeServiceRequest : TransferTypeServiceResponse;
+
+ dst_node_id_ = static_cast<uint8_t>(bitunpack<8, 7>(id));
+ data_type_id_ = static_cast<uint16_t>(bitunpack<16, 8>(id));
+ }
+ else
+ {
+ transfer_type_ = TransferTypeMessageBroadcast;
+ dst_node_id_ = NodeID::Broadcast;
+
+ data_type_id_ = static_cast<uint16_t>(bitunpack<8, 16>(id));
+
+ if (src_node_id_.isBroadcast())
+ {
+ // Removing the discriminator
+ data_type_id_ = static_cast<uint16_t>(data_type_id_.get() & 3U);
+ }
+ }
+
+ /*
+ * CAN payload parsing
+ */
+ payload_len_ = static_cast<uint8_t>(can_frame.dlc - 1U);
+ (void)copy(can_frame.data, can_frame.data + payload_len_, payload_);
+
+ const uint8_t tail = can_frame.data[can_frame.dlc - 1U];
+
+ start_of_transfer_ = (tail & (1U << 7)) != 0;
+ end_of_transfer_ = (tail & (1U << 6)) != 0;
+ toggle_ = (tail & (1U << 5)) != 0;
+
+ transfer_id_ = tail & TransferID::Max;
+
+ return isValid();
+}
+
+template <int OFFSET, int WIDTH>
+inline static uint32_t bitpack(uint32_t field)
+{
+ StaticAssert<(OFFSET >= 0)>::check();
+ StaticAssert<(WIDTH > 0)>::check();
+ StaticAssert<((OFFSET + WIDTH) <= 29)>::check();
+ UAVCAN_ASSERT((field & ((1UL << WIDTH) - 1)) == field);
+ return uint32_t((field & ((1UL << WIDTH) - 1)) << OFFSET);
+}
+
+bool Frame::compile(CanFrame& out_can_frame) const
+{
+ if (!isValid())
+ {
+ UAVCAN_ASSERT(0); // This is an application error, so we need to maximize it.
+ return false;
+ }
+
+ /*
+ * CAN ID field
+ */
+ out_can_frame.id = CanFrame::FlagEFF |
+ bitpack<0, 7>(src_node_id_.get()) |
+ bitpack<24, 5>(transfer_priority_.get());
+
+ if (transfer_type_ == TransferTypeMessageBroadcast)
+ {
+ out_can_frame.id |=
+ bitpack<7, 1>(0U) |
+ bitpack<8, 16>(data_type_id_.get());
+ }
+ else
+ {
+ const bool request_not_response = transfer_type_ == TransferTypeServiceRequest;
+ out_can_frame.id |=
+ bitpack<7, 1>(1U) |
+ bitpack<8, 7>(dst_node_id_.get()) |
+ bitpack<15, 1>(request_not_response ? 1U : 0U) |
+ bitpack<16, 8>(data_type_id_.get());
+ }
+
+ /*
+ * Payload
+ */
+ uint8_t tail = transfer_id_.get();
+ if (start_of_transfer_)
+ {
+ tail |= (1U << 7);
+ }
+ if (end_of_transfer_)
+ {
+ tail |= (1U << 6);
+ }
+ if (toggle_)
+ {
+ tail |= (1U << 5);
+ }
+
+ UAVCAN_ASSERT(payload_len_ < sizeof(static_cast<CanFrame*>(UAVCAN_NULLPTR)->data));
+
+ out_can_frame.dlc = static_cast<uint8_t>(payload_len_);
+ (void)copy(payload_, payload_ + payload_len_, out_can_frame.data);
+
+ out_can_frame.data[out_can_frame.dlc] = tail;
+ out_can_frame.dlc++;
+
+ /*
+ * Discriminator
+ */
+ if (src_node_id_.isBroadcast())
+ {
+ TransferCRC crc;
+ crc.add(out_can_frame.data, out_can_frame.dlc);
+ out_can_frame.id |= bitpack<10, 14>(crc.get() & ((1U << 14) - 1U));
+ }
+
+ return true;
+}
+
+bool Frame::isValid() const
+{
+ /*
+ * Toggle
+ */
+ if (start_of_transfer_ && toggle_)
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ /*
+ * Node ID
+ */
+ if (!src_node_id_.isValid() || !dst_node_id_.isValid())
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ if (src_node_id_.isUnicast() && (src_node_id_ == dst_node_id_))
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ /*
+ * Transfer type
+ */
+ if (transfer_type_ >= NumTransferTypes)
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast())
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ // Anonymous transfers
+ if (src_node_id_.isBroadcast() &&
+ (!start_of_transfer_ || !end_of_transfer_ || (transfer_type_ != TransferTypeMessageBroadcast)))
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ /*
+ * Payload
+ */
+ if (payload_len_ > getPayloadCapacity())
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ /*
+ * Data type ID
+ */
+ if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_)))
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ /*
+ * Priority
+ */
+ if (!transfer_priority_.isValid())
+ {
+ UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
+ return false;
+ }
+
+ return true;
+}
+
+bool Frame::operator==(const Frame& rhs) const
+{
+ return
+ (transfer_priority_ == rhs.transfer_priority_) &&
+ (transfer_type_ == rhs.transfer_type_) &&
+ (data_type_id_ == rhs.data_type_id_) &&
+ (src_node_id_ == rhs.src_node_id_) &&
+ (dst_node_id_ == rhs.dst_node_id_) &&
+ (transfer_id_ == rhs.transfer_id_) &&
+ (toggle_ == rhs.toggle_) &&
+ (start_of_transfer_ == rhs.start_of_transfer_) &&
+ (end_of_transfer_ == rhs.end_of_transfer_) &&
+ (payload_len_ == rhs.payload_len_) &&
+ equal(payload_, payload_ + payload_len_, rhs.payload_);
+}
+
+#if UAVCAN_TOSTRING
+std::string Frame::toString() const
+{
+ static const int BUFLEN = 100;
+ char buf[BUFLEN];
+ int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d sot=%d eot=%d togl=%d tid=%d payload=[",
+ int(transfer_priority_.get()), int(data_type_id_.get()), int(transfer_type_),
+ int(src_node_id_.get()), int(dst_node_id_.get()),
+ int(start_of_transfer_), int(end_of_transfer_), int(toggle_), int(transfer_id_.get()));
+
+ for (unsigned i = 0; i < payload_len_; i++)
+ {
+ // Coverity Scan complains about payload_ being not default initialized. This is OK.
+ // coverity[read_parm_fld]
+ ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), "%02x", payload_[i]);
+ if ((i + 1) < payload_len_)
+ {
+ ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), " ");
+ }
+ }
+ (void)snprintf(buf + ofs, unsigned(BUFLEN - ofs), "]");
+ return std::string(buf);
+}
+#endif
+
+/**
+ * RxFrame
+ */
+bool RxFrame::parse(const CanRxFrame& can_frame)
+{
+ if (!Frame::parse(can_frame))
+ {
+ return false;
+ }
+ if (can_frame.ts_mono.isZero()) // Monotonic timestamps are mandatory.
+ {
+ UAVCAN_ASSERT(0); // If it is not set, it's a driver failure.
+ return false;
+ }
+ ts_mono_ = can_frame.ts_mono;
+ ts_utc_ = can_frame.ts_utc;
+ iface_index_ = can_frame.iface_index;
+ return true;
+}
+
+#if UAVCAN_TOSTRING
+std::string RxFrame::toString() const
+{
+ std::string out = Frame::toString();
+ out.reserve(128);
+ out += " ts_m=" + ts_mono_.toString();
+ out += " ts_utc=" + ts_utc_.toString();
+ out += " iface=";
+ out += char('0' + iface_index_);
+ return out;
+}
+#endif
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/outgoing_transfer_registry.hpp>
+
+namespace uavcan
+{
+/*
+ * OutgoingTransferRegistryKey
+ */
+#if UAVCAN_TOSTRING
+std::string OutgoingTransferRegistryKey::toString() const
+{
+ char buf[40];
+ (void)snprintf(buf, sizeof(buf), "dtid=%u tt=%u dnid=%u",
+ int(data_type_id_.get()), int(transfer_type_), int(destination_node_id_.get()));
+ return std::string(buf);
+}
+#endif
+
+/*
+ * OutgoingTransferRegistry
+ */
+const MonotonicDuration OutgoingTransferRegistry::MinEntryLifetime = MonotonicDuration::fromMSec(2000);
+
+TransferID* OutgoingTransferRegistry::accessOrCreate(const OutgoingTransferRegistryKey& key,
+ MonotonicTime new_deadline)
+{
+ UAVCAN_ASSERT(!new_deadline.isZero());
+ Value* p = map_.access(key);
+ if (p == UAVCAN_NULLPTR)
+ {
+ p = map_.insert(key, Value());
+ if (p == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ UAVCAN_TRACE("OutgoingTransferRegistry", "Created %s", key.toString().c_str());
+ }
+ p->deadline = new_deadline;
+ return &p->tid;
+}
+
+bool OutgoingTransferRegistry::exists(DataTypeID dtid, TransferType tt) const
+{
+ return UAVCAN_NULLPTR != map_.find(ExistenceCheckingPredicate(dtid, tt));
+}
+
+void OutgoingTransferRegistry::cleanup(MonotonicTime ts)
+{
+ map_.removeAllWhere(DeadlineExpiredPredicate(ts));
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_transfer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/transfer.hpp>
+#include <uavcan/transport/frame.hpp>
+#include <uavcan/transport/can_io.hpp>
+
+namespace uavcan
+{
+/**
+ * TransferPriority
+ */
+const uint8_t TransferPriority::BitLen;
+const uint8_t TransferPriority::NumericallyMax;
+const uint8_t TransferPriority::NumericallyMin;
+
+const TransferPriority TransferPriority::Default((1U << BitLen) / 2);
+const TransferPriority TransferPriority::MiddleLower((1U << BitLen) / 2 + (1U << BitLen) / 4);
+const TransferPriority TransferPriority::OneHigherThanLowest(NumericallyMax - 1);
+const TransferPriority TransferPriority::OneLowerThanHighest(NumericallyMin + 1);
+const TransferPriority TransferPriority::Lowest(NumericallyMax);
+
+/**
+ * TransferID
+ */
+const uint8_t TransferID::BitLen;
+const uint8_t TransferID::Max;
+const uint8_t TransferID::Half;
+
+/**
+ * NodeID
+ */
+const uint8_t NodeID::ValueBroadcast;
+const uint8_t NodeID::ValueInvalid;
+const uint8_t NodeID::BitLen;
+const uint8_t NodeID::Max;
+const uint8_t NodeID::MaxRecommendedForRegularNodes;
+const NodeID NodeID::Broadcast(ValueBroadcast);
+
+/**
+ * TransferID
+ */
+int TransferID::computeForwardDistance(TransferID rhs) const
+{
+ int d = int(rhs.get()) - int(get());
+ if (d < 0)
+ {
+ d += 1 << BitLen;
+ }
+
+ UAVCAN_ASSERT(((get() + d) & Max) == rhs.get());
+ return d;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_transfer_buffer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,362 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/transfer_buffer.hpp>
+#include <cassert>
+#include <cstdlib>
+
+namespace uavcan
+{
+/*
+ * StaticTransferBufferImpl
+ */
+int StaticTransferBufferImpl::read(unsigned offset, uint8_t* data, unsigned len) const
+{
+ if (!data)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+ if (offset >= max_write_pos_)
+ {
+ return 0;
+ }
+ if ((offset + len) > max_write_pos_)
+ {
+ len = max_write_pos_ - offset;
+ }
+ UAVCAN_ASSERT((offset + len) <= max_write_pos_);
+ (void)copy(data_ + offset, data_ + offset + len, data);
+ return int(len);
+}
+
+int StaticTransferBufferImpl::write(unsigned offset, const uint8_t* data, unsigned len)
+{
+ if (!data)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+ if (offset >= size_)
+ {
+ return 0;
+ }
+ if ((offset + len) > size_)
+ {
+ len = size_ - offset;
+ }
+ UAVCAN_ASSERT((offset + len) <= size_);
+ (void)copy(data, data + len, data_ + offset);
+ max_write_pos_ = max(uint16_t(offset + len), uint16_t(max_write_pos_));
+ return int(len);
+}
+
+void StaticTransferBufferImpl::reset()
+{
+ max_write_pos_ = 0;
+#if UAVCAN_DEBUG
+ fill(data_, data_ + size_, uint8_t(0));
+#endif
+}
+
+/*
+ * TransferBufferManagerKey
+ */
+#if UAVCAN_TOSTRING
+std::string TransferBufferManagerKey::toString() const
+{
+ char buf[24];
+ (void)snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_));
+ return std::string(buf);
+}
+#endif
+
+/*
+ * DynamicTransferBuffer::Block
+ */
+TransferBufferManagerEntry::Block*
+TransferBufferManagerEntry::Block::instantiate(IPoolAllocator& allocator)
+{
+ void* const praw = allocator.allocate(sizeof(Block));
+ if (praw == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ return new (praw) Block;
+}
+
+void TransferBufferManagerEntry::Block::destroy(Block*& obj, IPoolAllocator& allocator)
+{
+ if (obj != UAVCAN_NULLPTR)
+ {
+ obj->~Block();
+ allocator.deallocate(obj);
+ obj = UAVCAN_NULLPTR;
+ }
+}
+
+void TransferBufferManagerEntry::Block::read(uint8_t*& outptr, unsigned target_offset,
+ unsigned& total_offset, unsigned& left_to_read)
+{
+ UAVCAN_ASSERT(outptr);
+ for (unsigned i = 0; (i < Block::Size) && (left_to_read > 0); i++, total_offset++)
+ {
+ if (total_offset >= target_offset)
+ {
+ *outptr++ = data[i];
+ left_to_read--;
+ }
+ }
+}
+
+void TransferBufferManagerEntry::Block::write(const uint8_t*& inptr, unsigned target_offset,
+ unsigned& total_offset, unsigned& left_to_write)
+{
+ UAVCAN_ASSERT(inptr);
+ for (unsigned i = 0; (i < Block::Size) && (left_to_write > 0); i++, total_offset++)
+ {
+ if (total_offset >= target_offset)
+ {
+ data[i] = *inptr++;
+ left_to_write--;
+ }
+ }
+}
+
+/*
+ * DynamicTransferBuffer
+ */
+TransferBufferManagerEntry* TransferBufferManagerEntry::instantiate(IPoolAllocator& allocator,
+ uint16_t max_size)
+{
+ void* const praw = allocator.allocate(sizeof(TransferBufferManagerEntry));
+ if (praw == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR;
+ }
+ return new (praw) TransferBufferManagerEntry(allocator, max_size);
+}
+
+void TransferBufferManagerEntry::destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator)
+{
+ if (obj != UAVCAN_NULLPTR)
+ {
+ obj->~TransferBufferManagerEntry();
+ allocator.deallocate(obj);
+ obj = UAVCAN_NULLPTR;
+ }
+}
+
+int TransferBufferManagerEntry::read(unsigned offset, uint8_t* data, unsigned len) const
+{
+ if (!data)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+ if (offset >= max_write_pos_)
+ {
+ return 0;
+ }
+ if ((offset + len) > max_write_pos_)
+ {
+ len = max_write_pos_ - offset;
+ }
+ UAVCAN_ASSERT((offset + len) <= max_write_pos_);
+
+ // This shall be optimized.
+ unsigned total_offset = 0;
+ unsigned left_to_read = len;
+ uint8_t* outptr = data;
+ Block* p = blocks_.get();
+ while (p)
+ {
+ p->read(outptr, offset, total_offset, left_to_read);
+ if (left_to_read == 0)
+ {
+ break;
+ }
+ p = p->getNextListNode();
+ }
+
+ UAVCAN_ASSERT(left_to_read == 0);
+ return int(len);
+}
+
+int TransferBufferManagerEntry::write(unsigned offset, const uint8_t* data, unsigned len)
+{
+ if (!data)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+
+ if (offset >= max_size_)
+ {
+ return 0;
+ }
+ if ((offset + len) > max_size_)
+ {
+ len = max_size_ - offset;
+ }
+ UAVCAN_ASSERT((offset + len) <= max_size_);
+
+ unsigned total_offset = 0;
+ unsigned left_to_write = len;
+ const uint8_t* inptr = data;
+ Block* p = blocks_.get();
+ Block* last_written_block = UAVCAN_NULLPTR;
+
+ // First we need to write the part that is already allocated
+ while (p)
+ {
+ last_written_block = p;
+ p->write(inptr, offset, total_offset, left_to_write);
+ if (left_to_write == 0)
+ {
+ break;
+ }
+ p = p->getNextListNode();
+ }
+
+ // Then we need to append new chunks until all data is written
+ while (left_to_write > 0)
+ {
+ // cppcheck-suppress nullPointer
+ UAVCAN_ASSERT(p == UAVCAN_NULLPTR);
+
+ // Allocating the chunk
+ Block* new_block = Block::instantiate(allocator_);
+ if (new_block == UAVCAN_NULLPTR)
+ {
+ break; // We're in deep shit.
+ }
+ // Appending the chain with the new block
+ if (last_written_block != UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(last_written_block->getNextListNode() == UAVCAN_NULLPTR); // Because it is last in the chain
+ last_written_block->setNextListNode(new_block);
+ new_block->setNextListNode(UAVCAN_NULLPTR);
+ }
+ else
+ {
+ blocks_.insert(new_block);
+ }
+ last_written_block = new_block;
+
+ // Writing the data
+ new_block->write(inptr, offset, total_offset, left_to_write);
+ }
+
+ UAVCAN_ASSERT(len >= left_to_write);
+ const unsigned actually_written = len - left_to_write;
+ max_write_pos_ = max(uint16_t(offset + actually_written), uint16_t(max_write_pos_));
+ return int(actually_written);
+}
+
+void TransferBufferManagerEntry::reset(const TransferBufferManagerKey& key)
+{
+ key_ = key;
+ max_write_pos_ = 0;
+ Block* p = blocks_.get();
+ while (p)
+ {
+ Block* const next = p->getNextListNode();
+ blocks_.remove(p);
+ Block::destroy(p, allocator_);
+ p = next;
+ }
+}
+
+/*
+ * TransferBufferManager
+ */
+TransferBufferManagerEntry* TransferBufferManager::findFirst(const TransferBufferManagerKey& key)
+{
+ TransferBufferManagerEntry* dyn = buffers_.get();
+ while (dyn)
+ {
+ UAVCAN_ASSERT(!dyn->isEmpty());
+ if (dyn->getKey() == key)
+ {
+ return dyn;
+ }
+ dyn = dyn->getNextListNode();
+ }
+ return UAVCAN_NULLPTR;
+}
+
+TransferBufferManager::~TransferBufferManager()
+{
+ TransferBufferManagerEntry* dyn = buffers_.get();
+ while (dyn)
+ {
+ TransferBufferManagerEntry* const next = dyn->getNextListNode();
+ buffers_.remove(dyn);
+ TransferBufferManagerEntry::destroy(dyn, allocator_);
+ dyn = next;
+ }
+}
+
+ITransferBuffer* TransferBufferManager::access(const TransferBufferManagerKey& key)
+{
+ if (key.isEmpty())
+ {
+ UAVCAN_ASSERT(0);
+ return UAVCAN_NULLPTR;
+ }
+ return findFirst(key);
+}
+
+ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key)
+{
+ if (key.isEmpty())
+ {
+ UAVCAN_ASSERT(0);
+ return UAVCAN_NULLPTR;
+ }
+ remove(key);
+
+ TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_);
+ if (tbme == UAVCAN_NULLPTR)
+ {
+ return UAVCAN_NULLPTR; // Epic fail.
+ }
+
+ buffers_.insert(tbme);
+
+ UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str());
+
+ if (tbme != UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(tbme->isEmpty());
+ tbme->reset(key);
+ }
+ return tbme;
+}
+
+void TransferBufferManager::remove(const TransferBufferManagerKey& key)
+{
+ UAVCAN_ASSERT(!key.isEmpty());
+
+ TransferBufferManagerEntry* dyn = findFirst(key);
+ if (dyn != UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("TransferBufferManager", "Buffer deleted, %s", key.toString().c_str());
+ buffers_.remove(dyn);
+ TransferBufferManagerEntry::destroy(dyn, allocator_);
+ }
+}
+
+bool TransferBufferManager::isEmpty() const
+{
+ return getNumBuffers() == 0;
+}
+
+unsigned TransferBufferManager::getNumBuffers() const
+{
+ return buffers_.getLength();
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_transfer_listener.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,258 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/transfer_listener.hpp>
+#include <uavcan/debug.hpp>
+#include <cstdlib>
+#include <cassert>
+
+namespace uavcan
+{
+/*
+ * IncomingTransfer
+ */
+int IncomingTransfer::write(unsigned, const uint8_t*, unsigned)
+{
+ UAVCAN_ASSERT(0); // Incoming transfer container is read-only
+ return -ErrLogic;
+}
+
+/*
+ * SingleFrameIncomingTransfer
+ */
+SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm)
+ : IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getPriority(),
+ frm.getTransferType(), frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex())
+ , payload_(frm.getPayloadPtr())
+ , payload_len_(uint8_t(frm.getPayloadLen()))
+{
+ UAVCAN_ASSERT(frm.isValid());
+}
+
+int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const
+{
+ if (data == UAVCAN_NULLPTR)
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrInvalidParam;
+ }
+ if (offset >= payload_len_)
+ {
+ return 0;
+ }
+ if ((offset + len) > payload_len_)
+ {
+ len = payload_len_ - offset;
+ }
+ UAVCAN_ASSERT((offset + len) <= payload_len_);
+ (void)copy(payload_ + offset, payload_ + offset + len, data);
+ return int(len);
+}
+
+bool SingleFrameIncomingTransfer::isAnonymousTransfer() const
+{
+ return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast();
+}
+
+/*
+ * MultiFrameIncomingTransfer
+ */
+MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc,
+ const RxFrame& last_frame, TransferBufferAccessor& tba)
+ : IncomingTransfer(ts_mono, ts_utc, last_frame.getPriority(), last_frame.getTransferType(),
+ last_frame.getTransferID(), last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
+ , buf_acc_(tba)
+{
+ UAVCAN_ASSERT(last_frame.isValid());
+ UAVCAN_ASSERT(last_frame.isEndOfTransfer());
+}
+
+int MultiFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const
+{
+ const ITransferBuffer* const tbb = const_cast<TransferBufferAccessor&>(buf_acc_).access();
+ if (tbb == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer");
+ return -ErrLogic;
+ }
+ return tbb->read(offset, data, len);
+}
+
+/*
+ * TransferListener::TimedOutReceiverPredicate
+ */
+bool TransferListener::TimedOutReceiverPredicate::operator()(const TransferBufferManagerKey& key,
+ const TransferReceiver& value) const
+{
+ if (value.isTimedOut(ts_))
+ {
+ UAVCAN_TRACE("TransferListener", "Timed out receiver: %s", key.toString().c_str());
+ /*
+ * TransferReceivers do not own their buffers - this helps the Map<> container to copy them
+ * around quickly and safely (using default assignment operator). Downside is that we need to
+ * destroy the buffers manually.
+ * Maybe it is not good that the predicate has side effects, but I ran out of better ideas.
+ */
+ parent_bufmgr_.remove(key);
+ return true;
+ }
+ return false;
+}
+
+/*
+ * TransferListener
+ */
+bool TransferListener::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const
+{
+ TransferCRC crc = crc_base_;
+ unsigned offset = 0;
+ while (true)
+ {
+ uint8_t buf[16];
+ const int res = tbb.read(offset, buf, sizeof(buf));
+ if (res < 0)
+ {
+ UAVCAN_TRACE("TransferListener", "Failed to check CRC: Buffer read failure %i", res);
+ return false;
+ }
+ if (res == 0)
+ {
+ break;
+ }
+ offset += unsigned(res);
+ crc.add(buf, unsigned(res));
+ }
+ if (crc.get() != compare_with)
+ {
+ UAVCAN_TRACE("TransferListener", "CRC mismatch, expected=0x%04x, got=0x%04x",
+ int(compare_with), int(crc.get()));
+ return false;
+ }
+ return true;
+}
+
+void TransferListener::handleReception(TransferReceiver& receiver, const RxFrame& frame,
+ TransferBufferAccessor& tba)
+{
+ switch (receiver.addFrame(frame, tba))
+ {
+ case TransferReceiver::ResultNotComplete:
+ {
+ perf_.addErrors(receiver.yieldErrorCount());
+ break;
+ }
+ case TransferReceiver::ResultSingleFrame:
+ {
+ perf_.addRxTransfer();
+ SingleFrameIncomingTransfer it(frame);
+ handleIncomingTransfer(it);
+ break;
+ }
+ case TransferReceiver::ResultComplete:
+ {
+ perf_.addRxTransfer();
+ const ITransferBuffer* tbb = tba.access();
+ if (tbb == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("TransferListener", "Buffer access failure, last frame: %s", frame.toString().c_str());
+ break;
+ }
+ if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb))
+ {
+ UAVCAN_TRACE("TransferListener", "CRC error, last frame: %s", frame.toString().c_str());
+ break;
+ }
+ MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(),
+ receiver.getLastTransferTimestampUtc(), frame, tba);
+ handleIncomingTransfer(it);
+ it.release();
+ break;
+ }
+ default:
+ {
+ UAVCAN_ASSERT(0);
+ break;
+ }
+ }
+}
+
+void TransferListener::handleAnonymousTransferReception(const RxFrame& frame)
+{
+ if (allow_anonymous_transfers_)
+ {
+ perf_.addRxTransfer();
+ SingleFrameIncomingTransfer it(frame);
+ handleIncomingTransfer(it);
+ }
+}
+
+TransferListener::~TransferListener()
+{
+ // Map must be cleared before bufmgr is destroyed
+ receivers_.clear();
+}
+
+void TransferListener::cleanup(MonotonicTime ts)
+{
+ receivers_.removeAllWhere(TimedOutReceiverPredicate(ts, bufmgr_));
+ UAVCAN_ASSERT(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
+}
+
+void TransferListener::handleFrame(const RxFrame& frame)
+{
+ if (frame.getSrcNodeID().isUnicast()) // Normal transfer
+ {
+ const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
+
+ TransferReceiver* recv = receivers_.access(key);
+ if (recv == UAVCAN_NULLPTR)
+ {
+ if (!frame.isStartOfTransfer())
+ {
+ return;
+ }
+
+ TransferReceiver new_recv;
+ recv = receivers_.insert(key, new_recv);
+ if (recv == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
+ return;
+ }
+ }
+ TransferBufferAccessor tba(bufmgr_, key);
+ handleReception(*recv, frame, tba);
+ }
+ else if (frame.getSrcNodeID().isBroadcast() &&
+ frame.isStartOfTransfer() &&
+ frame.isEndOfTransfer() &&
+ frame.getDstNodeID().isBroadcast()) // Anonymous transfer
+ {
+ handleAnonymousTransferReception(frame);
+ }
+ else
+ {
+ UAVCAN_TRACE("TransferListener", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame
+ }
+}
+
+/*
+ * TransferListenerWithFilter
+ */
+void TransferListenerWithFilter::handleFrame(const RxFrame& frame)
+{
+ if (filter_ != UAVCAN_NULLPTR)
+ {
+ if (filter_->shouldAcceptFrame(frame))
+ {
+ TransferListener::handleFrame(frame);
+ }
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_transfer_receiver.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,248 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/transfer_receiver.hpp>
+#include <uavcan/transport/crc.hpp>
+#include <uavcan/debug.hpp>
+#include <cstdlib>
+#include <cassert>
+
+namespace uavcan
+{
+
+const uint16_t TransferReceiver::MinTransferIntervalMSec;
+const uint16_t TransferReceiver::MaxTransferIntervalMSec;
+const uint16_t TransferReceiver::DefaultTransferIntervalMSec;
+const uint16_t TransferReceiver::DefaultTidTimeoutMSec;
+
+MonotonicDuration TransferReceiver::getIfaceSwitchDelay() const
+{
+ return MonotonicDuration::fromMSec(transfer_interval_msec_);
+}
+
+MonotonicDuration TransferReceiver::getTidTimeout() const
+{
+ return MonotonicDuration::fromMSec(DefaultTidTimeoutMSec);
+}
+
+void TransferReceiver::registerError() const
+{
+ error_cnt_ = static_cast<uint8_t>(error_cnt_ + 1) & ErrorCntMask;
+}
+
+void TransferReceiver::updateTransferTimings()
+{
+ UAVCAN_ASSERT(!this_transfer_ts_.isZero());
+
+ const MonotonicTime prev_prev_ts = prev_transfer_ts_;
+ prev_transfer_ts_ = this_transfer_ts_;
+
+ if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts))
+ {
+ uint64_t interval_msec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toMSec());
+ interval_msec = min(interval_msec, uint64_t(MaxTransferIntervalMSec));
+ interval_msec = max(interval_msec, uint64_t(MinTransferIntervalMSec));
+ transfer_interval_msec_ = static_cast<uint16_t>((uint64_t(transfer_interval_msec_) * 7U + interval_msec) / 8U);
+ }
+}
+
+void TransferReceiver::prepareForNextTransfer()
+{
+ tid_.increment();
+ next_toggle_ = false;
+ buffer_write_pos_ = 0;
+}
+
+bool TransferReceiver::validate(const RxFrame& frame) const
+{
+ if (iface_index_ != frame.getIfaceIndex())
+ {
+ return false;
+ }
+ if (frame.isStartOfTransfer() && !frame.isEndOfTransfer() && (frame.getPayloadLen() < TransferCRC::NumBytes))
+ {
+ UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
+ registerError();
+ return false;
+ }
+ if (frame.isStartOfTransfer() && frame.getToggle())
+ {
+ UAVCAN_TRACE("TransferReceiver", "Toggle bit is not cleared, %s", frame.toString().c_str());
+ registerError();
+ return false;
+ }
+ if (frame.isStartOfTransfer() && isMidTransfer())
+ {
+ UAVCAN_TRACE("TransferReceiver", "Unexpected start of transfer, %s", frame.toString().c_str());
+ registerError();
+ }
+ if (frame.getToggle() != next_toggle_)
+ {
+ UAVCAN_TRACE("TransferReceiver", "Unexpected toggle bit (not %i), %s",
+ int(next_toggle_), frame.toString().c_str());
+ registerError();
+ return false;
+ }
+ if (frame.getTransferID() != tid_)
+ {
+ UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str());
+ registerError();
+ return false;
+ }
+ return true;
+}
+
+bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
+{
+ const uint8_t* const payload = frame.getPayloadPtr();
+ const unsigned payload_len = frame.getPayloadLen();
+
+ if (frame.isStartOfTransfer()) // First frame contains CRC, we need to extract it now
+ {
+ if (frame.getPayloadLen() < TransferCRC::NumBytes)
+ {
+ return false; // Must have been validated earlier though. I think I'm paranoid.
+ }
+ this_transfer_crc_ = static_cast<uint16_t>(payload[0] & 0xFF);
+ this_transfer_crc_ |= static_cast<uint16_t>(static_cast<uint16_t>(payload[1] & 0xFF) << 8); // Little endian.
+
+ const unsigned effective_payload_len = payload_len - TransferCRC::NumBytes;
+ const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len);
+ const bool success = res == static_cast<int>(effective_payload_len);
+ if (success)
+ {
+ buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len);
+ }
+ return success;
+ }
+ else
+ {
+ const int res = buf.write(buffer_write_pos_, payload, payload_len);
+ const bool success = res == static_cast<int>(payload_len);
+ if (success)
+ {
+ buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len);
+ }
+ return success;
+ }
+}
+
+TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
+{
+ // Transfer timestamps are derived from the first frame
+ if (frame.isStartOfTransfer())
+ {
+ this_transfer_ts_ = frame.getMonotonicTimestamp();
+ first_frame_ts_ = frame.getUtcTimestamp();
+ }
+
+ if (frame.isStartOfTransfer() && frame.isEndOfTransfer())
+ {
+ tba.remove();
+ updateTransferTimings();
+ prepareForNextTransfer();
+ this_transfer_crc_ = 0; // SFT has no CRC
+ return ResultSingleFrame;
+ }
+
+ // Payload write
+ ITransferBuffer* buf = tba.access();
+ if (buf == UAVCAN_NULLPTR)
+ {
+ buf = tba.create();
+ }
+ if (buf == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
+ prepareForNextTransfer();
+ registerError();
+ return ResultNotComplete;
+ }
+ if (!writePayload(frame, *buf))
+ {
+ UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str());
+ tba.remove();
+ prepareForNextTransfer();
+ registerError();
+ return ResultNotComplete;
+ }
+ next_toggle_ = !next_toggle_;
+
+ if (frame.isEndOfTransfer())
+ {
+ updateTransferTimings();
+ prepareForNextTransfer();
+ return ResultComplete;
+ }
+ return ResultNotComplete;
+}
+
+bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
+{
+ return (current_ts - this_transfer_ts_) > getTidTimeout();
+}
+
+TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
+{
+ if ((frame.getMonotonicTimestamp().isZero()) ||
+ (frame.getMonotonicTimestamp() < prev_transfer_ts_) ||
+ (frame.getMonotonicTimestamp() < this_transfer_ts_))
+ {
+ UAVCAN_TRACE("TransferReceiver", "Invalid frame, %s", frame.toString().c_str());
+ return ResultNotComplete;
+ }
+
+ const bool not_initialized = !isInitialized();
+ const bool tid_timed_out = isTimedOut(frame.getMonotonicTimestamp());
+ const bool same_iface = frame.getIfaceIndex() == iface_index_;
+ const bool first_frame = frame.isStartOfTransfer();
+ const bool non_wrapped_tid = tid_.computeForwardDistance(frame.getTransferID()) < TransferID::Half;
+ const bool not_previous_tid = frame.getTransferID().computeForwardDistance(tid_) > 1;
+ const bool iface_switch_allowed = (frame.getMonotonicTimestamp() - this_transfer_ts_) > getIfaceSwitchDelay();
+
+ // FSM, the hard way
+ const bool need_restart =
+ (not_initialized) ||
+ (tid_timed_out) ||
+ (same_iface && first_frame && not_previous_tid) ||
+ (iface_switch_allowed && first_frame && non_wrapped_tid);
+
+ if (need_restart)
+ {
+ if (!not_initialized && (tid_ != frame.getTransferID()))
+ {
+ registerError();
+ }
+ UAVCAN_TRACE("TransferReceiver", "Restart [ni=%d, isa=%d, tt=%d, si=%d, ff=%d, nwtid=%d, nptid=%d, tid=%d], %s",
+ int(not_initialized), int(iface_switch_allowed), int(tid_timed_out), int(same_iface),
+ int(first_frame), int(non_wrapped_tid), int(not_previous_tid), int(tid_.get()),
+ frame.toString().c_str());
+ tba.remove();
+ iface_index_ = frame.getIfaceIndex() & IfaceIndexMask;
+ tid_ = frame.getTransferID();
+ next_toggle_ = false;
+ buffer_write_pos_ = 0;
+ this_transfer_crc_ = 0;
+ if (!first_frame)
+ {
+ tid_.increment();
+ return ResultNotComplete;
+ }
+ }
+
+ if (!validate(frame))
+ {
+ return ResultNotComplete;
+ }
+ return receive(frame, tba);
+}
+
+uint8_t TransferReceiver::yieldErrorCount()
+{
+ const uint8_t ret = error_cnt_;
+ error_cnt_ = 0;
+ return ret;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/transport/uc_transfer_sender.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,173 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/transport/transfer_sender.hpp>
+#include <uavcan/debug.hpp>
+#include <cassert>
+
+
+namespace uavcan
+{
+
+void TransferSender::registerError() const
+{
+ dispatcher_.getTransferPerfCounter().addError();
+}
+
+void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos)
+{
+ UAVCAN_ASSERT(!isInitialized());
+
+ qos_ = qos;
+ data_type_id_ = dtid.getID();
+ crc_base_ = dtid.getSignature().toTransferCRC();
+}
+
+int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
+ MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
+ TransferID tid) const
+{
+ Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, tid);
+
+ frame.setPriority(priority_);
+ frame.setStartOfTransfer(true);
+
+ UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str());
+
+ /*
+ * Checking if we're allowed to send.
+ * In passive mode we can send only anonymous transfers, if they are enabled.
+ */
+ if (dispatcher_.isPassiveMode())
+ {
+ const bool allow = allow_anonymous_transfers_ &&
+ (transfer_type == TransferTypeMessageBroadcast) &&
+ (int(payload_len) <= frame.getPayloadCapacity());
+ if (!allow)
+ {
+ return -ErrPassiveMode;
+ }
+ }
+
+ dispatcher_.getTransferPerfCounter().addTxTransfer();
+
+ /*
+ * Sending frames
+ */
+ if (frame.getPayloadCapacity() >= payload_len) // Single Frame Transfer
+ {
+ const int res = frame.setPayload(payload, payload_len);
+ if (res != int(payload_len))
+ {
+ UAVCAN_ASSERT(0);
+ UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res);
+ registerError();
+ return (res < 0) ? res : -ErrLogic;
+ }
+
+ frame.setEndOfTransfer(true);
+ UAVCAN_ASSERT(frame.isStartOfTransfer() && frame.isEndOfTransfer() && !frame.getToggle());
+
+ const CanIOFlags flags = frame.getSrcNodeID().isUnicast() ? flags_ : (flags_ | CanIOFlagAbortOnError);
+
+ return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags, iface_mask_);
+ }
+ else // Multi Frame Transfer
+ {
+ UAVCAN_ASSERT(!dispatcher_.isPassiveMode());
+ UAVCAN_ASSERT(frame.getSrcNodeID().isUnicast());
+
+ int offset = 0;
+ {
+ TransferCRC crc = crc_base_;
+ crc.add(payload, payload_len);
+
+ static const int BUFLEN = sizeof(static_cast<CanFrame*>(0)->data);
+ uint8_t buf[BUFLEN];
+
+ buf[0] = uint8_t(crc.get() & 0xFFU); // Transfer CRC, little endian
+ buf[1] = uint8_t((crc.get() >> 8) & 0xFF);
+ (void)copy(payload, payload + BUFLEN - 2, buf + 2);
+
+ const int write_res = frame.setPayload(buf, BUFLEN);
+ if (write_res < 2)
+ {
+ UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
+ registerError();
+ return write_res;
+ }
+ offset = write_res - 2;
+ UAVCAN_ASSERT(int(payload_len) > offset);
+ }
+
+ int num_sent = 0;
+
+ while (true)
+ {
+ const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_);
+ if (send_res < 0)
+ {
+ registerError();
+ return send_res;
+ }
+
+ num_sent++;
+ if (frame.isEndOfTransfer())
+ {
+ return num_sent; // Number of frames transmitted
+ }
+
+ frame.setStartOfTransfer(false);
+ frame.flipToggle();
+
+ UAVCAN_ASSERT(offset >= 0);
+ const int write_res = frame.setPayload(payload + offset, payload_len - unsigned(offset));
+ if (write_res < 0)
+ {
+ UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
+ registerError();
+ return write_res;
+ }
+
+ offset += write_res;
+ UAVCAN_ASSERT(offset <= int(payload_len));
+ if (offset >= int(payload_len))
+ {
+ frame.setEndOfTransfer(true);
+ }
+ }
+ }
+
+ UAVCAN_ASSERT(0);
+ return -ErrLogic; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
+}
+
+int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
+ MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const
+{
+ /*
+ * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped?
+ */
+ const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id);
+
+ UAVCAN_ASSERT(!tx_deadline.isZero());
+ const MonotonicTime otr_deadline = tx_deadline + max(max_transfer_interval_ * 2,
+ OutgoingTransferRegistry::MinEntryLifetime);
+
+ TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
+ if (tid == UAVCAN_NULLPTR)
+ {
+ UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i",
+ int(data_type_id_.get()), int(transfer_type));
+ return -ErrMemory;
+ }
+
+ const TransferID this_tid = tid->get();
+ tid->increment();
+
+ return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type,
+ dst_node_id, this_tid);
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/uc_data_type.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/data_type.hpp>
+#include <uavcan/transport/crc.hpp>
+#include <cstring>
+#include <cassert>
+
+namespace uavcan
+{
+/*
+ * DataTypeID
+ */
+const uint16_t DataTypeID::MaxServiceDataTypeIDValue;
+const uint16_t DataTypeID::MaxMessageDataTypeIDValue;
+const uint16_t DataTypeID::MaxPossibleDataTypeIDValue;
+
+DataTypeID DataTypeID::getMaxValueForDataTypeKind(const DataTypeKind dtkind)
+{
+ if (dtkind == DataTypeKindService)
+ {
+ return MaxServiceDataTypeIDValue;
+ }
+ else if (dtkind == DataTypeKindMessage)
+ {
+ return MaxMessageDataTypeIDValue;
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ return DataTypeID(0);
+ }
+}
+
+/*
+ * DataTypeSignatureCRC
+ */
+DataTypeSignatureCRC DataTypeSignatureCRC::extend(uint64_t crc)
+{
+ DataTypeSignatureCRC ret;
+ ret.crc_ = crc ^ 0xFFFFFFFFFFFFFFFF;
+ return ret;
+}
+
+void DataTypeSignatureCRC::add(uint8_t byte)
+{
+ static const uint64_t Poly = 0x42F0E1EBA9EA3693;
+ crc_ ^= uint64_t(byte) << 56;
+ for (int i = 0; i < 8; i++)
+ {
+ crc_ = (crc_ & (uint64_t(1) << 63)) ? (crc_ << 1) ^ Poly : crc_ << 1;
+ }
+}
+
+void DataTypeSignatureCRC::add(const uint8_t* bytes, unsigned len)
+{
+ UAVCAN_ASSERT(bytes);
+ while (len--)
+ {
+ add(*bytes++);
+ }
+}
+
+/*
+ * DataTypeSignature
+ */
+void DataTypeSignature::mixin64(uint64_t x)
+{
+ DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_);
+ for (int i = 0; i < 64; i += 8) // LSB first
+ {
+ crc.add((x >> i) & 0xFF);
+ }
+ value_ = crc.get();
+}
+
+void DataTypeSignature::extend(DataTypeSignature dts)
+{
+ const uint64_t y = value_;
+ mixin64(dts.get());
+ mixin64(y);
+}
+
+TransferCRC DataTypeSignature::toTransferCRC() const
+{
+ TransferCRC tcrc;
+ for (int i = 0; i < 64; i += 8) // LSB first
+ {
+ tcrc.add((value_ >> i) & 0xFF);
+ }
+ return tcrc;
+}
+
+/*
+ * DataTypeDescriptor
+ */
+const unsigned DataTypeDescriptor::MaxFullNameLen;
+
+bool DataTypeDescriptor::isValid() const
+{
+ return id_.isValidForDataTypeKind(kind_) &&
+ (full_name_ != UAVCAN_NULLPTR) &&
+ (*full_name_ != '\0');
+}
+
+bool DataTypeDescriptor::match(DataTypeKind kind, const char* name) const
+{
+ return (kind_ == kind) && !std::strncmp(full_name_, name, MaxFullNameLen);
+}
+
+bool DataTypeDescriptor::match(DataTypeKind kind, DataTypeID id) const
+{
+ return (kind_ == kind) && (id_ == id);
+}
+
+#if UAVCAN_TOSTRING
+std::string DataTypeDescriptor::toString() const
+{
+ char kindch = '?';
+ switch (kind_)
+ {
+ case DataTypeKindMessage:
+ {
+ kindch = 'm';
+ break;
+ }
+ case DataTypeKindService:
+ {
+ kindch = 's';
+ break;
+ }
+ default:
+ {
+ UAVCAN_ASSERT(0);
+ break;
+ }
+ }
+
+ char buf[128];
+ (void)snprintf(buf, sizeof(buf), "%s:%u%c:%016llx",
+ full_name_, static_cast<unsigned>(id_.get()), kindch,
+ static_cast<unsigned long long>(signature_.get()));
+ return std::string(buf);
+}
+#endif
+
+bool DataTypeDescriptor::operator==(const DataTypeDescriptor& rhs) const
+{
+ return
+ (kind_ == rhs.kind_) &&
+ (id_ == rhs.id_) &&
+ (signature_ == rhs.signature_) &&
+ !std::strncmp(full_name_, rhs.full_name_, MaxFullNameLen);
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/uc_dynamic_memory.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/dynamic_memory.hpp>
+
+namespace uavcan
+{
+/*
+ * LimitedPoolAllocator
+ */
+void* LimitedPoolAllocator::allocate(std::size_t size)
+{
+ if (used_blocks_ < max_blocks_)
+ {
+ used_blocks_++;
+ return allocator_.allocate(size);
+ }
+ else
+ {
+ return UAVCAN_NULLPTR;
+ }
+}
+
+void LimitedPoolAllocator::deallocate(const void* ptr)
+{
+ allocator_.deallocate(ptr);
+
+ UAVCAN_ASSERT(used_blocks_ > 0);
+ if (used_blocks_ > 0)
+ {
+ used_blocks_--;
+ }
+}
+
+uint16_t LimitedPoolAllocator::getBlockCapacity() const
+{
+ return min(max_blocks_, allocator_.getBlockCapacity());
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/src/uc_error.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/error.hpp>
+#include <cassert>
+#include <cstdlib>
+
+#ifndef UAVCAN_EXCEPTIONS
+# error UAVCAN_EXCEPTIONS
+#endif
+
+#if UAVCAN_EXCEPTIONS
+# include <stdexcept>
+#endif
+
+namespace uavcan
+{
+
+void handleFatalError(const char* msg)
+{
+#if UAVCAN_EXCEPTIONS
+ throw std::runtime_error(msg);
+#else
+ (void)msg;
+ UAVCAN_ASSERT(0);
+ std::abort();
+#endif
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/clock.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2014 <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <time.h>
+#include <sys/time.h>
+#include <uavcan/driver/system_clock.hpp>
+
+class SystemClockMock : public uavcan::ISystemClock
+{
+public:
+ mutable uint64_t monotonic;
+ mutable uint64_t utc;
+ uavcan::UtcDuration last_adjustment;
+ uint64_t monotonic_auto_advance;
+ bool preserve_utc;
+
+ SystemClockMock(uint64_t initial = 0)
+ : monotonic(initial)
+ , utc(initial)
+ , monotonic_auto_advance(0)
+ , preserve_utc(false)
+ { }
+
+ void advance(uint64_t usec) const
+ {
+ monotonic += usec;
+ if (!preserve_utc)
+ {
+ utc += usec;
+ }
+ }
+
+ virtual uavcan::MonotonicTime getMonotonic() const
+ {
+ assert(this);
+ const uint64_t res = monotonic;
+ advance(monotonic_auto_advance);
+ return uavcan::MonotonicTime::fromUSec(res);
+ }
+
+ virtual uavcan::UtcTime getUtc() const
+ {
+ assert(this);
+ return uavcan::UtcTime::fromUSec(utc);
+ }
+
+ virtual void adjustUtc(uavcan::UtcDuration adjustment)
+ {
+ assert(this);
+ const uint64_t prev_utc = utc;
+ utc = uint64_t(int64_t(utc) + adjustment.toUSec());
+ last_adjustment = adjustment;
+ std::cout << "Clock adjustment " << prev_utc << " --> " << utc << std::endl;
+ }
+};
+
+
+class SystemClockDriver : public uavcan::ISystemClock
+{
+public:
+ uavcan::UtcDuration utc_adjustment;
+
+ virtual uavcan::MonotonicTime getMonotonic() const
+ {
+ struct timespec ts;
+ const int ret = clock_gettime(CLOCK_MONOTONIC, &ts);
+ if (ret != 0)
+ {
+ assert(0);
+ return uavcan::MonotonicTime();
+ }
+ return uavcan::MonotonicTime::fromUSec(uint64_t(int64_t(ts.tv_sec) * 1000000L + int64_t(ts.tv_nsec / 1000L)));
+ }
+
+ virtual uavcan::UtcTime getUtc() const
+ {
+ struct timeval tv;
+ const int ret = gettimeofday(&tv, UAVCAN_NULLPTR);
+ if (ret != 0)
+ {
+ assert(0);
+ return uavcan::UtcTime();
+ }
+ return uavcan::UtcTime::fromUSec(uint64_t(int64_t(tv.tv_sec) * 1000000L + tv.tv_usec)) + utc_adjustment;
+ }
+
+ virtual void adjustUtc(uavcan::UtcDuration adjustment)
+ {
+ utc_adjustment += adjustment;
+ }
+};
+
+inline uavcan::MonotonicTime tsMono(uint64_t usec) { return uavcan::MonotonicTime::fromUSec(usec); }
+inline uavcan::UtcTime tsUtc(uint64_t usec) { return uavcan::UtcTime::fromUSec(usec); }
+
+inline uavcan::MonotonicDuration durMono(int64_t usec) { return uavcan::MonotonicDuration::fromUSec(usec); }
+
+template <typename T>
+static bool areTimestampsClose(const T& a, const T& b, int64_t precision_usec = 100000)
+{
+ return (a - b).getAbs().toUSec() < precision_usec;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/data_type.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/data_type.hpp>
+
+
+TEST(DataTypeSignatureCRC, Correctness)
+{
+ uavcan::DataTypeSignatureCRC crc;
+
+ ASSERT_EQ(0xFFFFFFFFFFFFFFFF ^ 0xFFFFFFFFFFFFFFFF, crc.get());
+
+ crc.add('1');
+ crc.add('2');
+ crc.add('3');
+ crc.add(reinterpret_cast<const uint8_t*>("456789"), 6);
+
+ ASSERT_EQ(0x62EC59E3F1A4F00A, crc.get());
+}
+
+
+TEST(DataTypeSignatureCRC, Extension)
+{
+ uavcan::DataTypeSignatureCRC crc1;
+
+ crc1.add('1');
+ crc1.add('2');
+ crc1.add('3');
+
+ uavcan::DataTypeSignatureCRC crc2 = uavcan::DataTypeSignatureCRC::extend(crc1.get());
+
+ crc2.add(reinterpret_cast<const uint8_t*>("456789"), 6);
+
+ ASSERT_EQ(0x62EC59E3F1A4F00A, crc2.get());
+}
+
+
+TEST(DataTypeSignature, Correctness)
+{
+ using uavcan::DataTypeSignature;
+ using uavcan::DataTypeSignatureCRC;
+
+ DataTypeSignature signature;
+ ASSERT_EQ(0, signature.get());
+
+ /*
+ * First extension
+ */
+ signature.extend(DataTypeSignature(0x123456789abcdef0));
+
+ DataTypeSignatureCRC crc;
+ crc.add(0xF0);
+ crc.add(0xDE);
+ crc.add(0xBC);
+ crc.add(0x9A);
+ crc.add(0x78);
+ crc.add(0x56);
+ crc.add(0x34);
+ crc.add(0x12);
+ for (int i = 0; i < 8; i++)
+ {
+ crc.add(0);
+ }
+
+ ASSERT_EQ(crc.get(), signature.get());
+
+ const uint64_t old_signature = signature.get();
+
+ /*
+ * Second extension
+ */
+ signature.extend(DataTypeSignature(0xfedcba9876543210));
+ crc.add(0x10);
+ crc.add(0x32);
+ crc.add(0x54);
+ crc.add(0x76);
+ crc.add(0x98);
+ crc.add(0xba);
+ crc.add(0xdc);
+ crc.add(0xfe);
+ for (int i = 0; i < 64; i += 8)
+ {
+ crc.add((old_signature >> i) & 0xFF);
+ }
+
+ ASSERT_EQ(crc.get(), signature.get());
+
+ /*
+ * Comparison
+ */
+ ASSERT_TRUE(signature == DataTypeSignature(signature.get()));
+ ASSERT_FALSE(signature == DataTypeSignature());
+ ASSERT_FALSE(signature != DataTypeSignature(signature.get()));
+ ASSERT_TRUE(signature != DataTypeSignature());
+}
+
+
+TEST(DataTypeDescriptor, ToString)
+{
+ uavcan::DataTypeDescriptor desc;
+ ASSERT_EQ(":65535s:0000000000000000", desc.toString());
+
+ desc = uavcan::DataTypeDescriptor(uavcan::DataTypeKindMessage, 123,
+ uavcan::DataTypeSignature(0xdeadbeef1234), "Bar");
+ ASSERT_EQ("Bar:123m:0000deadbeef1234", desc.toString());
+
+ // Max length - 80 chars
+ desc = uavcan::DataTypeDescriptor(uavcan::DataTypeKindMessage, 1023, uavcan::DataTypeSignature(0xdeadbeef12345678),
+ "sirius_cybernetics_corporation.marvin.model_a.LongDataTypeName123456789abcdefghi");
+ ASSERT_EQ("sirius_cybernetics_corporation.marvin.model_a.LongDataTypeName123456789abcdefghi:1023m:deadbeef12345678",
+ desc.toString());
+}
+
+
+TEST(DataTypeDescriptor, Match)
+{
+ uavcan::DataTypeDescriptor desc(uavcan::DataTypeKindMessage, 123,
+ uavcan::DataTypeSignature(0xdeadbeef1234), "namespace.TypeName");
+ ASSERT_TRUE(desc.match(uavcan::DataTypeKindMessage, "namespace.TypeName"));
+ ASSERT_FALSE(desc.match(uavcan::DataTypeKindMessage, "boo"));
+ ASSERT_FALSE(desc.match(uavcan::DataTypeKindService, "namespace.TypeName"));
+}
+
+
+TEST(DataTypeID, Basic)
+{
+ uavcan::DataTypeID id;
+
+ ASSERT_EQ(0xFFFF, id.get());
+ ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage));
+ ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindService));
+
+ id = 123;
+ uavcan::DataTypeID id2 = 255;
+
+ ASSERT_EQ(123, id.get());
+ ASSERT_EQ(255, id2.get());
+
+ ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage));
+ ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindService));
+ ASSERT_TRUE(id2.isValidForDataTypeKind(uavcan::DataTypeKindMessage));
+ ASSERT_TRUE(id2.isValidForDataTypeKind(uavcan::DataTypeKindService));
+
+ ASSERT_TRUE(id < id2);
+ ASSERT_TRUE(id <= id2);
+ ASSERT_TRUE(id2 > id);
+ ASSERT_TRUE(id2 >= id);
+ ASSERT_TRUE(id != id2);
+
+ id = id2;
+ ASSERT_FALSE(id < id2);
+ ASSERT_TRUE(id <= id2);
+ ASSERT_FALSE(id2 > id);
+ ASSERT_TRUE(id2 >= id);
+ ASSERT_TRUE(id == id2);
+
+ id = 1024;
+ ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage));
+ ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindService));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/dsdl_test/dsdl_const_1.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/Timestamp.hpp>
+
+/*
+ * Objective of this test is to make sure that the generated types are being linked correcly.
+ * This test requests the address of some static const member variables to make sure that there
+ * will be no duplicated symbol linking errors.
+ */
+TEST(DsdlConst1, Timestamp)
+{
+ using uavcan::Timestamp;
+
+ std::cout << &Timestamp::UNKNOWN << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/dsdl_test/dsdl_const_2.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/Timestamp.hpp>
+
+
+TEST(DsdlConst2, Timestamp)
+{
+ using uavcan::Timestamp;
+
+ std::cout << &Timestamp::UNKNOWN << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/dsdl_test/dsdl_test.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+# pragma GCC diagnostic ignored "-Wfloat-equal"
+#endif
+
+#include <gtest/gtest.h>
+#include <uavcan/transport/transfer_buffer.hpp>
+#include <limits>
+#include <root_ns_a/EmptyService.hpp>
+#include <root_ns_a/EmptyMessage.hpp>
+#include <root_ns_a/NestedMessage.hpp>
+#include <root_ns_a/A.hpp>
+#include <root_ns_a/ReportBackSoldier.hpp>
+#include <root_ns_b/ServiceWithEmptyRequest.hpp>
+#include <root_ns_b/ServiceWithEmptyResponse.hpp>
+#include <root_ns_b/T.hpp>
+
+
+TEST(Dsdl, EmptyServices)
+{
+ uavcan::StaticTransferBuffer<100> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ root_ns_b::ServiceWithEmptyRequest::Request req;
+ ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Request::encode(req, sc_wr));
+ ASSERT_EQ("", bs_wr.toString());
+
+ root_ns_b::ServiceWithEmptyRequest::Response resp;
+ ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Response::encode(resp, sc_wr));
+ ASSERT_EQ("", bs_wr.toString());
+
+ resp.covariance.push_back(-2);
+ resp.covariance.push_back(65504);
+ root_ns_b::ServiceWithEmptyRequest::Response::encode(resp, sc_wr);
+ ASSERT_EQ("00000000 11000000 11111111 01111011", bs_wr.toString());
+
+ resp.covariance.push_back(42);
+ resp.covariance[0] = 999;
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Response::decode(resp, sc_rd));
+
+ ASSERT_EQ(2, resp.covariance.size());
+ ASSERT_EQ(-2, resp.covariance[0]);
+ ASSERT_EQ(65504, resp.covariance[1]);
+}
+
+
+TEST(Dsdl, Signature)
+{
+ ASSERT_EQ(0xe74617107a34aa9c, root_ns_a::EmptyService::getDataTypeSignature().get());
+ ASSERT_STREQ("root_ns_a.EmptyService", root_ns_a::EmptyService::getDataTypeFullName());
+ ASSERT_EQ(uavcan::DataTypeKindService, root_ns_a::EmptyService::DataTypeKind);
+
+ ASSERT_EQ(0x99604d7066e0d713, root_ns_a::NestedMessage::getDataTypeSignature().get()); // Computed manually
+ ASSERT_STREQ("root_ns_a.NestedMessage", root_ns_a::NestedMessage::getDataTypeFullName());
+ ASSERT_EQ(uavcan::DataTypeKindMessage, root_ns_a::NestedMessage::DataTypeKind);
+}
+
+
+TEST(Dsdl, Operators)
+{
+ {
+ root_ns_a::EmptyService::Request a, b;
+ ASSERT_TRUE(a == b);
+ ASSERT_FALSE(a != b);
+ }
+ {
+ root_ns_a::NestedMessage c, d;
+ ASSERT_TRUE(c == d);
+ ASSERT_FALSE(c != d);
+
+ c.field = 1;
+ ASSERT_FALSE(c == d);
+ ASSERT_TRUE(c != d);
+ }
+}
+
+
+TEST(Dsdl, CloseComparison)
+{
+ root_ns_a::A first, second;
+
+ ASSERT_TRUE(first == second);
+
+ first.vector[1].vector[1] = std::numeric_limits<double>::epsilon();
+ ASSERT_TRUE(first.isClose(second)); // Still close
+ ASSERT_FALSE(first == second); // But not exactly
+
+ first.vector[1].vector[1] = std::numeric_limits<float>::epsilon();
+ ASSERT_FALSE(first.isClose(second)); // Nope
+ ASSERT_FALSE(first == second); // Ditto
+}
+
+/*
+ * This test assumes that it will be executed before other GDTR tests; otherwise it fails.
+ * TODO: Probably it needs to be called directly from main()
+ */
+//TEST(Dsdl, Registration)
+//{
+// using uavcan::GlobalDataTypeRegistry;
+// /*
+// * Descriptors
+// */
+// const uavcan::DataTypeDescriptor* desc = UAVCAN_NULLPTR;
+//
+// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "root_ns_a.EmptyMessage");
+// ASSERT_TRUE(desc);
+// ASSERT_EQ(root_ns_a::EmptyMessage::DefaultDataTypeID, desc->getID());
+// ASSERT_EQ(root_ns_a::EmptyMessage::DataTypeKind, desc->getKind());
+// ASSERT_EQ(root_ns_a::EmptyMessage::getDataTypeSignature(), desc->getSignature());
+// ASSERT_STREQ(root_ns_a::EmptyMessage::getDataTypeFullName(), desc->getFullName());
+//
+// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "root_ns_a.EmptyService");
+// ASSERT_TRUE(desc);
+// ASSERT_EQ(root_ns_a::EmptyService::DefaultDataTypeID, desc->getID());
+// ASSERT_EQ(root_ns_a::EmptyService::DataTypeKind, desc->getKind());
+// ASSERT_EQ(root_ns_a::EmptyService::getDataTypeSignature(), desc->getSignature());
+// ASSERT_STREQ(root_ns_a::EmptyService::getDataTypeFullName(), desc->getFullName());
+//
+// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "root_ns_a.ReportBackSoldier");
+// ASSERT_TRUE(desc);
+// ASSERT_EQ(root_ns_a::ReportBackSoldier::DefaultDataTypeID, desc->getID());
+// ASSERT_EQ(root_ns_a::ReportBackSoldier::DataTypeKind, desc->getKind());
+// ASSERT_EQ(root_ns_a::ReportBackSoldier::getDataTypeSignature(), desc->getSignature());
+// ASSERT_STREQ(root_ns_a::ReportBackSoldier::getDataTypeFullName(), desc->getFullName());
+//
+// /*
+// * Mask
+// */
+// uavcan::DataTypeIDMask mask;
+//
+// GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, mask);
+// ASSERT_TRUE(mask[8]);
+// mask[8] = false;
+//
+// GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, mask);
+// ASSERT_TRUE(mask[1]);
+// ASSERT_TRUE(mask[3]);
+// mask[1] = mask[3] = false;
+//
+// /*
+// * Reset
+// */
+// GlobalDataTypeRegistry::instance().reset();
+// ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen());
+//}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,260 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+
+#include <uavcan/helpers/ostream.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+#include <uavcan/Timestamp.hpp>
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/GetTransportStats.hpp>
+#include <uavcan/protocol/Panic.hpp>
+#include <uavcan/protocol/RestartNode.hpp>
+#include <uavcan/protocol/GlobalTimeSync.hpp>
+#include <uavcan/protocol/DataTypeKind.hpp>
+#include <uavcan/protocol/GetDataTypeInfo.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+#include <uavcan/protocol/debug/LogMessage.hpp>
+#include <uavcan/protocol/debug/KeyValue.hpp>
+
+#include <root_ns_a/Deep.hpp>
+#include <root_ns_a/UnionTest.hpp>
+
+template <typename T>
+static bool validateYaml(const T& obj, const std::string& reference)
+{
+ uavcan::OStream::instance() << "Validating YAML:\n" << obj << "\n" << uavcan::OStream::endl;
+
+ std::ostringstream os;
+ os << obj;
+ if (os.str() == reference)
+ {
+ return true;
+ }
+ else
+ {
+ std::cout << "INVALID YAML:\n"
+ << "EXPECTED:\n"
+ << "===\n"
+ << reference
+ << "\n===\n"
+ << "ACTUAL:\n"
+ << "\n===\n"
+ << os.str()
+ << "\n===\n" << std::endl;
+ return false;
+ }
+}
+
+TEST(Dsdl, Streaming)
+{
+ EXPECT_TRUE(validateYaml(uavcan::protocol::GetNodeInfo::Response(),
+ "status: \n"
+ " uptime_sec: 0\n"
+ " health: 0\n"
+ " mode: 0\n"
+ " sub_mode: 0\n"
+ " vendor_specific_status_code: 0\n"
+ "software_version: \n"
+ " major: 0\n"
+ " minor: 0\n"
+ " optional_field_flags: 0\n"
+ " vcs_commit: 0\n"
+ " image_crc: 0\n"
+ "hardware_version: \n"
+ " major: 0\n"
+ " minor: 0\n"
+ " unique_id: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n"
+ " certificate_of_authenticity: \"\"\n"
+ "name: \"\""));
+
+ root_ns_a::Deep ps;
+ ps.a.resize(1);
+ EXPECT_TRUE(validateYaml(ps,
+ "c: 0\n"
+ "str: \"\"\n"
+ "a: \n"
+ " - \n"
+ " scalar: 0\n"
+ " vector: \n"
+ " - \n"
+ " vector: [0, 0]\n"
+ " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n"
+ " - \n"
+ " vector: [0, 0]\n"
+ " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n"
+ "b: \n"
+ " - \n"
+ " vector: [0, 0]\n"
+ " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n"
+ " - \n"
+ " vector: [0, 0]\n"
+ " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]"));
+}
+
+
+template <typename T>
+static bool encodeDecodeValidate(const T& obj, const std::string& reference_bit_string)
+{
+ uavcan::StaticTransferBuffer<256> buf;
+
+ {
+ uavcan::BitStream bits(buf);
+ uavcan::ScalarCodec codec(bits);
+ /*
+ * Coding
+ */
+ if (0 > T::encode(obj, codec))
+ {
+ std::cout << "Failed to encode" << std::endl;
+ return false;
+ }
+
+ /*
+ * Validating the encoded bitstream
+ */
+ const std::string result = bits.toString();
+ if (result != reference_bit_string)
+ {
+ std::cout << "ENCODED VALUE DOESN'T MATCH THE REFERENCE:\nEXPECTED:\n"
+ << reference_bit_string << "\nACTUAL:\n"
+ << result << std::endl;
+ return false;
+ }
+ }
+
+ /*
+ * Decoding back and comparing
+ */
+ uavcan::BitStream bits(buf);
+ uavcan::ScalarCodec codec(bits);
+
+ T decoded;
+
+ if (0 > T::decode(decoded, codec))
+ {
+ std::cout << "Failed to decode" << std::endl;
+ return false;
+ }
+
+ if (!decoded.isClose(obj))
+ {
+ std::cout << "DECODED OBJECT DOESN'T MATCH THE REFERENCE:\nEXPECTED:\n"
+ << obj << "\nACTUAL:\n"
+ << decoded << std::endl;
+ return false;
+ }
+ return true;
+}
+
+
+TEST(Dsdl, Union)
+{
+ using root_ns_a::UnionTest;
+ using root_ns_a::NestedInUnion;
+
+ ASSERT_EQ(3, UnionTest::MinBitLen);
+ ASSERT_EQ(16, UnionTest::MaxBitLen);
+ ASSERT_EQ(13, NestedInUnion::MinBitLen);
+ ASSERT_EQ(13, NestedInUnion::MaxBitLen);
+
+ UnionTest s;
+
+ EXPECT_TRUE(validateYaml(s, "z: "));
+ encodeDecodeValidate(s, "00000000");
+
+ s.to<UnionTest::Tag::a>() = 16;
+ EXPECT_TRUE(validateYaml(s, "a: 16"));
+ EXPECT_TRUE(encodeDecodeValidate(s, "00110000"));
+
+ s.to<UnionTest::Tag::b>() = 31;
+ EXPECT_TRUE(validateYaml(s, "b: 31"));
+ EXPECT_TRUE(encodeDecodeValidate(s, "01011111"));
+
+ s.to<UnionTest::Tag::c>() = 256;
+ EXPECT_TRUE(validateYaml(s, "c: 256"));
+ EXPECT_TRUE(encodeDecodeValidate(s, "01100000 00000001"));
+
+ s.to<UnionTest::Tag::d>().push_back(true);
+ s.to<UnionTest::Tag::d>().push_back(false);
+ s.to<UnionTest::Tag::d>().push_back(true);
+ s.to<UnionTest::Tag::d>().push_back(true);
+ s.to<UnionTest::Tag::d>().push_back(false);
+ s.to<UnionTest::Tag::d>().push_back(false);
+ s.to<UnionTest::Tag::d>().push_back(true);
+ s.to<UnionTest::Tag::d>().push_back(true);
+ s.to<UnionTest::Tag::d>().push_back(true);
+ ASSERT_EQ(9, s.to<UnionTest::Tag::d>().size());
+ EXPECT_TRUE(validateYaml(s, "d: [1, 0, 1, 1, 0, 0, 1, 1, 1]"));
+ EXPECT_TRUE(encodeDecodeValidate(s, "10010011 01100111"));
+
+ s.to<UnionTest::Tag::e>().array[0] = 0;
+ s.to<UnionTest::Tag::e>().array[1] = 1;
+ s.to<UnionTest::Tag::e>().array[2] = 2;
+ s.to<UnionTest::Tag::e>().array[3] = 3;
+ EXPECT_TRUE(validateYaml(s, "e: \n array: [0, 1, 2, 3]"));
+ EXPECT_TRUE(encodeDecodeValidate(s, "10100000 11011000"));
+}
+
+
+TEST(Dsdl, ParamGetSetRequestUnion)
+{
+ uavcan::protocol::param::GetSet::Request req;
+
+ req.index = 8191;
+ req.name = "123"; // 49, 50, 51 // 00110001, 00110010, 00110011
+ EXPECT_TRUE(encodeDecodeValidate(req, "11111111 11111000 00110001 00110010 00110011"));
+
+ req.value.to<uavcan::protocol::param::Value::Tag::string_value>() = "abc"; // 01100001, 01100010, 01100011
+ EXPECT_TRUE(encodeDecodeValidate(req,
+ "11111111 11111100 " // Index, Union tag
+ "00000011 " // Array length
+ "01100001 01100010 01100011 " // Payload
+ "00110001 00110010 00110011")); // Name
+
+ EXPECT_TRUE(validateYaml(req,
+ "index: 8191\n"
+ "value: \n"
+ " string_value: \"abc\"\n"
+ "name: \"123\""));
+
+ req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = 1;
+ EXPECT_TRUE(encodeDecodeValidate(req,
+ "11111111 11111001 " // Index, Union tag
+ "00000001 00000000 00000000 00000000 00000000 00000000 00000000 00000000 " // Payload
+ "00110001 00110010 00110011")); // Name
+}
+
+
+TEST(Dsdl, ParamGetSetResponseUnion)
+{
+ uavcan::protocol::param::GetSet::Response res;
+
+ res.value.to<uavcan::protocol::param::Value::Tag::string_value>() = "abc";
+ res.default_value.to<uavcan::protocol::param::Value::Tag::string_value>(); // Empty
+ res.name = "123";
+ EXPECT_TRUE(encodeDecodeValidate(res,
+ "00000100 " // Value union tag
+ "00000011 " // Value array length
+ "01100001 01100010 01100011 " // Value array payload
+ "00000100 " // Default union tag
+ "00000000 " // Default array length
+ "00000000 " // Max value tag
+ "00000000 " // Min value tag
+ "00110001 00110010 00110011")); // Name
+
+ res.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = true;
+ res.default_value.to<uavcan::protocol::param::Value::Tag::boolean_value>(); // False
+ res.name = "123";
+ EXPECT_TRUE(encodeDecodeValidate(res,
+ "00000011 " // Value union tag
+ "00000001 " // Value
+ "00000011 " // Default union tag
+ "00000000 " // Default value
+ "00000000 " // Max value tag
+ "00000000 " // Min value tag
+ "00110001 00110010 00110011")); // Name
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,1 @@ +--- \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,9 @@ +bool TRUE = true +bool NOT_TRUE = false * true +uint64 BIG = (1 << 64) - 1 +float64 FLOAT_CONSTANT = 3.14 +uint8[<128] string_request +--- +float64 FLOAT_CONSTANT = 1.79769313486231570815e+308 +root_ns_b.SuperIntelligentShadeOfBlue blue +uint8[<128] string_response
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,10 @@ +# +# This thing is only needed for testing +# + +uint8 seq +uint8 sysid +uint8 compid +uint8 msgid + +uint8[<256] payload
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,1 @@ +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,3 @@ +uint8[<=64] string_request +--- +uint8[<=64] string_response
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/A.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,8 @@ +# +# Nested type. +# Energy and capacity are expressed in watt hours (Joules are infeasible because of range limitations). +# Unknown values should be represented as NAN. +# + +float32 scalar +B[2] vector
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/B.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,2 @@ +float64[2] vector +bool[16] bools \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,8 @@ +# +# Info for all battery packs of the primary power supply. +# + +bool c +uint8[<20] str +A[<2] a +B[2] b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,4 @@ +# This should be 13 bits long +void2 # 2 bits +uint2[4] array # 8 bits +void3 # 3 bits
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,5 @@ + +float32 VALUE = 2 +( 2 *2) +bool BOOLEAN = true + false +int2 field +EmptyMessage empty
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,8 @@ +# Max bit len 16 bits +@union # 3 bits +Empty z +uint5 a +uint5 b # Conflicting with a +uint13 c +bool[<=9] d # 4 bit length field + 9 bit payload +NestedInUnion e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,2 @@ +--- +float16[<=9] covariance
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,2 @@ +float16[<=9] covariance +---
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,2 @@ +float16[<32] array_f16 +root_ns_a.NestedMessage[3] nested_message
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan/test/dsdl_test/root_ns_b/T.uavcan Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,2 @@ +bool T = true +bool F = false \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/dynamic_memory.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/dynamic_memory.hpp>
+
+TEST(DynamicMemory, Basic)
+{
+ uavcan::PoolAllocator<128, 32> pool32;
+ EXPECT_EQ(4, pool32.getNumFreeBlocks());
+ EXPECT_EQ(0, pool32.getPeakNumUsedBlocks());
+ const void* ptr1 = pool32.allocate(16);
+ ASSERT_TRUE(ptr1);
+ EXPECT_EQ(1, pool32.getNumUsedBlocks());
+ EXPECT_FALSE(pool32.allocate(120));
+ EXPECT_EQ(1, pool32.getNumUsedBlocks());
+ pool32.deallocate(ptr1);
+ EXPECT_EQ(0, pool32.getNumUsedBlocks());
+ EXPECT_EQ(1, pool32.getPeakNumUsedBlocks());
+}
+
+TEST(DynamicMemory, OutOfMemory)
+{
+ uavcan::PoolAllocator<64, 32> pool32;
+
+ EXPECT_EQ(2, pool32.getNumFreeBlocks());
+ EXPECT_EQ(0, pool32.getNumUsedBlocks());
+ EXPECT_EQ(0, pool32.getPeakNumUsedBlocks());
+
+ const void* ptr1 = pool32.allocate(32);
+ ASSERT_TRUE(ptr1);
+ EXPECT_EQ(1, pool32.getNumUsedBlocks());
+ EXPECT_EQ(1, pool32.getPeakNumUsedBlocks());
+
+ const void* ptr2 = pool32.allocate(32);
+ ASSERT_TRUE(ptr2);
+ EXPECT_EQ(2, pool32.getNumUsedBlocks());
+ EXPECT_EQ(2, pool32.getPeakNumUsedBlocks());
+
+ ASSERT_FALSE(pool32.allocate(32)); // No free blocks left --> UAVCAN_NULLPTR
+ EXPECT_EQ(2, pool32.getNumUsedBlocks());
+ EXPECT_EQ(0, pool32.getNumFreeBlocks());
+ EXPECT_EQ(2, pool32.getPeakNumUsedBlocks());
+}
+
+TEST(DynamicMemory, LimitedPoolAllocator)
+{
+ uavcan::PoolAllocator<128, 32> pool32;
+ uavcan::LimitedPoolAllocator lim(pool32, 2);
+
+ EXPECT_EQ(2, lim.getBlockCapacity());
+ EXPECT_EQ(0, pool32.getPeakNumUsedBlocks());
+
+ const void* ptr1 = lim.allocate(1);
+ const void* ptr2 = lim.allocate(1);
+ const void* ptr3 = lim.allocate(1);
+
+ EXPECT_TRUE(ptr1);
+ EXPECT_TRUE(ptr2);
+ EXPECT_FALSE(ptr3);
+
+ lim.deallocate(ptr2);
+ const void* ptr4 = lim.allocate(1);
+ lim.deallocate(ptr1);
+ const void* ptr5 = lim.allocate(1);
+ const void* ptr6 = lim.allocate(1);
+
+ EXPECT_TRUE(ptr4);
+ EXPECT_TRUE(ptr5);
+ EXPECT_FALSE(ptr6);
+
+ EXPECT_EQ(2, pool32.getPeakNumUsedBlocks());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/helpers/heap_based_pool_allocator.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
+#include <malloc.h>
+
+
+TEST(HeapBasedPoolAllocator, Basic)
+{
+ std::cout << ">>> HEAP BEFORE:" << std::endl;
+ malloc_stats();
+
+ uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize> al(0xEEEE);
+
+ ASSERT_EQ(0, al.getNumReservedBlocks());
+ ASSERT_EQ(0, al.getNumAllocatedBlocks());
+
+ ASSERT_EQ(0xEEEE, al.getBlockCapacity());
+ ASSERT_EQ(0xFFFF, al.getBlockCapacityHardLimit());
+
+ void* a = al.allocate(10);
+ void* b = al.allocate(10);
+ void* c = al.allocate(10);
+ void* d = al.allocate(10);
+
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(4, al.getNumAllocatedBlocks());
+
+ al.deallocate(a);
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(3, al.getNumAllocatedBlocks());
+
+ al.deallocate(b);
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(2, al.getNumAllocatedBlocks());
+
+ al.deallocate(c);
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(1, al.getNumAllocatedBlocks());
+
+ a = al.allocate(10);
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(2, al.getNumAllocatedBlocks());
+ ASSERT_EQ(c, a);
+
+ al.deallocate(a);
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(1, al.getNumAllocatedBlocks());
+
+ al.shrink();
+ ASSERT_EQ(1, al.getNumReservedBlocks());
+ ASSERT_EQ(1, al.getNumAllocatedBlocks());
+
+ al.deallocate(d);
+ ASSERT_EQ(1, al.getNumReservedBlocks());
+ ASSERT_EQ(0, al.getNumAllocatedBlocks());
+
+ al.shrink();
+ ASSERT_EQ(0, al.getNumReservedBlocks());
+ ASSERT_EQ(0, al.getNumAllocatedBlocks());
+
+ std::cout << ">>> HEAP AFTER:" << std::endl;
+ malloc_stats();
+}
+
+
+TEST(HeapBasedPoolAllocator, Limits)
+{
+ uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize> al(2);
+
+ ASSERT_EQ(2, al.getBlockCapacity());
+ ASSERT_EQ(4, al.getBlockCapacityHardLimit());
+
+ ASSERT_EQ(0, al.getNumReservedBlocks());
+ ASSERT_EQ(0, al.getNumAllocatedBlocks());
+
+ void* a = al.allocate(10);
+ void* b = al.allocate(10);
+ void* c = al.allocate(10);
+ void* d = al.allocate(10);
+
+ ASSERT_TRUE(a);
+ ASSERT_TRUE(b);
+ ASSERT_TRUE(c);
+ ASSERT_TRUE(d);
+
+ ASSERT_FALSE(al.allocate(10));
+
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(4, al.getNumAllocatedBlocks());
+
+ al.deallocate(a);
+ al.deallocate(b);
+ al.deallocate(c);
+ al.deallocate(d);
+
+ ASSERT_EQ(4, al.getNumReservedBlocks());
+ ASSERT_EQ(0, al.getNumAllocatedBlocks());
+}
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+#include <thread>
+#include <mutex>
+
+struct RaiiSynchronizer
+{
+ static std::mutex mutex;
+ std::lock_guard<std::mutex> guard{mutex};
+};
+
+std::mutex RaiiSynchronizer::mutex;
+
+TEST(HeapBasedPoolAllocator, Concurrency)
+{
+ std::cout << ">>> HEAP BEFORE:" << std::endl;
+ malloc_stats();
+
+ uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, RaiiSynchronizer> al(1000);
+
+ ASSERT_EQ(1000, al.getBlockCapacity());
+ ASSERT_EQ(2000, al.getBlockCapacityHardLimit());
+
+ volatile bool terminate = false;
+
+ /*
+ * Starting the testing threads
+ */
+ std::thread threads[3];
+
+ for (auto& x : threads)
+ {
+ x = std::thread([&al, &terminate]()
+ {
+ while (!terminate)
+ {
+ auto a = al.allocate(1);
+ auto b = al.allocate(1);
+ auto c = al.allocate(1);
+ al.deallocate(al.allocate(1));
+ al.deallocate(a);
+ al.deallocate(b);
+ al.deallocate(c);
+ }
+ });
+ }
+
+ /*
+ * Running the threads for some time, then terminating
+ */
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+
+ terminate = true;
+ std::cout << "Terminating workers..." << std::endl;
+
+ for (auto& x : threads)
+ {
+ x.join();
+ }
+ std::cout << "All workers joined" << std::endl;
+
+ /*
+ * Now, there must not be any leaked memory, because the worker threads deallocate everything before completion.
+ */
+ std::cout << "Allocated: " << al.getNumAllocatedBlocks() << std::endl;
+ std::cout << "Reserved: " << al.getNumReservedBlocks() << std::endl;
+
+ std::cout << ">>> HEAP BEFORE SHRINK:" << std::endl;
+ malloc_stats();
+
+ al.shrink();
+
+ std::cout << ">>> HEAP AFTER SHRINK:" << std::endl;
+ malloc_stats();
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/array.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,1419 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+# pragma GCC diagnostic ignored "-Wfloat-equal"
+# pragma GCC diagnostic ignored "-Wdouble-promotion"
+#endif
+
+#include <gtest/gtest.h>
+#include <uavcan/marshal/types.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+using uavcan::Array;
+using uavcan::ArrayModeDynamic;
+using uavcan::ArrayModeStatic;
+using uavcan::IntegerSpec;
+using uavcan::FloatSpec;
+using uavcan::SignednessSigned;
+using uavcan::SignednessUnsigned;
+using uavcan::CastModeSaturate;
+using uavcan::CastModeTruncate;
+
+struct CustomType
+{
+ typedef uavcan::IntegerSpec<8, uavcan::SignednessSigned, uavcan::CastModeTruncate> A;
+ typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> B;
+ // Dynamic array of max len 5 --> 3 bits for len, 5 bits for data --> 1 byte max len
+ typedef uavcan::Array<uavcan::IntegerSpec<1, uavcan::SignednessUnsigned, uavcan::CastModeSaturate>,
+ uavcan::ArrayModeDynamic, 5> C;
+
+ enum { MinBitLen = A::MinBitLen + B::MinBitLen + C::MinBitLen };
+ enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen + C::MaxBitLen };
+
+ typename uavcan::StorageType<A>::Type a;
+ typename uavcan::StorageType<B>::Type b;
+ typename uavcan::StorageType<C>::Type c;
+
+ CustomType()
+ : a()
+ , b()
+ , c()
+ { }
+
+ bool operator==(const CustomType& rhs) const
+ {
+ return a == rhs.a &&
+ uavcan::areFloatsExactlyEqual(b, rhs.b) &&
+ c == rhs.c;
+ }
+
+ static int encode(const CustomType& obj, uavcan::ScalarCodec& codec,
+ uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled)
+ {
+ int res = 0;
+ res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ res = B::encode(obj.b, codec, uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ res = C::encode(obj.c, codec, tao_mode);
+ if (res <= 0)
+ {
+ return res;
+ }
+ return 1;
+ }
+
+ static int decode(CustomType& obj, uavcan::ScalarCodec& codec,
+ uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled)
+ {
+ int res = 0;
+ res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ res = B::decode(obj.b, codec, uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ res = C::decode(obj.c, codec, tao_mode);
+ if (res <= 0)
+ {
+ return res;
+ }
+ return 1;
+ }
+};
+
+
+TEST(Array, Basic)
+{
+ typedef Array<IntegerSpec<8, SignednessSigned, CastModeTruncate>, ArrayModeStatic, 4> A1;
+ typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeStatic, 2> A2;
+ typedef Array<CustomType, ArrayModeStatic, 2> A3;
+
+ A1 a1;
+ A2 a2;
+ A3 a3;
+
+ ASSERT_EQ(1, A3::ValueType::C::RawValueType::BitLen);
+
+ ASSERT_EQ(8 * 4, A1::MaxBitLen);
+ ASSERT_EQ(16 * 2, A2::MaxBitLen);
+ ASSERT_EQ((8 + 16 + 5 + 3) * 2, A3::MaxBitLen);
+
+ /*
+ * Zero initialization check
+ */
+ ASSERT_FALSE(a1.empty());
+ for (A1::const_iterator it = a1.begin(); it != a1.end(); ++it)
+ {
+ ASSERT_EQ(0, *it);
+ }
+
+ ASSERT_FALSE(a2.empty());
+ for (A2::const_iterator it = a2.begin(); it != a2.end(); ++it)
+ {
+ ASSERT_EQ(0, *it);
+ }
+
+ for (A3::const_iterator it = a3.begin(); it != a3.end(); ++it)
+ {
+ ASSERT_EQ(0, it->a);
+ ASSERT_EQ(0, it->b);
+ ASSERT_EQ(0, it->c.size());
+ ASSERT_TRUE(it->c.empty());
+ }
+
+ /*
+ * Modification with known values; array lengths are hard coded.
+ */
+ for (uint8_t i = 0; i < 4; i++)
+ {
+ a1.at(i) = int8_t(i);
+ }
+ for (uint8_t i = 0; i < 2; i++)
+ {
+ a2.at(i) = i;
+ }
+ for (uint8_t i = 0; i < 2; i++)
+ {
+ a3[i].a = int8_t(i);
+ a3[i].b = i;
+ for (uint8_t i2 = 0; i2 < 5; i2++)
+ {
+ a3[i].c.push_back(i2 & 1);
+ }
+ ASSERT_EQ(5, a3[i].c.size());
+ ASSERT_FALSE(a3[i].c.empty());
+ }
+
+ /*
+ * Representation check
+ * Note that TAO in A3 is not possible because A3::C has less than one byte per item
+ */
+ uavcan::StaticTransferBuffer<16> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ ASSERT_EQ(1, A1::encode(a1, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, A2::encode(a2, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled));
+
+ ASSERT_EQ(0, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); // Out of buffer space
+
+ static const std::string Reference =
+ "00000000 00000001 00000010 00000011 " // A1 (0, 1, 2, 3)
+ "00000000 00000000 00000000 00111100 " // A2 (0, 1)
+ "00000000 00000000 00000000 10101010 " // A3[0] (0, 0, bool[5])
+ "00000001 00000000 00111100 10101010"; // A3[1] (1, 1, bool[5])
+
+ ASSERT_EQ(Reference, bs_wr.toString());
+
+ /*
+ * Read back
+ */
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+
+ A1 a1_;
+ A2 a2_;
+ A3 a3_;
+
+ ASSERT_EQ(1, A1::decode(a1_, sc_rd, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, A2::decode(a2_, sc_rd, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled));
+
+ ASSERT_EQ(a1_, a1);
+ ASSERT_EQ(a2_, a2);
+ ASSERT_EQ(a3_, a3);
+
+ for (uint8_t i = 0; i < 4; i++)
+ {
+ ASSERT_EQ(a1[i], a1_[i]);
+ }
+ for (uint8_t i = 0; i < 2; i++)
+ {
+ ASSERT_EQ(a2[i], a2_[i]);
+ }
+ for (uint8_t i = 0; i < 2; i++)
+ {
+ ASSERT_EQ(a3[i].a, a3_[i].a);
+ ASSERT_EQ(a3[i].b, a3_[i].b);
+ ASSERT_EQ(a3[i].c, a3_[i].c);
+ }
+
+ ASSERT_EQ(0, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); // Out of buffer space
+
+ /*
+ * STL compatibility
+ */
+ std::vector<int> v1;
+ v1.push_back(0);
+ v1.push_back(1);
+ v1.push_back(2);
+ v1.push_back(3);
+
+ ASSERT_TRUE(a1 == v1);
+ ASSERT_FALSE(a1 != v1);
+ ASSERT_TRUE(v1 == a1);
+ ASSERT_FALSE(v1 != a1);
+ ASSERT_FALSE(a1 < v1);
+
+ v1[0] = 9000;
+ ASSERT_FALSE(a1 == v1);
+ ASSERT_TRUE(a1 != v1);
+ ASSERT_TRUE(a1 < v1);
+
+ ASSERT_EQ(0, a1.front());
+ ASSERT_EQ(3, a1.back());
+
+ // Boolean vector
+ std::vector<bool> v2;
+ v2.push_back(false);
+ v2.push_back(true);
+ v2.push_back(false);
+ v2.push_back(true);
+ v2.push_back(false);
+
+ ASSERT_TRUE(a3[0].c == v2);
+ ASSERT_FALSE(a3[0].c == v1);
+ ASSERT_FALSE(a3[0].c != v2);
+ ASSERT_TRUE(a3[0].c != v1);
+
+ v2[0] = true;
+ ASSERT_TRUE(a3[0].c != v2);
+ ASSERT_FALSE(a3[0].c == v2);
+}
+
+
+TEST(Array, Dynamic)
+{
+ typedef Array<IntegerSpec<1, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> A;
+ typedef Array<IntegerSpec<8, SignednessSigned, CastModeSaturate>, ArrayModeDynamic, 255> B;
+
+ A a;
+ B b;
+ B b2;
+
+ ASSERT_EQ(3 + 5, A::MaxBitLen);
+ ASSERT_EQ(8 + 255 * 8, B::MaxBitLen);
+
+ ASSERT_TRUE(a.empty());
+ ASSERT_TRUE(b.empty());
+ ASSERT_TRUE(b2.empty());
+
+ {
+ uavcan::StaticTransferBuffer<16> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled));
+
+ ASSERT_EQ("000" "00000 000" "00000", bs_wr.toString()); // Last array was optimized away completely
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+
+ ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled));
+
+ ASSERT_TRUE(a.empty());
+ ASSERT_TRUE(b.empty());
+ ASSERT_TRUE(b2.empty());
+ }
+
+ a.push_back(true);
+ a.push_back(false);
+ a.push_back(true);
+ a.push_back(false);
+ a.push_back(true);
+
+ b.push_back(42);
+ b.push_back(-42);
+
+ b2.push_back(123);
+ b2.push_back(72);
+
+ {
+ uavcan::StaticTransferBuffer<16> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); // No length field
+
+ // A B len B[0] B[1] B2[0] B2[1]
+ ASSERT_EQ("10110101 00000010 00101010 11010110 01111011 01001000", bs_wr.toString());
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+
+ a.clear();
+ b.clear();
+ b2.clear();
+ ASSERT_TRUE(a.empty());
+ ASSERT_TRUE(b.empty());
+ ASSERT_TRUE(b2.empty());
+
+ ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled));
+
+ ASSERT_EQ(5, a.size());
+ ASSERT_EQ(2, b.size());
+ ASSERT_EQ(2, b2.size());
+
+ ASSERT_TRUE(a[0]);
+ ASSERT_FALSE(a[1]);
+ ASSERT_TRUE(a[2]);
+ ASSERT_FALSE(a[3]);
+ ASSERT_TRUE(a[4]);
+
+ ASSERT_EQ(42, b[0]);
+ ASSERT_EQ(-42, b[1]);
+
+ ASSERT_EQ(123, b2[0]);
+ ASSERT_EQ(72, b2[1]);
+ }
+
+ ASSERT_FALSE(a == b);
+ ASSERT_FALSE(b == a);
+ ASSERT_TRUE(a != b);
+ ASSERT_TRUE(b != a);
+
+ a.resize(0);
+ b.resize(0);
+ ASSERT_TRUE(a.empty());
+ ASSERT_TRUE(b.empty());
+
+ a.resize(5, true);
+ b.resize(255, 72);
+ ASSERT_EQ(5, a.size());
+ ASSERT_EQ(255, b.size());
+
+ for (uint8_t i = 0; i < 5; i++)
+ {
+ ASSERT_TRUE(a[i]);
+ }
+ for (uint8_t i = 0; i < 255; i++)
+ {
+ ASSERT_EQ(72, b[i]);
+ }
+}
+
+
+template <typename B>
+struct CustomType2
+{
+ typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> A;
+
+ enum { MinBitLen = A::MinBitLen + B::MinBitLen };
+ enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen };
+
+ typename uavcan::StorageType<A>::Type a;
+ typename uavcan::StorageType<B>::Type b;
+
+ CustomType2()
+ : a()
+ , b()
+ { }
+
+ bool operator==(const CustomType2& rhs) const
+ {
+ return uavcan::areFloatsExactlyEqual(a, rhs.a) &&
+ b == rhs.b;
+ }
+
+ static int encode(const CustomType2& obj, uavcan::ScalarCodec& codec,
+ uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled)
+ {
+ int res = 0;
+ res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ res = B::encode(obj.b, codec, tao_mode);
+ if (res <= 0)
+ {
+ return res;
+ }
+ return 1;
+ }
+
+ static int decode(CustomType2& obj, uavcan::ScalarCodec& codec,
+ uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled)
+ {
+ int res = 0;
+ res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled);
+ if (res <= 0)
+ {
+ return res;
+ }
+ res = B::decode(obj.b, codec, tao_mode);
+ if (res <= 0)
+ {
+ return res;
+ }
+ return 1;
+ }
+};
+
+
+template <typename T>
+static std::string runEncodeDecode(const typename uavcan::StorageType<T>::Type& value,
+ const uavcan::TailArrayOptimizationMode tao_mode)
+{
+ uavcan::StaticTransferBuffer<(T::MaxBitLen + 7) / 8> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+ EXPECT_EQ(1, T::encode(value, sc_wr, tao_mode));
+
+ typename uavcan::StorageType<T>::Type value2 = typename uavcan::StorageType<T>::Type();
+ // Decode multiple times to make sure that the decoded type is being correctly de-initialized
+ for (int i = 0; i < 3; i++)
+ {
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ EXPECT_EQ(1, T::decode(value2, sc_rd, tao_mode));
+ EXPECT_EQ(value, value2);
+ }
+ return bs_wr.toString();
+}
+
+
+TEST(Array, TailArrayOptimization)
+{
+ typedef Array<IntegerSpec<1, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> OneBitArray;
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 255> EightBitArray;
+ typedef CustomType2<Array<OneBitArray, ArrayModeDynamic, 255> > A;
+ typedef CustomType2<Array<EightBitArray, ArrayModeDynamic, 255> > B;
+ typedef CustomType2<EightBitArray> C;
+
+ A a;
+ B b;
+ C c;
+
+ /*
+ * Empty
+ */
+ // a LSB a MSB b len
+ ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled));
+ ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled));
+
+ // a LSB a MSB b len
+ ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<B>(b, uavcan::TailArrayOptEnabled));
+ ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<B>(b, uavcan::TailArrayOptDisabled));
+
+ // a LSB a MSB
+ ASSERT_EQ("00000000 00000000", runEncodeDecode<C>(c, uavcan::TailArrayOptEnabled));
+ ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<C>(c, uavcan::TailArrayOptDisabled));
+
+ /*
+ * A
+ */
+ a.b.resize(2);
+ a.b[0].push_back(true);
+ a.b[0].push_back(false);
+ // a.b[1] remains empty
+ // a LSB a MSB b len b: len(2), 1, 0, len(0)
+ ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled));
+ ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled));
+
+ /*
+ * B
+ */
+ b.b.resize(3);
+ b.b[0].push_back(42);
+ b.b[0].push_back(72);
+ // b.b[1] remains empty
+ b.b[2].push_back(123);
+ b.b[2].push_back(99);
+ // a LSB a MSB b len b[0]len 42 72 b[1]len 123 99 (b[2] len optimized out)
+ ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 01111011 01100011",
+ runEncodeDecode<B>(b, uavcan::TailArrayOptEnabled));
+ // Same as above, but b[2] len is present v here v
+ ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 00000010 01111011 01100011",
+ runEncodeDecode<B>(b, uavcan::TailArrayOptDisabled));
+
+ /*
+ * C
+ */
+ c.a = 1;
+ c.b.push_back(1);
+ c.b.push_back(2);
+ c.b.push_back(3);
+ // a LSB a MSB 1 2 3
+ ASSERT_EQ("00000000 00111100 00000001 00000010 00000011",
+ runEncodeDecode<C>(c, uavcan::TailArrayOptEnabled));
+ // a LSB a MSB b len 1 2 3
+ ASSERT_EQ("00000000 00111100 00000011 00000001 00000010 00000011",
+ runEncodeDecode<C>(c, uavcan::TailArrayOptDisabled));
+}
+
+
+TEST(Array, TailArrayOptimizationErrors)
+{
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> A;
+
+ A a;
+ ASSERT_TRUE(a.empty());
+ ASSERT_EQ("", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled));
+ ASSERT_EQ("00000000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled));
+
+ // Correct decode/encode
+ a.push_back(1);
+ a.push_back(126);
+ a.push_back(5);
+ ASSERT_FALSE(a.empty());
+ ASSERT_EQ("00000001 01111110 00000101", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled));
+ ASSERT_EQ("01100000 00101111 11000000 10100000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled));
+
+ // Invalid decode - length field is out of range
+ uavcan::StaticTransferBuffer<7> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ ASSERT_EQ(1, sc_wr.encode<3>(uint8_t(6))); // Length - more than 5 items, error
+ ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(42)));
+ ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(72)));
+ ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(126)));
+ ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(1)));
+ ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(2)));
+ ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(3))); // Out of range - only 5 items allowed
+
+ // 197 73 15 192 32 ...
+ ASSERT_EQ("11000101 01001001 00001111 11000000 00100000 01000000 01100000", bs_wr.toString());
+
+ {
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ A a2;
+ a2.push_back(56); // Garbage
+ ASSERT_EQ(1, a2.size());
+ // Will fail - declared length is more than 5 items
+ ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptDisabled));
+ // Must be cleared
+ ASSERT_TRUE(a2.empty());
+ }
+ {
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ A a2;
+ a2.push_back(56); // Garbage
+ ASSERT_EQ(1, a2.size());
+ // Will fail - no length field, but the stream is too long
+ ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptEnabled));
+ // Will contain some garbage
+ ASSERT_EQ(5, a2.size());
+ // Interpreted stream - see the values above
+ ASSERT_EQ(197, a2[0]);
+ ASSERT_EQ(73, a2[1]);
+ ASSERT_EQ(15, a2[2]);
+ ASSERT_EQ(192, a2[3]);
+ ASSERT_EQ(32, a2[4]);
+ }
+}
+
+
+TEST(Array, DynamicEncodeDecodeErrors)
+{
+ typedef CustomType2<Array<Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>,
+ ArrayModeDynamic, 255>,
+ ArrayModeDynamic, 255> > A;
+ A a;
+ a.b.resize(2);
+ a.b[0].push_back(55);
+ a.b[0].push_back(66);
+ {
+ uavcan::StaticTransferBuffer<4> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+ ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptEnabled)); // Not enough buffer space
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled));
+ }
+ {
+ uavcan::StaticTransferBuffer<4> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+ ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); // Not enough buffer space
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled));
+ }
+}
+
+
+TEST(Array, StaticEncodeDecodeErrors)
+{
+ typedef CustomType2<Array<Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>,
+ ArrayModeStatic, 2>,
+ ArrayModeStatic, 2> > A;
+ A a;
+ a.a = 1.0;
+ a.b[0][0] = 0x11;
+ a.b[0][1] = 0x22;
+ a.b[1][0] = 0x33;
+ a.b[1][1] = 0x44;
+ { // Just enough buffer space - 6 bytes
+ uavcan::StaticTransferBuffer<6> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+ ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled));
+
+ ASSERT_EQ("00000000 00111100 00010001 00100010 00110011 01000100", bs_wr.toString());
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled));
+ }
+ { // Not enough space
+ uavcan::StaticTransferBuffer<5> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+ ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled));
+
+ ASSERT_EQ("00000000 00111100 00010001 00100010 00110011", bs_wr.toString());
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+ ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled));
+ }
+}
+
+
+TEST(Array, Copyability)
+{
+ typedef Array<IntegerSpec<1, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> OneBitArray;
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 255> EightBitArray;
+ typedef Array<OneBitArray, ArrayModeDynamic, 255> A;
+ typedef Array<EightBitArray, ArrayModeDynamic, 255> B;
+ typedef EightBitArray C;
+
+ A a;
+ B b;
+ C c;
+
+ A a2 = a;
+ B b2 = b;
+ C c2 = c;
+
+ ASSERT_TRUE(a == a2);
+ ASSERT_TRUE(b == b2);
+ ASSERT_TRUE(c == c2);
+
+ a.push_back(OneBitArray());
+ b.push_back(EightBitArray());
+ c.push_back(42);
+
+ ASSERT_TRUE(a != a2);
+ ASSERT_TRUE(b != b2);
+ ASSERT_TRUE(c != c2);
+
+ a2 = a;
+ b2 = b;
+ c2 = c;
+
+ ASSERT_TRUE(a2 == a);
+ ASSERT_TRUE(b2 == b);
+ ASSERT_TRUE(c2 == c);
+}
+
+
+TEST(Array, Appending)
+{
+ typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 2> A;
+ typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 257> B;
+ A a;
+ B b;
+
+ a.push_back(1);
+ a.push_back(2);
+ a += b;
+ ASSERT_EQ(2, a.size());
+ ASSERT_EQ(1, a[0]);
+ ASSERT_EQ(2, a[1]);
+
+ b += a;
+ ASSERT_TRUE(b == a);
+ b += a;
+ ASSERT_EQ(4, b.size());
+ ASSERT_EQ(1, b[0]);
+ ASSERT_EQ(2, b[1]);
+ ASSERT_EQ(1, b[2]);
+ ASSERT_EQ(2, b[3]);
+}
+
+
+TEST(Array, Strings)
+{
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 32> A8;
+ typedef Array<IntegerSpec<7, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 32> A7;
+
+ A8 a8;
+ A8 a8_2;
+ A7 a7;
+
+ ASSERT_TRUE(a8 == a7);
+ // cppcheck-suppress duplicateExpression
+ ASSERT_TRUE(a8 == a8);
+ // cppcheck-suppress duplicateExpression
+ ASSERT_TRUE(a7 == a7);
+ ASSERT_TRUE(a8 == "");
+ ASSERT_TRUE(a7 == "");
+
+ a8 = "Hello world!";
+ a7 = "123";
+ ASSERT_TRUE(a8 == "Hello world!");
+ ASSERT_TRUE(a7 == "123");
+
+ a8 = "Our sun is dying.";
+ a7 = "456";
+ ASSERT_TRUE("Our sun is dying." == a8);
+ ASSERT_TRUE("456" == a7);
+
+ a8 += " 123456";
+ a8 += "-789";
+ ASSERT_TRUE("Our sun is dying. 123456-789" == a8);
+
+ ASSERT_TRUE(a8_2 == "");
+ ASSERT_TRUE(a8_2.empty());
+ ASSERT_TRUE(a8_2 != a8);
+ a8_2 = a8;
+ ASSERT_TRUE(a8_2 == "Our sun is dying. 123456-789");
+ ASSERT_TRUE(a8_2 == a8);
+
+ /*
+ * c_str()
+ */
+ ASSERT_STREQ("", A8().c_str());
+ ASSERT_STREQ("", A7().c_str());
+ ASSERT_STREQ("Our sun is dying. 123456-789", a8_2.c_str());
+ ASSERT_STREQ("Our sun is dying. 123456-789", a8.c_str());
+ ASSERT_STREQ("456", a7.c_str());
+
+ /*
+ * String constructor
+ */
+ A8 a8_3("123");
+ A7 a7_3 = "456";
+ ASSERT_EQ(3, a8_3.size());
+ ASSERT_EQ(3, a7_3.size());
+ ASSERT_STREQ("123", a8_3.c_str());
+ ASSERT_STREQ("456", a7_3.c_str());
+}
+
+
+TEST(Array, AppendFormatted)
+{
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 45> A8;
+
+ A8 a;
+
+ ASSERT_TRUE("" == a);
+
+ a.appendFormatted("%4.1f", 12.3); // 4
+ a += " "; // 1
+ a.appendFormatted("%li", -123456789L); // 10
+ a.appendFormatted("%s", " TOTAL PERSPECTIVE VORTEX "); // 26
+ a.appendFormatted("0x%X", 0xDEADBEEF); // 10 --> 4
+
+ ASSERT_STREQ("12.3 -123456789 TOTAL PERSPECTIVE VORTEX 0xDE", a.c_str());
+}
+
+
+TEST(Array, FlatStreaming)
+{
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 32> A8D;
+ typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 16> AF16D;
+ typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeStatic, 3> AF16S;
+
+ A8D a1;
+ a1 = "12\n3\x44\xa5\xde\xad\x79";
+ uavcan::YamlStreamer<A8D>::stream(std::cout, a1, 0);
+ std::cout << std::endl;
+
+ A8D a2;
+ a2 = "Hello";
+ uavcan::YamlStreamer<A8D>::stream(std::cout, a2, 0);
+ std::cout << std::endl;
+
+ AF16D af16d1;
+ af16d1.push_back(1.23F);
+ af16d1.push_back(4.56F);
+ uavcan::YamlStreamer<AF16D>::stream(std::cout, af16d1, 0);
+ std::cout << std::endl;
+
+ AF16D af16d2;
+ uavcan::YamlStreamer<AF16D>::stream(std::cout, af16d2, 0);
+ std::cout << std::endl;
+
+ AF16S af16s;
+ uavcan::YamlStreamer<AF16S>::stream(std::cout, af16s, 0);
+ std::cout << std::endl;
+}
+
+
+TEST(Array, MultidimensionalStreaming)
+{
+ typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 16> Float16Array;
+ typedef Array<Float16Array, ArrayModeDynamic, 8> TwoDimensional;
+ typedef Array<TwoDimensional, ArrayModeDynamic, 4> ThreeDimensional;
+
+ ThreeDimensional threedee;
+ threedee.resize(3);
+ for (uint8_t x = 0; x < threedee.size(); x++)
+ {
+ threedee[x].resize(3);
+ for (uint8_t y = 0; y < threedee[x].size(); y++)
+ {
+ threedee[x][y].resize(3);
+ for (uint8_t z = 0; z < threedee[x][y].size(); z++)
+ {
+ threedee[x][y][z] = 1.0F / (float(x + y + z) + 1.0F);
+ }
+ }
+ }
+
+ uavcan::YamlStreamer<ThreeDimensional>::stream(std::cout, threedee, 0);
+ std::cout << std::endl;
+}
+
+
+TEST(Array, SquareMatrixPacking)
+{
+ Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s;
+ Array<FloatSpec<32, CastModeSaturate>, ArrayModeDynamic, 4> m2x2f;
+ Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 36> m6x6d;
+
+ // NAN will be reduced to empty array
+ {
+ const double nans3x3[] =
+ {
+ NAN, NAN, NAN,
+ NAN, NAN, NAN,
+ NAN, NAN, NAN
+ };
+ m3x3s.packSquareMatrix(nans3x3);
+ ASSERT_EQ(0, m3x3s.size());
+
+ // Empty array will be decoded as zero matrix
+ double nans3x3_out[9];
+ m3x3s.unpackSquareMatrix(nans3x3_out);
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ ASSERT_DOUBLE_EQ(0, nans3x3_out[i]);
+ }
+ }
+ {
+ std::vector<double> empty;
+ m3x3s.packSquareMatrix(empty);
+ ASSERT_EQ(0, m3x3s.size());
+
+ empty.resize(9);
+ m3x3s.unpackSquareMatrix(empty);
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ ASSERT_DOUBLE_EQ(0, empty.at(i));
+ }
+ }
+
+ // Scalar matrix will be reduced to a single value
+ {
+ std::vector<float> scalar2x2(4);
+ scalar2x2[0] = scalar2x2[3] = 3.14F;
+ m2x2f.packSquareMatrix(scalar2x2);
+ ASSERT_EQ(1, m2x2f.size());
+ ASSERT_FLOAT_EQ(3.14F, m2x2f[0]);
+
+ m2x2f.unpackSquareMatrix(scalar2x2);
+ const float reference[] =
+ {
+ 3.14F, 0.0F,
+ 0.0F, 3.14F
+ };
+ ASSERT_TRUE(std::equal(scalar2x2.begin(), scalar2x2.end(), reference));
+ }
+ {
+ const float scalar6x6[] =
+ {
+ -18, 0, 0, 0, 0, 0,
+ 0, -18, 0, 0, 0, 0,
+ 0, 0, -18, 0, 0, 0,
+ 0, 0, 0, -18, 0, 0,
+ 0, 0, 0, 0, -18, 0,
+ 0, 0, 0, 0, 0, -18
+ };
+ m6x6d.packSquareMatrix(scalar6x6);
+ ASSERT_EQ(1, m6x6d.size());
+ ASSERT_DOUBLE_EQ(-18, m6x6d[0]);
+
+ std::vector<long double> output(36);
+ m6x6d.unpackSquareMatrix(output);
+ ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar6x6));
+ }
+
+ // Diagonal matrix will be reduced to an array of length Width
+ {
+ const float diagonal6x6[] =
+ {
+ 1, 0, 0, 0, 0, 0,
+ 0, -2, 0, 0, 0, 0,
+ 0, 0, 3, 0, 0, 0,
+ 0, 0, 0, -4, 0, 0,
+ 0, 0, 0, 0, 5, 0,
+ 0, 0, 0, 0, 0, -6
+ };
+ m6x6d.packSquareMatrix(diagonal6x6);
+ ASSERT_EQ(6, m6x6d.size());
+ ASSERT_DOUBLE_EQ(1, m6x6d[0]);
+ ASSERT_DOUBLE_EQ(-2, m6x6d[1]);
+ ASSERT_DOUBLE_EQ(3, m6x6d[2]);
+ ASSERT_DOUBLE_EQ(-4, m6x6d[3]);
+ ASSERT_DOUBLE_EQ(5, m6x6d[4]);
+ ASSERT_DOUBLE_EQ(-6, m6x6d[5]);
+
+ std::vector<long double> output(36);
+ m6x6d.unpackSquareMatrix(output);
+ ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6));
+ }
+
+ // A matrix filled with random values will not be compressed
+ {
+ std::vector<float> full3x3(9);
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ full3x3[i] = float(i);
+ }
+ m3x3s.packSquareMatrix(full3x3);
+ ASSERT_EQ(9, m3x3s.size());
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ ASSERT_FLOAT_EQ(float(i), m3x3s[i]);
+ }
+
+ long output[9];
+ m3x3s.unpackSquareMatrix(output);
+ ASSERT_TRUE(std::equal(full3x3.begin(), full3x3.end(), output));
+ }
+
+ // This will be represented as diagonal - NANs are exceptional
+ {
+ const double scalarnan3x3[] =
+ {
+ NAN, 0, 0,
+ 0, NAN, 0,
+ 0, 0, NAN
+ };
+ m3x3s.packSquareMatrix(scalarnan3x3);
+ ASSERT_EQ(3, m3x3s.size());
+ ASSERT_FALSE(m3x3s[0] <= m3x3s[0]); // NAN
+ ASSERT_FALSE(m3x3s[1] <= m3x3s[1]); // NAN
+ ASSERT_FALSE(m3x3s[2] <= m3x3s[2]); // NAN
+
+ float output[9];
+ m3x3s.unpackSquareMatrix(output);
+ ASSERT_FALSE(output[0] <= output[0]); // NAN
+ ASSERT_EQ(0, output[1]);
+ ASSERT_EQ(0, output[2]);
+ ASSERT_EQ(0, output[3]);
+ ASSERT_FALSE(output[4] <= output[4]); // NAN
+ ASSERT_EQ(0, output[5]);
+ ASSERT_EQ(0, output[6]);
+ ASSERT_EQ(0, output[7]);
+ ASSERT_FALSE(output[8] <= output[8]); // NAN
+ }
+
+ // This is a full matrix too (notice the NAN)
+ {
+ const float full2x2[] =
+ {
+ 1, NAN,
+ 0, -2
+ };
+ m2x2f.packSquareMatrix(full2x2);
+ ASSERT_EQ(4, m2x2f.size());
+ ASSERT_FLOAT_EQ(1, m2x2f[0]);
+ ASSERT_FALSE(m2x2f[1] <= m2x2f[1]); // NAN
+ ASSERT_FLOAT_EQ(0, m2x2f[2]);
+ ASSERT_FLOAT_EQ(-2, m2x2f[3]);
+
+ float output[4];
+ m2x2f.unpackSquareMatrix(output);
+ ASSERT_EQ(1, output[0]);
+ ASSERT_FALSE(output[1] <= output[1]); // NAN
+ ASSERT_EQ(0, output[2]);
+ ASSERT_EQ(-2, output[3]);
+ }
+
+ // Zero matrix will be represented as scalar matrix
+ {
+ const float zero2x2[] =
+ {
+ 0, 0,
+ 0, 0
+ };
+ m2x2f.packSquareMatrix(zero2x2);
+ ASSERT_EQ(1, m2x2f.size());
+ ASSERT_FLOAT_EQ(0, m2x2f[0]);
+ }
+
+ // Symmetric matrix will contain only upper-right triangle
+ {
+ const float sym2x2[] =
+ {
+ 1, 2,
+ 2, 1
+ };
+ m2x2f.packSquareMatrix(sym2x2);
+ ASSERT_EQ(3, m2x2f.size());
+
+ float sym2x2_out[4];
+ m2x2f.unpackSquareMatrix(sym2x2_out);
+ ASSERT_FLOAT_EQ(1, sym2x2_out[0]);
+ ASSERT_FLOAT_EQ(2, sym2x2_out[1]);
+ ASSERT_FLOAT_EQ(2, sym2x2_out[2]);
+ ASSERT_FLOAT_EQ(1, sym2x2_out[3]);
+ }
+ {
+ const float sym3x3[] =
+ {
+ 1, 2, 3,
+ 2, 4, 5,
+ 3, 5, 6
+ };
+ m3x3s.packSquareMatrix(sym3x3);
+ ASSERT_EQ(6, m3x3s.size());
+ ASSERT_EQ(1, m3x3s[0]);
+ ASSERT_EQ(2, m3x3s[1]);
+ ASSERT_EQ(3, m3x3s[2]);
+ ASSERT_EQ(4, m3x3s[3]);
+ ASSERT_EQ(5, m3x3s[4]);
+ ASSERT_EQ(6, m3x3s[5]);
+
+ float sym3x3_out[9];
+ m3x3s.unpackSquareMatrix(sym3x3_out);
+
+ for (int i = 0; i < 9; i++)
+ {
+ ASSERT_FLOAT_EQ(sym3x3[i], sym3x3_out[i]);
+ }
+ }
+ {
+ const double sym6x6[] =
+ {
+ 1, 2, 3, 4, 5, 6,
+ 2, 7, 8, 9, 10, 11,
+ 3, 8, 12, 13, 14, 15,
+ 4, 9, 13, 16, 17, 18,
+ 5, 10, 14, 17, 19, 20,
+ 6, 11, 15, 18, 20, 21
+ };
+ m6x6d.packSquareMatrix(sym6x6);
+ ASSERT_EQ(21, m6x6d.size());
+ for (uavcan::uint8_t i = 0; i < 21; i++)
+ {
+ ASSERT_DOUBLE_EQ(double(i + 1), m6x6d[i]);
+ }
+
+ double sym6x6_out[36];
+ m6x6d.unpackSquareMatrix(sym6x6_out);
+
+ for (int i = 0; i < 36; i++)
+ {
+ ASSERT_DOUBLE_EQ(sym6x6[i], sym6x6_out[i]);
+ }
+ }
+}
+
+
+TEST(Array, FuzzySquareMatrixPacking)
+{
+ Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 36> m6x6d;
+
+ // Diagonal matrix will be reduced to an array of length Width
+ {
+ float diagonal6x6[] =
+ {
+ 1, 0, 0, 0, 0, 0,
+ 0, -2, 0, 0, 0, 0,
+ 0, 0, 3, 0, 0, 0,
+ 0, 0, 0, -4, 0, 0,
+ 0, 0, 0, 0, 5, 0,
+ 0, 0, 0, 0, 0, -6
+ };
+
+ // Some almost-zeroes
+ diagonal6x6[1] = std::numeric_limits<float>::epsilon();
+ diagonal6x6[4] = -std::numeric_limits<float>::epsilon();
+ diagonal6x6[34] = -std::numeric_limits<float>::epsilon();
+
+ m6x6d.packSquareMatrix(diagonal6x6);
+ ASSERT_EQ(6, m6x6d.size());
+ ASSERT_DOUBLE_EQ(1, m6x6d[0]);
+ ASSERT_DOUBLE_EQ(-2, m6x6d[1]);
+ ASSERT_DOUBLE_EQ(3, m6x6d[2]);
+ ASSERT_DOUBLE_EQ(-4, m6x6d[3]);
+ ASSERT_DOUBLE_EQ(5, m6x6d[4]);
+ ASSERT_DOUBLE_EQ(-6, m6x6d[5]);
+
+ std::vector<long double> output(36);
+ m6x6d.unpackSquareMatrix(output);
+
+ // This comparison will fail because epsilons
+ ASSERT_FALSE(std::equal(output.begin(), output.end(), diagonal6x6));
+
+ // This comparison will be ok
+ ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6, &uavcan::areClose<float, float>));
+ }
+}
+
+
+TEST(Array, SquareMatrixPackingIntegers)
+{
+ Array<IntegerSpec<30, SignednessSigned, CastModeSaturate>, ArrayModeDynamic, 9> m3x3int;
+ {
+ const long scalar[] =
+ {
+ 42, 0, 0,
+ 0, 42, 0,
+ 0, 0, 42
+ };
+ m3x3int.packSquareMatrix(scalar);
+ ASSERT_EQ(1, m3x3int.size());
+ ASSERT_EQ(42, m3x3int[0]);
+
+ std::vector<int> output(9);
+ m3x3int.unpackSquareMatrix(output);
+ ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar));
+ }
+ {
+ std::vector<short> diagonal(9);
+ diagonal[0] = 6;
+ diagonal[4] = -57;
+ diagonal[8] = 1139;
+ m3x3int.packSquareMatrix(diagonal);
+ ASSERT_EQ(3, m3x3int.size());
+ ASSERT_EQ(6, m3x3int[0]);
+ ASSERT_EQ(-57, m3x3int[1]);
+ ASSERT_EQ(1139, m3x3int[2]);
+ }
+ {
+ std::vector<long double> full(9);
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ full[i] = i;
+ }
+ m3x3int.packSquareMatrix(full);
+ ASSERT_EQ(9, m3x3int.size());
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ ASSERT_EQ(i, m3x3int[i]);
+ }
+ }
+}
+
+#if UAVCAN_EXCEPTIONS
+TEST(Array, SquareMatrixPackingErrors)
+{
+ Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s;
+
+ std::vector<float> ill_formed_row_major(8);
+ ASSERT_THROW(m3x3s.packSquareMatrix(ill_formed_row_major), std::out_of_range);
+
+ ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range);
+}
+#endif
+
+TEST(Array, SquareMatrixPackingInPlace)
+{
+ Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s;
+
+ // Will do nothing - matrix is empty
+ m3x3s.packSquareMatrix();
+ ASSERT_TRUE(m3x3s.empty());
+
+ // Will fill with zeros - matrix is empty
+ m3x3s.unpackSquareMatrix();
+ ASSERT_EQ(9, m3x3s.size());
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ ASSERT_EQ(0, m3x3s[i]);
+ }
+
+ // Fill an unpackaple matrix
+ m3x3s.clear();
+ m3x3s.push_back(11);
+ m3x3s.push_back(12);
+ m3x3s.push_back(13);
+
+#if UAVCAN_EXCEPTIONS
+ // Shall throw - matrix is ill-formed
+ ASSERT_THROW(m3x3s.packSquareMatrix(), std::out_of_range);
+#endif
+
+ m3x3s.push_back(21);
+ m3x3s.push_back(22);
+ m3x3s.push_back(23);
+ m3x3s.push_back(31);
+ m3x3s.push_back(32);
+ m3x3s.push_back(33);
+
+ // Will pack/unpack successfully
+ ASSERT_EQ(9, m3x3s.size());
+ m3x3s.packSquareMatrix();
+ ASSERT_EQ(9, m3x3s.size());
+ m3x3s.unpackSquareMatrix();
+
+ // Make sure it was unpacked properly
+ ASSERT_EQ(11, m3x3s[0]);
+ ASSERT_EQ(12, m3x3s[1]);
+ ASSERT_EQ(13, m3x3s[2]);
+ ASSERT_EQ(21, m3x3s[3]);
+ ASSERT_EQ(22, m3x3s[4]);
+ ASSERT_EQ(23, m3x3s[5]);
+ ASSERT_EQ(31, m3x3s[6]);
+ ASSERT_EQ(32, m3x3s[7]);
+ ASSERT_EQ(33, m3x3s[8]);
+
+ // Try again with a scalar matrix
+ m3x3s.clear();
+ for (unsigned i = 0; i < 9; i++)
+ {
+ const bool diagonal = (i == 0) || (i == 4) || (i == 8);
+ m3x3s.push_back(diagonal ? 123 : 0);
+ }
+
+ ASSERT_EQ(9, m3x3s.size());
+ m3x3s.packSquareMatrix();
+ ASSERT_EQ(1, m3x3s.size());
+ m3x3s.unpackSquareMatrix();
+ ASSERT_EQ(9, m3x3s.size());
+
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ const bool diagonal = (i == 0) || (i == 4) || (i == 8);
+ ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]);
+ }
+
+ // Try again with symmetric matrix
+ /*
+ * Full matrix:
+ * 1 2 3
+ * 2 4 5
+ * 3 5 6
+ * Compressed triangle:
+ * 1 2 3
+ * 4 5
+ * 6
+ */
+ m3x3s.clear();
+ m3x3s.push_back(1);
+ m3x3s.push_back(2);
+ m3x3s.push_back(3);
+ m3x3s.push_back(4);
+ m3x3s.push_back(5);
+ m3x3s.push_back(6);
+ // Unpacking
+ ASSERT_EQ(6, m3x3s.size());
+ m3x3s.unpackSquareMatrix();
+ ASSERT_EQ(9, m3x3s.size());
+ // Validating
+ ASSERT_EQ(1, m3x3s[0]);
+ ASSERT_EQ(2, m3x3s[1]);
+ ASSERT_EQ(3, m3x3s[2]);
+ ASSERT_EQ(2, m3x3s[3]);
+ ASSERT_EQ(4, m3x3s[4]);
+ ASSERT_EQ(5, m3x3s[5]);
+ ASSERT_EQ(3, m3x3s[6]);
+ ASSERT_EQ(5, m3x3s[7]);
+ ASSERT_EQ(6, m3x3s[8]);
+ // Packing back
+ m3x3s.packSquareMatrix();
+ ASSERT_EQ(6, m3x3s.size());
+ // Validating
+ ASSERT_EQ(1, m3x3s[0]);
+ ASSERT_EQ(2, m3x3s[1]);
+ ASSERT_EQ(3, m3x3s[2]);
+ ASSERT_EQ(4, m3x3s[3]);
+ ASSERT_EQ(5, m3x3s[4]);
+ ASSERT_EQ(6, m3x3s[5]);
+}
+
+TEST(Array, FuzzyComparison)
+{
+ typedef Array<Array<Array<FloatSpec<32, CastModeSaturate>, ArrayModeStatic, 2>,
+ ArrayModeStatic, 2>,
+ ArrayModeStatic, 2> ArrayStatic32;
+
+ typedef Array<Array<Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 2>,
+ ArrayModeDynamic, 2>,
+ ArrayModeDynamic, 2> ArrayDynamic64;
+
+ ArrayStatic32 array_s32;
+
+ ArrayDynamic64 array_d64;
+
+ array_d64.resize(2);
+ array_d64[0].resize(2);
+ array_d64[1].resize(2);
+ array_d64[0][0].resize(2);
+ array_d64[0][1].resize(2);
+ array_d64[1][0].resize(2);
+ array_d64[1][1].resize(2);
+
+ std::cout << "One:";
+ uavcan::YamlStreamer<ArrayStatic32>::stream(std::cout, array_s32, 0);
+ std::cout << std::endl << "------";
+ uavcan::YamlStreamer<ArrayDynamic64>::stream(std::cout, array_d64, 0);
+ std::cout << std::endl;
+
+ // Both are equal right now
+ ASSERT_TRUE(array_d64 == array_s32);
+ ASSERT_TRUE(array_d64.isClose(array_s32));
+ ASSERT_TRUE(array_s32.isClose(array_d64));
+
+ // Slightly modifying - still close enough
+ array_s32[0][0][0] = 123.456F + uavcan::NumericTraits<float>::epsilon() * 123.0F;
+ array_s32[0][0][1] = uavcan::NumericTraits<float>::infinity();
+ array_s32[0][1][0] = uavcan::NumericTraits<float>::epsilon();
+ array_s32[0][1][1] = -uavcan::NumericTraits<float>::epsilon();
+
+ array_d64[0][0][0] = 123.456;
+ array_d64[0][0][1] = uavcan::NumericTraits<double>::infinity();
+ array_d64[0][1][0] = -uavcan::NumericTraits<double>::epsilon(); // Note that the sign is inverted
+ array_d64[0][1][1] = uavcan::NumericTraits<double>::epsilon();
+
+ std::cout << "Two:";
+ uavcan::YamlStreamer<ArrayStatic32>::stream(std::cout, array_s32, 0);
+ std::cout << std::endl << "------";
+ uavcan::YamlStreamer<ArrayDynamic64>::stream(std::cout, array_d64, 0);
+ std::cout << std::endl;
+
+ // They are close bot not exactly equal
+ ASSERT_FALSE(array_d64 == array_s32);
+ ASSERT_TRUE(array_d64.isClose(array_s32));
+ ASSERT_TRUE(array_s32.isClose(array_d64));
+
+ // Not close
+ array_d64[0][0][0] = 123.457;
+
+ ASSERT_FALSE(array_d64 == array_s32);
+ ASSERT_FALSE(array_d64.isClose(array_s32));
+ ASSERT_FALSE(array_s32.isClose(array_d64));
+
+ // Values are close, but lengths differ
+ array_d64[0][0][0] = 123.456;
+
+ ASSERT_FALSE(array_d64 == array_s32);
+ ASSERT_TRUE(array_d64.isClose(array_s32));
+ ASSERT_TRUE(array_s32.isClose(array_d64));
+
+ array_d64[0][0].resize(1);
+
+ ASSERT_FALSE(array_d64 == array_s32);
+ ASSERT_FALSE(array_d64.isClose(array_s32));
+ ASSERT_FALSE(array_s32.isClose(array_d64));
+
+ std::cout << "Three:";
+ uavcan::YamlStreamer<ArrayStatic32>::stream(std::cout, array_s32, 0);
+ std::cout << std::endl << "------";
+ uavcan::YamlStreamer<ArrayDynamic64>::stream(std::cout, array_d64, 0);
+ std::cout << std::endl;
+}
+
+TEST(Array, CaseConversion)
+{
+ Array<IntegerSpec<8, SignednessUnsigned, CastModeTruncate>, ArrayModeDynamic, 30> str;
+
+ str.convertToLowerCaseASCII();
+ str.convertToUpperCaseASCII();
+
+ ASSERT_STREQ("", str.c_str());
+
+ str = "Hello World!";
+
+ ASSERT_STREQ("Hello World!", str.c_str());
+ str.convertToLowerCaseASCII();
+ ASSERT_STREQ("hello world!", str.c_str());
+ str.convertToUpperCaseASCII();
+ ASSERT_STREQ("HELLO WORLD!", str.c_str());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/bit_stream.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,194 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/marshal/bit_stream.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+
+TEST(BitStream, ToString)
+{
+ {
+ uavcan::StaticTransferBuffer<8> buf;
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ("", bs.toString());
+ ASSERT_EQ("", bs.toString());
+ }
+ {
+ const uint8_t data[] = {0xad}; // 10101101
+ uavcan::StaticTransferBuffer<8> buf;
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ(1, bs.write(data, 8)); // all 8
+ ASSERT_EQ("10101101", bs.toString());
+ }
+ {
+ const uint8_t data[] = {0xad, 0xbe}; // 10101101 10111110
+ uavcan::StaticTransferBuffer<8> buf;
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ(1, bs.write(data, 16)); // all 16
+ ASSERT_EQ("10101101 10111110", bs.toString());
+ }
+ {
+ const uint8_t data[] = {0xad, 0xbe, 0xfc}; // 10101101 10111110 11111100
+ uavcan::StaticTransferBuffer<8> buf;
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ(1, bs.write(data, 20)); // 10101101 10111110 1111
+ ASSERT_EQ("10101101 10111110 11110000", bs.toString());
+ }
+}
+
+
+TEST(BitStream, BitOrderSimple)
+{
+ /*
+ * a = 1010
+ * b = 1011
+ * c = 1100
+ * d = 1101
+ * e = 1110
+ * f = 1111
+ */
+ uavcan::StaticTransferBuffer<32> buf;
+ { // Write
+ const uint8_t data[] = {0xad, 0xbe}; // adbe
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ(1, bs.write(data, 12)); // adb0
+ ASSERT_EQ("10101101 10110000", bs.toString()); // adb0
+ }
+ { // Read
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ("10101101 10110000", bs.toString()); // Same data
+ uint8_t data[] = {0xFF, 0xFF}; // Uninitialized
+ ASSERT_EQ(1, bs.read(data, 12));
+ ASSERT_EQ(0xad, data[0]);
+ ASSERT_EQ(0xb0, data[1]);
+ }
+}
+
+
+TEST(BitStream, BitOrderComplex)
+{
+ static const std::string REFERENCE =
+ "10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100";
+
+ uavcan::StaticTransferBuffer<32> buf;
+ { // Write
+ const uint8_t data1[] = {0xad, 0xbe}; // 10101101 10111110
+ const uint8_t data2[] = {0xfc}; // 11111100
+ const uint8_t data3[] = {0xde, 0xad, 0xbe, 0xef}; // 11011110 10101101 10111110 11101111
+ const uint8_t data4[] = {0x12, 0x34, 0x56, 0x78, // 00010010 00110100 01010110 01111000
+ 0x9a, 0xbc, 0xde, 0xf0}; // 10011010 10111100 11011110 11110000
+
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ(1, bs.write(data1, 11)); // 10101101 101
+ std::cout << bs.toString() << std::endl;
+ ASSERT_EQ(1, bs.write(data2, 6)); // 11111 1
+ std::cout << bs.toString() << std::endl;
+ ASSERT_EQ(1, bs.write(data3, 25)); // 1101111 01010110 11011111 01
+ std::cout << bs.toString() << std::endl;
+ ASSERT_EQ(1, bs.write(data4, 64)); // all 64, total 42 + 64 = 106
+ std::cout << bs.toString() << std::endl;
+ ASSERT_EQ(1, bs.write(data4, 4)); // 0001
+ std::cout << bs.toString() << std::endl;
+
+ std::cout << "Reference:\n" << REFERENCE << std::endl;
+
+ ASSERT_EQ(REFERENCE, bs.toString());
+ }
+ { // Read back in the same order
+ uint8_t data[8];
+ std::fill(data, data + sizeof(data), 0xA5); // Filling with garbage
+ uavcan::BitStream bs(buf);
+ ASSERT_EQ(REFERENCE, bs.toString());
+
+ ASSERT_EQ(1, bs.read(data, 11)); // 10101101 10100000
+ ASSERT_EQ(0xad, data[0]);
+ ASSERT_EQ(0xa0, data[1]);
+
+ ASSERT_EQ(1, bs.read(data, 6)); // 11111100
+ ASSERT_EQ(0xfc, data[0]);
+
+ ASSERT_EQ(1, bs.read(data, 25)); // 11011110 10101101 10111110 10000000
+ ASSERT_EQ(0xde, data[0]);
+ ASSERT_EQ(0xad, data[1]);
+ ASSERT_EQ(0xbe, data[2]);
+ ASSERT_EQ(0x80, data[3]);
+
+ ASSERT_EQ(1, bs.read(data, 64)); // Data - see above
+ ASSERT_EQ(0x12, data[0]);
+ ASSERT_EQ(0x34, data[1]);
+ ASSERT_EQ(0x56, data[2]);
+ ASSERT_EQ(0x78, data[3]);
+ ASSERT_EQ(0x9a, data[4]);
+ ASSERT_EQ(0xbc, data[5]);
+ ASSERT_EQ(0xde, data[6]);
+ ASSERT_EQ(0xf0, data[7]);
+ }
+}
+
+
+TEST(BitStream, BitByBit)
+{
+ static const int NUM_BYTES = 1024;
+ uavcan::StaticTransferBuffer<NUM_BYTES> buf;
+ uavcan::BitStream bs_wr(buf);
+
+ std::string binary_string;
+ unsigned counter = 0;
+ for (int byte = 0; byte < NUM_BYTES; byte++)
+ {
+ for (int bit = 0; bit < 8; bit++, counter++)
+ {
+ const bool value = counter % 3 == 0;
+ binary_string.push_back(value ? '1' : '0');
+ const uint8_t data[] = { uint8_t(value << 7) };
+ ASSERT_EQ(1, bs_wr.write(data, 1));
+ }
+ binary_string.push_back(' ');
+ }
+ binary_string.erase(binary_string.length() - 1, 1);
+
+ /*
+ * Currently we have no free buffer space, so next write() must fail
+ */
+ const uint8_t dummy_data_wr[] = { 0xFF };
+ ASSERT_EQ(0, bs_wr.write(dummy_data_wr, 1));
+
+ /*
+ * Bitstream content validation
+ */
+// std::cout << bs.toString() << std::endl;
+// std::cout << "Reference:\n" << binary_string << std::endl;
+ ASSERT_EQ(binary_string, bs_wr.toString());
+
+ /*
+ * Read back
+ */
+ uavcan::BitStream bs_rd(buf);
+ counter = 0;
+ for (int byte = 0; byte < NUM_BYTES; byte++)
+ {
+ for (int bit = 0; bit < 8; bit++, counter++)
+ {
+ const bool value = counter % 3 == 0;
+ uint8_t data[1];
+ ASSERT_EQ(1, bs_rd.read(data, 1));
+ if (value)
+ {
+ ASSERT_EQ(0x80, data[0]);
+ }
+ else
+ {
+ ASSERT_EQ(0, data[0]);
+ }
+ }
+ }
+
+ /*
+ * Making sure that reading out of buffer range will fail with error
+ */
+ uint8_t dummy_data_rd[] = { 0xFF };
+ ASSERT_EQ(0, bs_wr.read(dummy_data_rd, 1));
+ ASSERT_EQ(0xFF, dummy_data_rd[0]);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/char_array_formatter.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/marshal/char_array_formatter.hpp>
+
+using uavcan::Array;
+using uavcan::ArrayModeDynamic;
+using uavcan::ArrayModeStatic;
+using uavcan::IntegerSpec;
+using uavcan::SignednessSigned;
+using uavcan::SignednessUnsigned;
+using uavcan::CastModeSaturate;
+using uavcan::CastModeTruncate;
+using uavcan::CharArrayFormatter;
+
+TEST(CharArrayFormatter, Basic)
+{
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 45> A8;
+ A8 a;
+
+ CharArrayFormatter<A8> f(a);
+ ASSERT_STREQ("", f.getArray().c_str());
+
+ f.write("Don't %s.", "Panic");
+ ASSERT_STREQ("Don't Panic.", f.getArray().c_str());
+
+ f.write(" abc%idef ", 123);
+ ASSERT_STREQ("Don't Panic. abc123def ", f.getArray().c_str());
+
+ f.write("%g", 0.0);
+ ASSERT_STREQ("Don't Panic. abc123def 0", f.getArray().c_str());
+
+ a.clear();
+ ASSERT_STREQ("", f.getArray().c_str());
+
+ f.write("123456789");
+ ASSERT_STREQ("123456789", f.getArray().c_str());
+}
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+TEST(CharArrayFormatter, Hardcore)
+{
+ typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 150> A8;
+ A8 a;
+
+ CharArrayFormatter<A8> f(a);
+
+ f.write(
+ "%% char='%*' double='%*' long='%*' unsigned long='%*' int='%s' long double='%*' bool='%*' const char='%*' %%",
+ '%', -12.3456, -123456789123456789L, 987654321, -123456789, 0.000000001L, true, "Don't Panic.");
+
+ static const std::string Reference =
+ "% char='%' double='-12.3456' long='-123456789123456789' unsigned long='987654321' int='-123456789' "
+ "long double='1e-09' bool='1' const char='Don't Pani"; // few chars truncated!
+
+ ASSERT_STREQ(Reference.c_str(), f.getArray().c_str());
+
+ a.clear();
+
+ f.write("");
+ ASSERT_STREQ("", f.getArray().c_str());
+
+ f.write("%%"); // Nothing to format --> "%%" is not expanded
+ ASSERT_STREQ("%%", f.getArray().c_str());
+
+ f.write("%*", "Test", 123, true); // Extra args ignored
+ ASSERT_STREQ("%%Test", f.getArray().c_str());
+
+ f.write("%% %* %* %% %*", true); // Insufficient args are OK; second "%%" is not expanded
+ ASSERT_STREQ("%%Test% 1 %* %% %*", f.getArray().c_str());
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/float_spec.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <limits>
+#include <uavcan/marshal/types.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+
+TEST(FloatSpec, Sizes)
+{
+ uavcan::StaticAssert<sizeof(uavcan::NativeFloatSelector<16>::Type) == 4>::check();
+ uavcan::StaticAssert<sizeof(uavcan::NativeFloatSelector<32>::Type) == 4>::check();
+ uavcan::StaticAssert<sizeof(uavcan::NativeFloatSelector<64>::Type) == 8>::check();
+ uavcan::StaticAssert<sizeof(uavcan::NativeFloatSelector<80>::Type) >= 10>::check();
+}
+
+TEST(FloatSpec, Limits)
+{
+ using uavcan::FloatSpec;
+ using uavcan::CastModeSaturate;
+ using uavcan::CastModeTruncate;
+
+ typedef FloatSpec<16, CastModeSaturate> F16S;
+ typedef FloatSpec<32, CastModeTruncate> F32T;
+ typedef FloatSpec<64, CastModeSaturate> F64S;
+
+ ASSERT_FALSE(F16S::IsExactRepresentation);
+ ASSERT_FLOAT_EQ(65504.0F, F16S::max());
+ ASSERT_FLOAT_EQ(9.77e-04F, F16S::epsilon());
+
+ ASSERT_TRUE(F32T::IsExactRepresentation);
+ ASSERT_FLOAT_EQ(std::numeric_limits<float>::max(), F32T::max());
+ ASSERT_FLOAT_EQ(std::numeric_limits<float>::epsilon(), F32T::epsilon());
+
+ ASSERT_TRUE(F64S::IsExactRepresentation);
+ ASSERT_DOUBLE_EQ(std::numeric_limits<double>::max(), F64S::max());
+ ASSERT_DOUBLE_EQ(std::numeric_limits<double>::epsilon(), F64S::epsilon());
+}
+
+TEST(FloatSpec, Basic)
+{
+ using uavcan::FloatSpec;
+ using uavcan::CastModeSaturate;
+ using uavcan::CastModeTruncate;
+ using uavcan::StorageType;
+
+ typedef FloatSpec<16, CastModeSaturate> F16S;
+ typedef FloatSpec<16, CastModeTruncate> F16T;
+ typedef FloatSpec<32, CastModeSaturate> F32S;
+ typedef FloatSpec<32, CastModeTruncate> F32T;
+ typedef FloatSpec<64, CastModeSaturate> F64S;
+ typedef FloatSpec<64, CastModeTruncate> F64T;
+
+ static const long double Values[] =
+ {
+ 0.0,
+ 1.0,
+ M_PI,
+ 123,
+ -123,
+ 99999,
+ -999999,
+ std::numeric_limits<float>::max(),
+ -std::numeric_limits<float>::max(),
+ std::numeric_limits<double>::infinity(),
+ -std::numeric_limits<double>::infinity(),
+ nanl("")
+ };
+ static const int NumValues = int(sizeof(Values) / sizeof(Values[0]));
+
+ static const long double ValuesF16S[] =
+ {
+ 0.0,
+ 1.0,
+ 3.140625,
+ 123,
+ -123,
+ F16S::max(),
+ -F16S::max(),
+ F16S::max(),
+ -F16S::max(),
+ std::numeric_limits<F16S::StorageType>::infinity(),
+ -std::numeric_limits<F16S::StorageType>::infinity(),
+ nanl("")
+ };
+ static const long double ValuesF16T[] =
+ {
+ 0.0,
+ 1.0,
+ 3.140625,
+ 123,
+ -123,
+ std::numeric_limits<F16S::StorageType>::infinity(),
+ -std::numeric_limits<F16S::StorageType>::infinity(),
+ std::numeric_limits<F16S::StorageType>::infinity(),
+ -std::numeric_limits<F16S::StorageType>::infinity(),
+ std::numeric_limits<F16S::StorageType>::infinity(),
+ -std::numeric_limits<F16S::StorageType>::infinity(),
+ nanl("")
+ };
+
+ /*
+ * Writing
+ */
+ uavcan::StaticTransferBuffer<NumValues * (2 + 4 + 8) * 2> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ for (int i = 0; i < NumValues; i++)
+ {
+ ASSERT_EQ(1, F16S::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F16T::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F32S::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F32T::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F64S::encode(double(Values[i]), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F64T::encode(double(Values[i]), sc_wr, uavcan::TailArrayOptDisabled));
+ }
+
+ ASSERT_EQ(0, F16S::encode(0, sc_wr, uavcan::TailArrayOptDisabled)); // Out of buffer space now
+
+ /*
+ * Reading
+ */
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+
+#define CHECK(FloatType, expected_value) \
+ do { \
+ StorageType<FloatType>::Type var = StorageType<FloatType>::Type(); \
+ ASSERT_EQ(1, FloatType::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); \
+ if (!std::isnan(expected_value)) { \
+ ASSERT_DOUBLE_EQ(expected_value, var); } \
+ else { \
+ ASSERT_EQ(!!std::isnan(expected_value), !!std::isnan(var)); } \
+ } while (0)
+
+ for (int i = 0; i < NumValues; i++)
+ {
+ CHECK(F16S, float(ValuesF16S[i]));
+ CHECK(F16T, float(ValuesF16T[i]));
+ CHECK(F32S, float(Values[i]));
+ CHECK(F32T, float(Values[i]));
+ CHECK(F64S, double(Values[i]));
+ CHECK(F64T, double(Values[i]));
+ }
+
+#undef CHECK
+}
+
+TEST(FloatSpec, Float16Representation)
+{
+ using uavcan::FloatSpec;
+ using uavcan::CastModeSaturate;
+ using uavcan::CastModeTruncate;
+
+ typedef FloatSpec<16, CastModeSaturate> F16S;
+ typedef FloatSpec<16, CastModeTruncate> F16T;
+
+ uavcan::StaticTransferBuffer<2 * 6> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ ASSERT_EQ(1, F16S::encode(0.0, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F16S::encode(1.0, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F16S::encode(-2.0, sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, F16T::encode(999999, sc_wr, uavcan::TailArrayOptDisabled)); // +inf
+ ASSERT_EQ(1, F16S::encode(-999999, sc_wr, uavcan::TailArrayOptDisabled)); // -max
+ ASSERT_EQ(1, F16S::encode(float(nan("")), sc_wr, uavcan::TailArrayOptDisabled)); // nan
+
+ ASSERT_EQ(0, F16S::encode(0, sc_wr, uavcan::TailArrayOptDisabled)); // Out of buffer space now
+
+ // Keep in mind that this is LITTLE ENDIAN representation
+ static const std::string Reference =
+ "00000000 00000000 " // 0.0
+ "00000000 00111100 " // 1.0
+ "00000000 11000000 " // -2.0
+ "00000000 01111100 " // +inf
+ "11111111 11111011 " // -max
+ "11111111 01111111"; // nan
+
+ ASSERT_EQ(Reference, bs_wr.toString());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/integer_spec.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/marshal/types.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+
+TEST(IntegerSpec, Limits)
+{
+ using uavcan::IntegerSpec;
+ using uavcan::SignednessSigned;
+ using uavcan::SignednessUnsigned;
+ using uavcan::CastModeSaturate;
+ using uavcan::CastModeTruncate;
+
+ typedef IntegerSpec<8, SignednessUnsigned, CastModeSaturate> UInt8;
+ typedef IntegerSpec<4, SignednessSigned, CastModeTruncate> SInt4;
+ typedef IntegerSpec<32, SignednessUnsigned, CastModeTruncate> UInt32;
+ typedef IntegerSpec<40, SignednessUnsigned, CastModeSaturate> UInt40;
+ typedef IntegerSpec<64, SignednessUnsigned, CastModeTruncate> UInt64;
+ typedef IntegerSpec<64, SignednessSigned, CastModeSaturate> SInt64;
+ typedef IntegerSpec<63, SignednessUnsigned, CastModeSaturate> UInt63;
+
+ ASSERT_EQ(255, UInt8::max());
+ ASSERT_EQ(0, UInt8::min());
+
+ ASSERT_EQ(7, SInt4::max());
+ ASSERT_EQ(-8, SInt4::min());
+
+ ASSERT_EQ(0xFFFFFFFF, UInt32::max());
+ ASSERT_EQ(0, UInt32::min());
+
+ ASSERT_EQ(0xFFFFFFFFFF, UInt40::max());
+ ASSERT_EQ(0, UInt40::min());
+
+ ASSERT_EQ(0xFFFFFFFFFFFFFFFF, UInt64::max());
+ ASSERT_EQ(0, UInt64::min());
+
+ ASSERT_EQ(0x7FFFFFFFFFFFFFFF, SInt64::max());
+ ASSERT_EQ(-0x8000000000000000, SInt64::min());
+
+ ASSERT_EQ(0x7FFFFFFFFFFFFFFF, UInt63::max());
+ ASSERT_EQ(0, UInt63::min());
+
+ ASSERT_EQ(SInt64::max(), UInt63::max());
+}
+
+
+TEST(IntegerSpec, Basic)
+{
+ using uavcan::IntegerSpec;
+ using uavcan::SignednessSigned;
+ using uavcan::SignednessUnsigned;
+ using uavcan::CastModeSaturate;
+ using uavcan::CastModeTruncate;
+ using uavcan::StorageType;
+
+ uavcan::StaticTransferBuffer<100> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ typedef IntegerSpec<8, SignednessUnsigned, CastModeSaturate> UInt8S;
+ typedef IntegerSpec<4, SignednessSigned, CastModeTruncate> SInt4T;
+ typedef IntegerSpec<32, SignednessUnsigned, CastModeTruncate> UInt32T;
+ typedef IntegerSpec<40, SignednessUnsigned, CastModeSaturate> UInt40S;
+ typedef IntegerSpec<64, SignednessUnsigned, CastModeTruncate> UInt64T;
+ typedef IntegerSpec<58, SignednessSigned, CastModeSaturate> SInt58S;
+ typedef IntegerSpec<63, SignednessUnsigned, CastModeSaturate> UInt63S;
+ typedef IntegerSpec<10, SignednessSigned, CastModeSaturate> SInt10S;
+ typedef IntegerSpec<1, SignednessUnsigned, CastModeSaturate> UInt1S;
+
+ ASSERT_EQ(1, UInt8S::encode(UInt8S::StorageType(123), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, SInt4T::encode(SInt4T::StorageType(-0x44), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, UInt32T::encode(UInt32T::StorageType(0xFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, UInt40S::encode(UInt40S::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, UInt64T::encode(UInt64T::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, SInt58S::encode(SInt58S::StorageType(0xFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, UInt63S::encode(UInt63S::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, SInt10S::encode(SInt10S::StorageType(-30000), sc_wr, uavcan::TailArrayOptDisabled));
+ ASSERT_EQ(1, UInt1S::encode(UInt1S::StorageType(42), sc_wr, uavcan::TailArrayOptDisabled));
+
+ std::cout << bs_wr.toString() << std::endl;
+
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+
+#define CHECK(IntType, expected_value) \
+ do { \
+ StorageType<IntType>::Type var = StorageType<IntType>::Type(); \
+ ASSERT_EQ(1, IntType::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); \
+ ASSERT_EQ(expected_value, var); \
+ } while (0)
+
+ CHECK(UInt8S, 123);
+ CHECK(SInt4T, -4);
+ CHECK(UInt32T, 0xFFFFFFFF);
+ CHECK(UInt40S, 0xFFFFFFFFFF);
+ CHECK(UInt64T, 0xFFFFFFFFFFFFFFFF);
+ CHECK(SInt58S, 0x1FFFFFFFFFFFFFF);
+ CHECK(UInt63S, 0x7FFFFFFFFFFFFFFF);
+ CHECK(SInt10S, -512);
+ CHECK(UInt1S, 1);
+
+#undef CHECK
+
+ StorageType<UInt1S>::Type var;
+ ASSERT_EQ(0, UInt1S::decode(var, sc_rd, uavcan::TailArrayOptDisabled));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/scalar_codec.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <limits>
+#include <uavcan/marshal/scalar_codec.hpp>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+
+TEST(ScalarCodec, Basic)
+{
+ uavcan::StaticTransferBuffer<38> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ {
+ uint64_t u64 = 0;
+ ASSERT_EQ(0, sc_wr.decode<64>(u64)); // Out of buffer space
+ }
+
+ /*
+ * Encoding some variables
+ */
+ ASSERT_EQ(1, sc_wr.encode<12>(uint16_t(0xbeda))); // --> 0xeda
+ ASSERT_EQ(1, sc_wr.encode<1>(uint8_t(1)));
+ ASSERT_EQ(1, sc_wr.encode<1>(uint16_t(0)));
+ ASSERT_EQ(1, sc_wr.encode<4>(uint8_t(8)));
+ ASSERT_EQ(1, sc_wr.encode<32>(uint32_t(0xdeadbeef)));
+ ASSERT_EQ(1, sc_wr.encode<3>(int8_t(-1)));
+ ASSERT_EQ(1, sc_wr.encode<4>(int8_t(-6)));
+ ASSERT_EQ(1, sc_wr.encode<20>(int32_t(-123456)));
+ ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits<int64_t>::min()));
+ ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits<int64_t>::max()));
+ ASSERT_EQ(1, sc_wr.encode<15>(int16_t(-1)));
+ ASSERT_EQ(1, sc_wr.encode<2>(int16_t(-1)));
+ ASSERT_EQ(1, sc_wr.encode<16>(std::numeric_limits<int16_t>::min()));
+ ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits<uint64_t>::max())); // Total 302 bit (38 bytes)
+
+ ASSERT_EQ(0, sc_wr.encode<64>(std::numeric_limits<uint64_t>::min())); // Out of buffer space
+
+ /*
+ * Decoding back
+ */
+ uavcan::BitStream bs_rd(buf);
+ uavcan::ScalarCodec sc_rd(bs_rd);
+
+ uint8_t u8 = 0;
+ int8_t i8 = 0;
+ uint16_t u16 = 0;
+ int16_t i16 = 0;
+ uint32_t u32 = 0;
+ int32_t i32 = 0;
+ uint64_t u64 = 0;
+ int64_t i64 = 0;
+
+#define CHECK(bitlen, variable, expected_value) \
+ do { \
+ ASSERT_EQ(1, sc_rd.decode<bitlen>(variable)); \
+ ASSERT_EQ(expected_value, variable); \
+ } while (0)
+
+ CHECK(12, u16, 0xeda);
+ CHECK(1, u8, 1);
+ CHECK(1, u16, 0);
+ CHECK(4, u8, 8);
+ CHECK(32, u32, 0xdeadbeef);
+ CHECK(3, i8, -1);
+ CHECK(4, i8, -6);
+ CHECK(20, i32, -123456);
+ CHECK(64, i64, std::numeric_limits<int64_t>::min());
+ CHECK(64, i64, std::numeric_limits<int64_t>::max());
+ CHECK(15, i16, -1);
+ CHECK(2, i8, -1);
+ CHECK(16, i16, std::numeric_limits<int16_t>::min());
+ CHECK(64, u64, std::numeric_limits<uint64_t>::max());
+
+#undef CHECK
+
+ ASSERT_EQ(0, sc_rd.decode<64>(u64)); // Out of buffer space
+}
+
+TEST(ScalarCodec, RepresentationCorrectness)
+{
+ uavcan::StaticTransferBuffer<4> buf;
+ uavcan::BitStream bs_wr(buf);
+ uavcan::ScalarCodec sc_wr(bs_wr);
+
+ ASSERT_EQ(1, sc_wr.encode<12>(uint16_t(0xbeda))); // --> 0xeda
+ ASSERT_EQ(1, sc_wr.encode<3>(int8_t(-1)));
+ ASSERT_EQ(1, sc_wr.encode<4>(int8_t(-5)));
+ ASSERT_EQ(1, sc_wr.encode<2>(int16_t(-1)));
+ ASSERT_EQ(1, sc_wr.encode<4>(uint8_t(0x88))); // --> 8
+
+ // This representation was carefully crafted and triple checked:
+ static const std::string REFERENCE = "11011010 11101111 01111100 00000000";
+ ASSERT_EQ(REFERENCE, bs_wr.toString());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/marshal/type_util.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/marshal/types.hpp>
+
+
+TEST(MarshalTypeUtil, IntegerBitLen)
+{
+ using uavcan::IntegerBitLen;
+
+ ASSERT_EQ(0, IntegerBitLen<0>::Result);
+ ASSERT_EQ(1, IntegerBitLen<1>::Result);
+ ASSERT_EQ(6, IntegerBitLen<42>::Result);
+ ASSERT_EQ(8, IntegerBitLen<232>::Result);
+ ASSERT_EQ(32, IntegerBitLen<0x81234567>::Result);
+}
+
+
+TEST(MarshalTypeUtil, BitLenToByteLen)
+{
+ using uavcan::BitLenToByteLen;
+
+ ASSERT_EQ(2, BitLenToByteLen<16>::Result);
+ ASSERT_EQ(1, BitLenToByteLen<8>::Result);
+ ASSERT_EQ(1, BitLenToByteLen<7>::Result);
+ ASSERT_EQ(1, BitLenToByteLen<1>::Result);
+ ASSERT_EQ(2, BitLenToByteLen<9>::Result);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/global_data_type_registry.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/global_data_type_registry.hpp>
+
+namespace
+{
+
+struct DataTypeAMessage
+{
+ enum { DefaultDataTypeID = 0 };
+ enum { DataTypeKind = uavcan::DataTypeKindMessage };
+ static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(123); }
+ static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; }
+};
+
+struct DataTypeAService
+{
+ enum { DefaultDataTypeID = 0 };
+ enum { DataTypeKind = uavcan::DataTypeKindService };
+ static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(789); }
+ static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; }
+};
+
+struct DataTypeB
+{
+ enum { DefaultDataTypeID = 42 };
+ enum { DataTypeKind = uavcan::DataTypeKindMessage };
+ static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(456); }
+ static const char* getDataTypeFullName() { return "my_namespace.DataTypeB"; }
+};
+
+struct DataTypeC
+{
+ enum { DefaultDataTypeID = 1023 };
+ enum { DataTypeKind = uavcan::DataTypeKindMessage };
+ static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(654); }
+ static const char* getDataTypeFullName() { return "foo.DataTypeC"; }
+};
+
+struct DataTypeD
+{
+ enum { DefaultDataTypeID = 43 };
+ enum { DataTypeKind = uavcan::DataTypeKindService };
+ static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(987); }
+ static const char* getDataTypeFullName() { return "foo.DataTypeD"; }
+};
+
+template <typename Type>
+uavcan::DataTypeDescriptor extractDescriptor(uint16_t dtid = Type::DefaultDataTypeID)
+{
+ return uavcan::DataTypeDescriptor(uavcan::DataTypeKind(Type::DataTypeKind), dtid,
+ Type::getDataTypeSignature(), Type::getDataTypeFullName());
+}
+
+}
+
+
+TEST(GlobalDataTypeRegistry, Basic)
+{
+ using uavcan::GlobalDataTypeRegistry;
+ using uavcan::DataTypeSignature;
+
+ GlobalDataTypeRegistry::instance().reset();
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen());
+ ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumMessageTypes());
+ ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes());
+
+ /*
+ * Static registrations
+ */
+ uavcan::DefaultDataTypeRegistrator<DataTypeAMessage> reg_DataTypeAMessage;
+ uavcan::DefaultDataTypeRegistrator<DataTypeB> reg_DataTypeB;
+
+ ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes());
+ ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes());
+
+ /*
+ * Runtime registrations
+ */
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeAService>(
+ DataTypeAService::DefaultDataTypeID));
+
+ ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes());
+ ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes());
+
+ /*
+ * Runtime re-registration
+ */
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeAService>(147));
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeB>(741));
+
+ ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes());
+ ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes());
+
+ /*
+ * These types will be necessary for the aggregate signature test
+ */
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultCollision,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeC>(741)); // ID COLLISION
+
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeC>(DataTypeC::DefaultDataTypeID));
+ uavcan::DefaultDataTypeRegistrator<DataTypeD> reg_DataTypeD;
+
+ /*
+ * Frozen state
+ */
+ GlobalDataTypeRegistry::instance().freeze();
+
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeAService>(555)); // Rejected
+
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeAMessage>(999)); // Rejected
+
+ ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen,
+ GlobalDataTypeRegistry::instance().registerDataType<DataTypeB>(888)); // Rejected
+
+ /*
+ * Searching
+ */
+ const uavcan::DataTypeDescriptor* pdtd = UAVCAN_NULLPTR;
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "Nonexistent"));
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().find("Nonexistent"));
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, 987));
+ // Asking for service, but this is a message:
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "my_namespace.DataTypeB"));
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, 42));
+
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage,
+ "my_namespace.DataTypeB")));
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find("my_namespace.DataTypeB")));
+ ASSERT_EQ(extractDescriptor<DataTypeB>(741), *pdtd);
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, 741)));
+ ASSERT_EQ(extractDescriptor<DataTypeB>(741), *pdtd);
+
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage,
+ "my_namespace.DataTypeA")));
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find("my_namespace.DataTypeA")));
+ ASSERT_EQ(extractDescriptor<DataTypeAMessage>(), *pdtd);
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, uavcan::DataTypeID(0))));
+ ASSERT_EQ(extractDescriptor<DataTypeAMessage>(), *pdtd);
+
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService,
+ "my_namespace.DataTypeA")));
+ ASSERT_EQ(extractDescriptor<DataTypeAService>(147), *pdtd);
+ ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, 147)));
+ ASSERT_EQ(extractDescriptor<DataTypeAService>(147), *pdtd);
+}
+
+
+TEST(GlobalDataTypeRegistry, Reset)
+{
+ using uavcan::GlobalDataTypeRegistry;
+
+ /*
+ * Since we're dealing with singleton, we need to reset it for other tests to use
+ */
+ ASSERT_TRUE(GlobalDataTypeRegistry::instance().isFrozen());
+ GlobalDataTypeRegistry::instance().reset();
+ ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/node.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/node.hpp>
+#include <uavcan/node/sub_node.hpp> // Compilability test
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "test_node.hpp"
+#include "../protocol/helpers.hpp"
+
+static void registerTypes()
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg2;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg4;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetDataTypeInfo> _reg5;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::debug::LogMessage> _reg6;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetTransportStats> _reg7;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::RestartNode> _reg8;
+}
+
+
+TEST(Node, Basic)
+{
+ registerTypes();
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::protocol::SoftwareVersion swver;
+ swver.major = 0;
+ swver.minor = 1;
+ swver.vcs_commit = 0xDEADBEEF;
+
+ std::cout << "sizeof(uavcan::Node<0>): " << sizeof(uavcan::Node<0>) << std::endl;
+
+ /*
+ * uavcan::Node
+ */
+ uavcan::Node<1024> node1(nodes.can_a, nodes.clock_a);
+ node1.setName("com.example");
+ node1.setNodeID(1);
+ node1.setSoftwareVersion(swver);
+
+ /*
+ * Companion test node
+ */
+ uavcan::Node<1024> node2(nodes.can_b, nodes.clock_b);
+ node2.setName("foobar");
+ node2.setNodeID(2);
+ node2.setSoftwareVersion(swver);
+
+ BackgroundSpinner bgspinner(node2, node1);
+ bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10));
+
+ uavcan::NodeStatusMonitor node_status_monitor(node2);
+ ASSERT_LE(0, node_status_monitor.start());
+
+ /*
+ * Init the second node - network is empty
+ */
+ ASSERT_LE(0, node2.start());
+ ASSERT_FALSE(node_status_monitor.findNodeWithWorstHealth().isValid());
+
+ /*
+ * Init the first node
+ */
+ ASSERT_FALSE(node1.isStarted());
+ ASSERT_EQ(-uavcan::ErrNotInited, node1.spin(uavcan::MonotonicDuration::fromMSec(20)));
+ ASSERT_LE(0, node1.start());
+ ASSERT_TRUE(node1.isStarted());
+
+ ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(2000)));
+
+ ASSERT_EQ(1, node_status_monitor.findNodeWithWorstHealth().get());
+
+ /*
+ * Some logging
+ */
+ SubscriberWithCollector<uavcan::protocol::debug::LogMessage> log_sub(node2);
+ ASSERT_LE(0, log_sub.start());
+
+ node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+ node1.logInfo("test", "6 * 9 = 42");
+
+ ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20)));
+ ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20)));
+
+ ASSERT_TRUE(log_sub.collector.msg.get());
+ std::cout << *log_sub.collector.msg << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/publisher.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/publisher.hpp>
+#include <root_ns_a/MavlinkMessage.hpp>
+#include "../clock.hpp"
+#include "../transport/can/can.hpp"
+#include "test_node.hpp"
+
+
+TEST(Publisher, Basic)
+{
+ SystemClockMock clock_mock(100);
+ CanDriverMock can_driver(2, clock_mock);
+ TestNode node(can_driver, clock_mock, 1);
+
+ uavcan::Publisher<root_ns_a::MavlinkMessage> publisher(node);
+
+ ASSERT_FALSE(publisher.getTransferSender().isInitialized());
+
+ std::cout <<
+ "sizeof(uavcan::Publisher<root_ns_a::MavlinkMessage>): " <<
+ sizeof(uavcan::Publisher<root_ns_a::MavlinkMessage>) << std::endl;
+
+ // Manual type registration - we can't rely on the GDTR state
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::MavlinkMessage> _registrator;
+
+ /*
+ * Message layout:
+ * uint8 seq
+ * uint8 sysid
+ * uint8 compid
+ * uint8 msgid
+ * uint8[<256] payload
+ */
+ root_ns_a::MavlinkMessage msg;
+ msg.seq = 0x42;
+ msg.sysid = 0x72;
+ msg.compid = 0x08;
+ msg.msgid = 0xa5;
+ msg.payload = "Msg";
+
+ const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
+ const uint64_t tx_timeout_usec = uint64_t(publisher.getDefaultTxTimeout().toUSec());
+
+ /*
+ * Broadcast
+ */
+ {
+ ASSERT_LT(0, publisher.broadcast(msg));
+
+ // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
+ uavcan::Frame expected_frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+ node.getNodeID(), uavcan::NodeID::Broadcast, 0);
+ expected_frame.setPayload(expected_transfer_payload, 7);
+ expected_frame.setStartOfTransfer(true);
+ expected_frame.setEndOfTransfer(true);
+
+ uavcan::CanFrame expected_can_frame;
+ ASSERT_TRUE(expected_frame.compile(expected_can_frame));
+
+ ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
+ ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
+ ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
+ ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
+
+ // Second shot - checking the transfer ID
+ publisher.setPriority(10);
+ ASSERT_LT(0, publisher.broadcast(msg));
+
+ expected_frame = uavcan::Frame(root_ns_a::MavlinkMessage::DefaultDataTypeID,
+ uavcan::TransferTypeMessageBroadcast,
+ node.getNodeID(), uavcan::NodeID::Broadcast, 1);
+ expected_frame.setStartOfTransfer(true);
+ expected_frame.setEndOfTransfer(true);
+ expected_frame.setPayload(expected_transfer_payload, 7);
+ expected_frame.setPriority(10);
+ ASSERT_TRUE(expected_frame.compile(expected_can_frame));
+
+ ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
+ ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100));
+ ASSERT_TRUE(can_driver.ifaces[0].tx.empty());
+ ASSERT_TRUE(can_driver.ifaces[1].tx.empty());
+ }
+
+ clock_mock.advance(1000);
+
+ /*
+ * Misc
+ */
+ ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen());
+ ASSERT_TRUE(publisher.getTransferSender().isInitialized());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/scheduler.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include "../clock.hpp"
+#include "../transport/can/can.hpp"
+#include "test_node.hpp"
+
+#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION)
+# error UAVCAN_CPP_VERSION
+#endif
+
+struct TimerCallCounter
+{
+ std::vector<uavcan::TimerEvent> events_a;
+ std::vector<uavcan::TimerEvent> events_b;
+
+ void callA(const uavcan::TimerEvent& ev) { events_a.push_back(ev); }
+ void callB(const uavcan::TimerEvent& ev) { events_b.push_back(ev); }
+
+ typedef uavcan::MethodBinder<TimerCallCounter*, void (TimerCallCounter::*)(const uavcan::TimerEvent&)> Binder;
+
+ Binder bindA() { return Binder(this, &TimerCallCounter::callA); }
+ Binder bindB() { return Binder(this, &TimerCallCounter::callB); }
+};
+
+/*
+ * This test can fail on a non real time system. That's kinda sad but okay.
+ */
+TEST(Scheduler, Timers)
+{
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(2, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ /*
+ * Registration
+ */
+ {
+ TimerCallCounter tcc;
+ uavcan::TimerEventForwarder<TimerCallCounter::Binder> a(node, tcc.bindA());
+ uavcan::TimerEventForwarder<TimerCallCounter::Binder> b(node, tcc.bindB());
+
+ ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers());
+
+ const uavcan::MonotonicTime start_ts = clock_driver.getMonotonic();
+
+ a.startOneShotWithDeadline(start_ts + durMono(100000));
+ b.startPeriodic(durMono(1000));
+
+ ASSERT_EQ(2, node.getScheduler().getDeadlineScheduler().getNumHandlers());
+
+ /*
+ * Spinning
+ */
+ ASSERT_EQ(0, node.spin(start_ts + durMono(1000000)));
+
+ ASSERT_EQ(1, tcc.events_a.size());
+ ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, start_ts + durMono(100000)));
+ ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, tcc.events_a[0].real_time));
+
+ ASSERT_LT(900, tcc.events_b.size());
+ ASSERT_GT(1100, tcc.events_b.size());
+ {
+ uavcan::MonotonicTime next_expected_deadline = start_ts + durMono(1000);
+ for (unsigned i = 0; i < tcc.events_b.size(); i++)
+ {
+ ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, next_expected_deadline));
+ ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, tcc.events_b[i].real_time));
+ next_expected_deadline += durMono(1000);
+ }
+ }
+
+ /*
+ * Deinitialization
+ */
+ ASSERT_EQ(1, node.getScheduler().getDeadlineScheduler().getNumHandlers());
+
+ ASSERT_FALSE(a.isRunning());
+ ASSERT_EQ(uavcan::MonotonicDuration::getInfinite(), a.getPeriod());
+
+ ASSERT_TRUE(b.isRunning());
+ ASSERT_EQ(1000, b.getPeriod().toUSec());
+ }
+
+ ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); // Both timers were destroyed by now
+ ASSERT_EQ(0, node.spin(durMono(1000))); // Spin some more without timers
+}
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+TEST(Scheduler, TimerCpp11)
+{
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(2, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ int count = 0;
+
+ uavcan::Timer tm(node, [&count](const uavcan::TimerEvent&) { count++; });
+
+ ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers());
+ tm.startPeriodic(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_EQ(1, node.getScheduler().getDeadlineScheduler().getNumHandlers());
+
+ ASSERT_EQ(0, node.spin(uavcan::MonotonicDuration::fromMSec(100)));
+
+ std::cout << count << std::endl;
+ ASSERT_LE(5, count);
+ ASSERT_GE(15, count);
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/service_client.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,454 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/RestartNode.hpp>
+#include <uavcan/protocol/GetDataTypeInfo.hpp>
+#include <root_ns_a/StringService.hpp>
+#include <root_ns_a/EmptyService.hpp>
+#include <queue>
+#include <sstream>
+#include "test_node.hpp"
+
+
+template <typename DataType>
+struct ServiceCallResultHandler
+{
+ typedef typename uavcan::ServiceCallResult<DataType>::Status StatusType;
+ StatusType last_status;
+ uavcan::NodeID last_server_node_id;
+ typename DataType::Response last_response;
+ std::queue<typename DataType::Response> responses;
+
+ void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
+ {
+ std::cout << result << std::endl;
+ last_status = result.getStatus();
+ last_response = result.getResponse();
+ last_server_node_id = result.getCallID().server_node_id;
+ responses.push(result.getResponse());
+ }
+
+ bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const
+ {
+ if (status == last_status &&
+ server_node_id == last_server_node_id &&
+ response == last_response)
+ {
+ return true;
+ }
+ else
+ {
+ std::cout << "MISMATCH: status=" << int(last_status) << ", last_server_node_id="
+ << int(last_server_node_id.get()) << ", last response:\n" << last_response << std::endl;
+ return false;
+ }
+ }
+
+ typedef uavcan::MethodBinder<ServiceCallResultHandler*,
+ void (ServiceCallResultHandler::*)(const uavcan::ServiceCallResult<DataType>&)> Binder;
+
+ Binder bind() { return Binder(this, &ServiceCallResultHandler::handleResponse); }
+};
+
+
+static void stringServiceServerCallback(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& req,
+ uavcan::ServiceResponseDataStructure<root_ns_a::StringService::Response>& rsp)
+{
+ rsp.string_response = "Request string: ";
+ rsp.string_response += req.string_request;
+}
+
+static void rejectingStringServiceServerCallback(
+ const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& req,
+ uavcan::ServiceResponseDataStructure<root_ns_a::StringService::Response>& rsp)
+{
+ rsp.string_response = "Request string: ";
+ rsp.string_response += req.string_request;
+ ASSERT_TRUE(rsp.isResponseEnabled());
+ rsp.setResponseEnabled(false);
+ ASSERT_FALSE(rsp.isResponseEnabled());
+}
+
+static void emptyServiceServerCallback(const uavcan::ReceivedDataStructure<root_ns_a::EmptyService::Request>&,
+ uavcan::ServiceResponseDataStructure<root_ns_a::EmptyService::Response>&)
+{
+ // Nothing to do - the service is empty
+}
+
+
+TEST(ServiceClient, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ // Type registration
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
+
+ // Server
+ uavcan::ServiceServer<root_ns_a::StringService> server(nodes.a);
+ ASSERT_EQ(0, server.start(stringServiceServerCallback));
+
+ {
+ // Caller
+ typedef uavcan::ServiceCallResult<root_ns_a::StringService> ResultType;
+ typedef uavcan::ServiceClient<root_ns_a::StringService,
+ typename ServiceCallResultHandler<root_ns_a::StringService>::Binder > ClientType;
+ ServiceCallResultHandler<root_ns_a::StringService> handler;
+
+ ClientType client1(nodes.b);
+ ClientType client2(nodes.b);
+ ClientType client3(nodes.b);
+
+ ASSERT_EQ(0, client1.getNumPendingCalls());
+ ASSERT_EQ(0, client2.getNumPendingCalls());
+ ASSERT_EQ(0, client3.getNumPendingCalls());
+
+ ASSERT_FALSE(client1.hasPendingCallToServer(1));
+
+ client1.setCallback(handler.bind());
+ client2.setCallback(client1.getCallback());
+ client3.setCallback(client1.getCallback());
+ client3.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
+
+ ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners());
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening!
+
+ root_ns_a::StringService::Request request;
+ request.string_request = "Hello world";
+
+ std::cout << "!!! Calling!" << std::endl;
+
+ ASSERT_LT(0, client1.call(1, request)); // OK
+ ASSERT_LT(0, client1.call(1, request)); // OK - second request
+ ASSERT_LT(0, client2.call(1, request)); // OK
+ ASSERT_LT(0, client3.call(99, request)); // Will timeout!
+ ASSERT_LT(0, client3.call(1, request)); // OK - second request
+
+ ASSERT_TRUE(client1.hasPendingCallToServer(1));
+ ASSERT_TRUE(client2.hasPendingCallToServer(1));
+ ASSERT_TRUE(client3.hasPendingCallToServer(99));
+ ASSERT_TRUE(client3.hasPendingCallToServer(1));
+
+ std::cout << "!!! Spinning!" << std::endl;
+
+ ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now!
+
+ ASSERT_TRUE(client1.hasPendingCalls());
+ ASSERT_TRUE(client2.hasPendingCalls());
+ ASSERT_TRUE(client3.hasPendingCalls());
+
+ ASSERT_EQ(2, client1.getNumPendingCalls());
+ ASSERT_EQ(1, client2.getNumPendingCalls());
+ ASSERT_EQ(2, client3.getNumPendingCalls());
+
+ ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id);
+ ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20));
+
+ std::cout << "!!! Spin finished!" << std::endl;
+
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening!
+
+ ASSERT_FALSE(client1.hasPendingCalls());
+ ASSERT_FALSE(client2.hasPendingCalls());
+ ASSERT_TRUE(client3.hasPendingCalls());
+
+ ASSERT_EQ(0, client1.getNumPendingCalls());
+ ASSERT_EQ(0, client2.getNumPendingCalls());
+ ASSERT_EQ(1, client3.getNumPendingCalls()); // The one addressed to 99 is still waiting
+
+ // Validating
+ root_ns_a::StringService::Response expected_response;
+ expected_response.string_response = "Request string: Hello world";
+ ASSERT_TRUE(handler.match(ResultType::Success, 1, expected_response));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
+
+ ASSERT_FALSE(client1.hasPendingCalls());
+ ASSERT_FALSE(client2.hasPendingCalls());
+ ASSERT_FALSE(client3.hasPendingCalls());
+
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :(
+
+ // Validating
+ ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response()));
+
+ // Stray request
+ ASSERT_LT(0, client3.call(99, request)); // Will timeout!
+ ASSERT_TRUE(client3.hasPendingCalls());
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());
+ }
+
+ // All destroyed - nobody listening
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners());
+}
+
+
+TEST(ServiceClient, Rejection)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ // Type registration
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
+
+ // Server
+ uavcan::ServiceServer<root_ns_a::StringService> server(nodes.a);
+ ASSERT_EQ(0, server.start(rejectingStringServiceServerCallback));
+
+ // Caller
+ typedef uavcan::ServiceCallResult<root_ns_a::StringService> ResultType;
+ typedef uavcan::ServiceClient<root_ns_a::StringService,
+ typename ServiceCallResultHandler<root_ns_a::StringService>::Binder > ClientType;
+ ServiceCallResultHandler<root_ns_a::StringService> handler;
+
+ ClientType client1(nodes.b);
+ client1.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
+ client1.setCallback(handler.bind());
+
+ root_ns_a::StringService::Request request;
+ request.string_request = "Hello world";
+
+ ASSERT_LT(0, client1.call(1, request));
+
+ ASSERT_EQ(uavcan::NodeID(1), client1.getCallIDByIndex(0).server_node_id);
+ ASSERT_EQ(uavcan::NodeID(), client1.getCallIDByIndex(1).server_node_id);
+
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());
+ ASSERT_TRUE(client1.hasPendingCalls());
+ ASSERT_TRUE(client1.hasPendingCallToServer(1));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
+ ASSERT_FALSE(client1.hasPendingCalls());
+ ASSERT_FALSE(client1.hasPendingCallToServer(1));
+
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out
+
+ ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 1, root_ns_a::StringService::Response()));
+}
+
+
+TEST(ServiceClient, ConcurrentCalls)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ // Type registration
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
+
+ // Server
+ uavcan::ServiceServer<root_ns_a::StringService> server(nodes.a);
+ ASSERT_EQ(0, server.start(stringServiceServerCallback));
+
+ // Caller
+ typedef uavcan::ServiceCallResult<root_ns_a::StringService> ResultType;
+ typedef uavcan::ServiceClient<root_ns_a::StringService,
+ typename ServiceCallResultHandler<root_ns_a::StringService>::Binder > ClientType;
+ ServiceCallResultHandler<root_ns_a::StringService> handler;
+
+ /*
+ * Initializing
+ */
+ ClientType client(nodes.b);
+
+ ASSERT_EQ(0, client.getNumPendingCalls());
+
+ client.setCallback(handler.bind());
+
+ ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners());
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening!
+
+ ASSERT_FALSE(client.hasPendingCalls());
+ ASSERT_EQ(0, client.getNumPendingCalls());
+
+ /*
+ * Calling ten requests, the last one will be cancelled
+ * Note that there will be non-unique transfer ID values; the client must handle that correctly
+ */
+ uavcan::ServiceCallID last_call_id;
+ for (int i = 0; i < 10; i++)
+ {
+ std::ostringstream os;
+ os << i;
+ root_ns_a::StringService::Request request;
+ request.string_request = os.str().c_str();
+ ASSERT_LT(0, client.call(1, request, last_call_id));
+ }
+
+ ASSERT_LT(0, client.call(99, root_ns_a::StringService::Request())); // Will timeout in 1000 ms
+
+ client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
+
+ ASSERT_LT(0, client.call(88, root_ns_a::StringService::Request())); // Will timeout in 100 ms
+
+ ASSERT_TRUE(client.hasPendingCalls());
+ ASSERT_EQ(12, client.getNumPendingCalls());
+
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening
+
+ /*
+ * Cancelling one
+ */
+ client.cancelCall(last_call_id);
+
+ ASSERT_TRUE(client.hasPendingCalls());
+ ASSERT_EQ(11, client.getNumPendingCalls());
+
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening
+
+ /*
+ * Spinning
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20));
+
+ ASSERT_TRUE(client.hasPendingCalls());
+ ASSERT_EQ(2, client.getNumPendingCalls()); // Two still pending
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening
+
+ /*
+ * Validating the ones that didn't timeout
+ */
+ root_ns_a::StringService::Response last_response;
+ for (int i = 0; i < 9; i++)
+ {
+ std::ostringstream os;
+ os << "Request string: " << i;
+ last_response.string_response = os.str().c_str();
+
+ ASSERT_FALSE(handler.responses.empty());
+
+ ASSERT_STREQ(last_response.string_response.c_str(), handler.responses.front().string_response.c_str());
+
+ handler.responses.pop();
+ }
+
+ ASSERT_TRUE(handler.responses.empty());
+
+ /*
+ * Validating the 100 ms timeout
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+
+ ASSERT_TRUE(client.hasPendingCalls());
+ ASSERT_EQ(1, client.getNumPendingCalls()); // One dropped
+ ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening
+
+ ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, root_ns_a::StringService::Response()));
+
+ /*
+ * Validating the 1000 ms timeout
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+
+ ASSERT_FALSE(client.hasPendingCalls());
+ ASSERT_EQ(0, client.getNumPendingCalls()); // All finished
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Not listening
+
+ ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response()));
+}
+
+
+TEST(ServiceClient, Empty)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ // Type registration
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::EmptyService> _registrator;
+
+ // Server
+ uavcan::ServiceServer<root_ns_a::EmptyService> server(nodes.a);
+ ASSERT_EQ(0, server.start(emptyServiceServerCallback));
+
+ {
+ // Caller
+ typedef uavcan::ServiceClient<root_ns_a::EmptyService,
+ typename ServiceCallResultHandler<root_ns_a::EmptyService>::Binder > ClientType;
+ ServiceCallResultHandler<root_ns_a::EmptyService> handler;
+
+ ClientType client(nodes.b);
+
+ client.setCallback(handler.bind());
+
+ root_ns_a::EmptyService::Request request;
+
+ ASSERT_LT(0, client.call(1, request)); // OK
+ }
+
+ // All destroyed - nobody listening
+ ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners());
+}
+
+
+TEST(ServiceClient, Priority)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ // Type registration
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::EmptyService> _registrator;
+
+ // Initializing
+ typedef uavcan::ServiceClient<root_ns_a::EmptyService,
+ typename ServiceCallResultHandler<root_ns_a::EmptyService>::Binder > ClientType;
+ ServiceCallResultHandler<root_ns_a::EmptyService> handler;
+ ClientType client(nodes.b);
+ client.setCallback(handler.bind());
+
+ // Calling
+ root_ns_a::EmptyService::Request request;
+
+ client.setPriority(0);
+ ASSERT_LT(0, client.call(1, request)); // OK
+
+ client.setPriority(10);
+ ASSERT_LT(0, client.call(1, request)); // OK
+
+ client.setPriority(20);
+ ASSERT_LT(0, client.call(1, request)); // OK
+
+ client.setPriority(31);
+ ASSERT_LT(0, client.call(1, request)); // OK
+
+ // Validating
+ ASSERT_EQ(4, nodes.can_a.read_queue.size());
+
+ uavcan::Frame frame;
+
+ ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front()));
+ nodes.can_a.read_queue.pop();
+ ASSERT_EQ(0, frame.getPriority().get());
+
+ ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front()));
+ nodes.can_a.read_queue.pop();
+ ASSERT_EQ(10, frame.getPriority().get());
+
+ ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front()));
+ nodes.can_a.read_queue.pop();
+ ASSERT_EQ(20, frame.getPriority().get());
+
+ ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front()));
+ nodes.can_a.read_queue.pop();
+ ASSERT_EQ(31, frame.getPriority().get());
+}
+
+
+TEST(ServiceClient, Sizes)
+{
+ using namespace uavcan;
+
+ std::cout << "GetDataTypeInfo server: " <<
+ sizeof(ServiceServer<protocol::GetDataTypeInfo>) << std::endl;
+
+ std::cout << "RestartNode server: " <<
+ sizeof(ServiceServer<protocol::RestartNode>) << std::endl;
+
+ std::cout << "GetDataTypeInfo client: " <<
+ sizeof(ServiceClient<protocol::GetDataTypeInfo>) << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/service_server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <root_ns_a/StringService.hpp>
+#include <root_ns_a/EmptyService.hpp>
+#include "../clock.hpp"
+#include "../transport/can/can.hpp"
+#include "test_node.hpp"
+
+
+struct StringServerImpl
+{
+ const char* string_response;
+
+ StringServerImpl(const char* string_response) : string_response(string_response) { }
+
+ void handleRequest(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& request,
+ root_ns_a::StringService::Response& response)
+ {
+ std::cout << request << std::endl;
+ response.string_response = request.string_request;
+ response.string_response += " --> ";
+ response.string_response += string_response;
+ std::cout << response << std::endl;
+ }
+
+ typedef uavcan::MethodBinder<StringServerImpl*,
+ void (StringServerImpl::*)(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>&,
+ root_ns_a::StringService::Response&)> Binder;
+
+ Binder bind() { return Binder(this, &StringServerImpl::handleRequest); }
+};
+
+
+struct EmptyServerImpl
+{
+ void handleRequest(const uavcan::ReceivedDataStructure<root_ns_a::EmptyService::Request>& request,
+ root_ns_a::EmptyService::Response&)
+ {
+ std::cout << request << std::endl;
+ }
+
+ typedef uavcan::MethodBinder<EmptyServerImpl*,
+ void (EmptyServerImpl::*)(const uavcan::ReceivedDataStructure<root_ns_a::EmptyService::Request>&,
+ root_ns_a::EmptyService::Response&)> Binder;
+
+ Binder bind() { return Binder(this, &EmptyServerImpl::handleRequest); }
+};
+
+
+TEST(ServiceServer, Basic)
+{
+ // Manual type registration - we can't rely on the GDTR state
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
+
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(1, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ StringServerImpl impl("456");
+
+ {
+ uavcan::ServiceServer<root_ns_a::StringService, StringServerImpl::Binder> server(node);
+
+ ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners());
+ server.start(impl.bind());
+ ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners());
+
+ /*
+ * Request frames
+ */
+ for (uint8_t i = 0; i < 2; i++)
+ {
+ uavcan::Frame frame(root_ns_a::StringService::DefaultDataTypeID, uavcan::TransferTypeServiceRequest,
+ uavcan::NodeID(uint8_t(i + 0x10)), 1, i);
+
+ const uint8_t req[] = {'r', 'e', 'q', uint8_t(i + '0')};
+ frame.setPayload(req, sizeof(req));
+
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+ frame.setPriority(i);
+
+ uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
+ can_driver.ifaces[0].pushRx(rx_frame);
+ }
+
+ node.spin(clock_driver.getMonotonic() + uavcan::MonotonicDuration::fromUSec(10000));
+
+ /*
+ * Responses (MFT)
+ */
+ ASSERT_EQ(4, can_driver.ifaces[0].tx.size());
+ for (int i = 0; i < 2; i++)
+ {
+ char payloads[2][8];
+ std::snprintf(payloads[0], 8, "req%i ", i);
+ std::snprintf(payloads[1], 8, "--> 456");
+
+ // First frame
+ uavcan::Frame fr;
+ ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame()));
+ std::cout << fr.toString() << std::endl;
+ ASSERT_FALSE(std::strncmp(payloads[0], reinterpret_cast<const char*>(fr.getPayloadPtr() + 2), 5)); // No CRC
+
+ ASSERT_EQ(i, fr.getTransferID().get());
+ ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType());
+ ASSERT_EQ(i + 0x10, fr.getDstNodeID().get());
+ ASSERT_EQ(i, fr.getPriority().get());
+
+ // Second frame
+ ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame()));
+ std::cout << fr.toString() << std::endl;
+ // cppcheck-suppress arrayIndexOutOfBounds
+ ASSERT_FALSE(std::strncmp(payloads[1], reinterpret_cast<const char*>(fr.getPayloadPtr()), 7));
+
+ ASSERT_EQ(i, fr.getTransferID().get());
+ ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType());
+ ASSERT_EQ(i + 0x10, fr.getDstNodeID().get());
+ }
+
+ ASSERT_EQ(0, server.getRequestFailureCount());
+ ASSERT_EQ(0, server.getResponseFailureCount());
+
+ ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners());
+ }
+ ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners());
+}
+
+
+TEST(ServiceServer, Empty)
+{
+ // Manual type registration - we can't rely on the GDTR state
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::EmptyService> _registrator;
+
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(1, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ EmptyServerImpl impl;
+
+ uavcan::ServiceServer<root_ns_a::EmptyService, EmptyServerImpl::Binder> server(node);
+
+ std::cout << "sizeof(ServiceServer<root_ns_a::EmptyService, EmptyServerImpl::Binder>): "
+ << sizeof(uavcan::ServiceServer<root_ns_a::EmptyService, EmptyServerImpl::Binder>) << std::endl;
+
+ ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners());
+ ASSERT_GE(0, server.start(impl.bind()));
+ ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/sub_node.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/node.hpp>
+#include <uavcan/node/sub_node.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "test_node.hpp"
+#include "../protocol/helpers.hpp"
+
+static void registerTypes()
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg2;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg4;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetDataTypeInfo> _reg5;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::debug::LogMessage> _reg6;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetTransportStats> _reg7;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::RestartNode> _reg8;
+}
+
+
+TEST(SubNode, Basic)
+{
+ registerTypes();
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::protocol::SoftwareVersion swver;
+ swver.major = 0;
+ swver.minor = 1;
+ swver.vcs_commit = 0xDEADBEEF;
+
+ std::cout << "sizeof(uavcan::SubNode<0>): " << sizeof(uavcan::SubNode<0>) << std::endl;
+
+ /*
+ * uavcan::Node
+ */
+ uavcan::Node<1024> node1(nodes.can_a, nodes.clock_a);
+ node1.setName("com.example");
+ node1.setNodeID(1);
+ node1.setSoftwareVersion(swver);
+
+ /*
+ * uavcan::SubNode
+ */
+ uavcan::SubNode<1024> node2(nodes.can_b, nodes.clock_b);
+
+ BackgroundSpinner bgspinner(node2, node1);
+ bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10));
+
+ uavcan::NodeStatusMonitor node_status_monitor(node2);
+ ASSERT_LE(0, node_status_monitor.start());
+
+ /*
+ * Init the first node
+ */
+ ASSERT_FALSE(node1.isStarted());
+ ASSERT_EQ(-uavcan::ErrNotInited, node1.spin(uavcan::MonotonicDuration::fromMSec(20)));
+ ASSERT_LE(0, node1.start());
+ ASSERT_TRUE(node1.isStarted());
+
+ ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(2000)));
+
+ ASSERT_EQ(1, node_status_monitor.findNodeWithWorstHealth().get());
+
+ /*
+ * Some logging
+ */
+ SubscriberWithCollector<uavcan::protocol::debug::LogMessage> log_sub(node2);
+ ASSERT_LE(0, log_sub.start());
+
+ node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+ node1.logInfo("test", "6 * 9 = 42");
+
+ ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20)));
+ ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20)));
+
+ ASSERT_TRUE(log_sub.collector.msg.get());
+ std::cout << *log_sub.collector.msg << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/subscriber.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,282 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <root_ns_a/EmptyMessage.hpp>
+#include <root_ns_a/MavlinkMessage.hpp>
+#include "../clock.hpp"
+#include "../transport/can/can.hpp"
+#include "test_node.hpp"
+
+
+template <typename DataType>
+struct SubscriptionListener
+{
+ typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructure;
+
+ struct ReceivedDataStructureCopy
+ {
+ uavcan::MonotonicTime ts_monotonic;
+ uavcan::UtcTime ts_utc;
+ uavcan::TransferType transfer_type;
+ uavcan::TransferID transfer_id;
+ uavcan::NodeID src_node_id;
+ uavcan::uint8_t iface_index;
+ DataType msg;
+
+ ReceivedDataStructureCopy(const ReceivedDataStructure& s)
+ : ts_monotonic(s.getMonotonicTimestamp())
+ , ts_utc(s.getUtcTimestamp())
+ , transfer_type(s.getTransferType())
+ , transfer_id(s.getTransferID())
+ , src_node_id(s.getSrcNodeID())
+ , iface_index(s.getIfaceIndex())
+ , msg(s)
+ { }
+ };
+
+ std::vector<DataType> simple;
+ std::vector<ReceivedDataStructureCopy> extended;
+
+ void receiveExtended(ReceivedDataStructure& msg)
+ {
+ extended.push_back(msg);
+ }
+
+ void receiveSimple(DataType& msg)
+ {
+ simple.push_back(msg);
+ }
+
+ typedef SubscriptionListener<DataType> SelfType;
+ typedef uavcan::MethodBinder<SelfType*, void (SelfType::*)(ReceivedDataStructure&)> ExtendedBinder;
+ typedef uavcan::MethodBinder<SelfType*, void (SelfType::*)(DataType&)> SimpleBinder;
+
+ ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); }
+ SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); }
+};
+
+
+TEST(Subscriber, Basic)
+{
+ // Manual type registration - we can't rely on the GDTR state
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::MavlinkMessage> _registrator;
+
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(2, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ typedef SubscriptionListener<root_ns_a::MavlinkMessage> Listener;
+
+ uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder> sub_extended(node);
+ uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder> sub_extended2(node); // Not used
+ uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::SimpleBinder> sub_simple(node);
+ uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::SimpleBinder> sub_simple2(node); // Not used
+
+ std::cout <<
+ "sizeof(uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder>): " <<
+ sizeof(uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder>) << std::endl;
+
+ // Null binder - will fail
+ ASSERT_EQ(-uavcan::ErrInvalidParam, sub_extended.start(Listener::ExtendedBinder(UAVCAN_NULLPTR, UAVCAN_NULLPTR)));
+
+ Listener listener;
+
+ /*
+ * Message layout:
+ * uint8 seq
+ * uint8 sysid
+ * uint8 compid
+ * uint8 msgid
+ * uint8[<256] payload
+ */
+ root_ns_a::MavlinkMessage expected_msg;
+ expected_msg.seq = 0x42;
+ expected_msg.sysid = 0x72;
+ expected_msg.compid = 0x08;
+ expected_msg.msgid = 0xa5;
+ expected_msg.payload = "Msg";
+
+ const uint8_t transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
+
+ /*
+ * RxFrame generation
+ */
+ std::vector<uavcan::RxFrame> rx_frames;
+ for (uint8_t i = 0; i < 4; i++)
+ {
+ uavcan::TransferType tt = uavcan::TransferTypeMessageBroadcast;
+ uavcan::NodeID dni = (tt == uavcan::TransferTypeMessageBroadcast) ?
+ uavcan::NodeID::Broadcast : node.getDispatcher().getNodeID();
+ // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
+ uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, tt, uavcan::NodeID(uint8_t(i + 100)),
+ dni, i);
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+ frame.setPayload(transfer_payload, 7);
+ uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
+ rx_frames.push_back(rx_frame);
+ }
+
+ /*
+ * Reception
+ */
+ ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());
+
+ ASSERT_EQ(0, sub_extended.start(listener.bindExtended()));
+ ASSERT_EQ(0, sub_extended2.start(listener.bindExtended()));
+ ASSERT_EQ(0, sub_simple.start(listener.bindSimple()));
+ ASSERT_EQ(0, sub_simple2.start(listener.bindSimple()));
+
+ ASSERT_EQ(4, node.getDispatcher().getNumMessageListeners());
+
+ sub_extended2.stop(); // These are not used - making sure they aren't receiving anything
+ sub_simple2.stop();
+
+ ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners());
+
+ for (unsigned i = 0; i < rx_frames.size(); i++)
+ {
+ can_driver.ifaces[0].pushRx(rx_frames[i]);
+ can_driver.ifaces[1].pushRx(rx_frames[i]);
+ }
+
+ ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000)));
+
+ /*
+ * Validation
+ */
+ ASSERT_EQ(listener.extended.size(), rx_frames.size());
+ for (unsigned i = 0; i < rx_frames.size(); i++)
+ {
+ const Listener::ReceivedDataStructureCopy s = listener.extended.at(i);
+ ASSERT_TRUE(s.msg == expected_msg);
+ ASSERT_EQ(rx_frames[i].getSrcNodeID(), s.src_node_id);
+ ASSERT_EQ(rx_frames[i].getTransferID(), s.transfer_id);
+ ASSERT_EQ(rx_frames[i].getTransferType(), s.transfer_type);
+ ASSERT_EQ(rx_frames[i].getMonotonicTimestamp(), s.ts_monotonic);
+ ASSERT_EQ(rx_frames[i].getIfaceIndex(), s.iface_index);
+ }
+
+ ASSERT_EQ(listener.simple.size(), rx_frames.size());
+ for (unsigned i = 0; i < rx_frames.size(); i++)
+ {
+ ASSERT_TRUE(listener.simple.at(i) == expected_msg);
+ }
+
+ ASSERT_EQ(0, sub_extended.getFailureCount());
+ ASSERT_EQ(0, sub_simple.getFailureCount());
+
+ /*
+ * Unregistration
+ */
+ ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners());
+
+ sub_extended.stop();
+ sub_extended2.stop();
+ sub_simple.stop();
+ sub_simple2.stop();
+
+ ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());
+}
+
+
+static void panickingSink(const uavcan::ReceivedDataStructure<root_ns_a::MavlinkMessage>&)
+{
+ FAIL() << "I just went mad";
+}
+
+
+TEST(Subscriber, FailureCount)
+{
+ // Manual type registration - we can't rely on the GDTR state
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::MavlinkMessage> _registrator;
+
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(2, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ {
+ uavcan::Subscriber<root_ns_a::MavlinkMessage> sub(node);
+ ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());
+ sub.start(panickingSink);
+ ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners());
+
+ ASSERT_EQ(0, sub.getFailureCount());
+
+ for (uint8_t i = 0; i < 4; i++)
+ {
+ // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
+ uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+ uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i);
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+ // No payload - broken transfer
+ uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
+ can_driver.ifaces[0].pushRx(rx_frame);
+ can_driver.ifaces[1].pushRx(rx_frame);
+ }
+
+ ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000)));
+
+ ASSERT_EQ(4, sub.getFailureCount());
+
+ ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners()); // Still there
+ }
+ ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); // Removed
+}
+
+
+TEST(Subscriber, SingleFrameTransfer)
+{
+ // Manual type registration - we can't rely on the GDTR state
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<root_ns_a::EmptyMessage> _registrator;
+
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(2, clock_driver);
+ TestNode node(can_driver, clock_driver, 1);
+
+ typedef SubscriptionListener<root_ns_a::EmptyMessage> Listener;
+
+ uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder> sub(node);
+
+ std::cout <<
+ "sizeof(uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder>): " <<
+ sizeof(uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder>) << std::endl;
+
+ Listener listener;
+
+ sub.start(listener.bindSimple());
+
+ for (uint8_t i = 0; i < 4; i++)
+ {
+ // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
+ uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+ uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i);
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+ // No payload - message is empty
+ uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
+ can_driver.ifaces[0].pushRx(rx_frame);
+ can_driver.ifaces[1].pushRx(rx_frame);
+ }
+
+ ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000)));
+
+ ASSERT_EQ(0, sub.getFailureCount());
+
+ ASSERT_EQ(4, listener.simple.size());
+ for (unsigned i = 0; i < 4; i++)
+ {
+ ASSERT_TRUE(listener.simple.at(i) == root_ns_a::EmptyMessage());
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/test_node.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,270 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <uavcan/node/abstract_node.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
+#include <memory>
+#include <set>
+#include <queue>
+#include "../transport/can/can.hpp"
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/subscriber.hpp>
+
+struct TestNode : public uavcan::INode
+{
+ /*
+ * This class used to use the simple pool allocator instead:
+ * uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 1024, uavcan::MemPoolBlockSize> pool;
+ * It has been replaced because unlike the simple allocator, heap-based one is not tested as extensively.
+ * Moreover, heap based allocator prints and error message upon destruction if some memory has not been freed.
+ */
+ uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize> pool;
+ uavcan::Scheduler scheduler;
+ uint64_t internal_failure_count;
+
+ TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id) :
+ pool(1024),
+ scheduler(can_driver, pool, clock_driver),
+ internal_failure_count(0)
+ {
+ setNodeID(self_node_id);
+ }
+
+ virtual void registerInternalFailure(const char* msg)
+ {
+ std::cout << "TestNode internal failure: " << msg << std::endl;
+ internal_failure_count++;
+ }
+
+ virtual uavcan::IPoolAllocator& getAllocator() { return pool; }
+ virtual uavcan::Scheduler& getScheduler() { return scheduler; }
+ virtual const uavcan::Scheduler& getScheduler() const { return scheduler; }
+};
+
+
+struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
+{
+ uavcan::ISystemClock& clock;
+ std::set<PairableCanDriver*> others;
+ std::queue<uavcan::CanFrame> read_queue;
+ std::queue<uavcan::CanFrame> loopback_queue;
+ uint64_t error_count;
+
+ PairableCanDriver(uavcan::ISystemClock& clock)
+ : clock(clock)
+ , error_count(0)
+ { }
+
+ void linkTogether(PairableCanDriver* with)
+ {
+ this->others.insert(with);
+ with->others.insert(this);
+ others.erase(this);
+ }
+
+ virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index)
+ {
+ return (iface_index == 0) ? this : UAVCAN_NULLPTR;
+ }
+ virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const
+ {
+ return (iface_index == 0) ? this : UAVCAN_NULLPTR;
+ }
+
+ virtual uavcan::uint8_t getNumIfaces() const { return 1; }
+
+ virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime blocking_deadline)
+ {
+ if (inout_masks.read == 1)
+ {
+ inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0;
+ }
+ if (inout_masks.read || inout_masks.write)
+ {
+ return 1;
+ }
+ while (clock.getMonotonic() < blocking_deadline)
+ {
+ usleep(1000);
+ }
+ return 0;
+ }
+
+ virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags)
+ {
+ assert(!others.empty());
+ for (std::set<PairableCanDriver*>::iterator it = others.begin(); it != others.end(); ++it)
+ {
+ (*it)->read_queue.push(frame);
+ }
+ if (flags & uavcan::CanIOFlagLoopback)
+ {
+ loopback_queue.push(frame);
+ }
+ return 1;
+ }
+
+ virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
+ {
+ out_flags = 0;
+ if (loopback_queue.empty())
+ {
+ assert(read_queue.size());
+ out_frame = read_queue.front();
+ read_queue.pop();
+ }
+ else
+ {
+ out_flags |= uavcan::CanIOFlagLoopback;
+ out_frame = loopback_queue.front();
+ loopback_queue.pop();
+ }
+ out_ts_monotonic = clock.getMonotonic();
+ out_ts_utc = clock.getUtc();
+ return 1;
+ }
+
+ void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
+ {
+ read_queue.push(can_frame);
+ }
+
+ virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; }
+ virtual uavcan::uint16_t getNumFilters() const { return 0; }
+ virtual uavcan::uint64_t getErrorCount() const { return error_count; }
+};
+
+
+template <typename ClockType>
+struct InterlinkedTestNodes
+{
+ ClockType clock_a;
+ ClockType clock_b;
+ PairableCanDriver can_a;
+ PairableCanDriver can_b;
+ TestNode a;
+ TestNode b;
+
+ InterlinkedTestNodes(uavcan::NodeID nid_first, uavcan::NodeID nid_second)
+ : can_a(clock_a)
+ , can_b(clock_b)
+ , a(can_a, clock_a, nid_first)
+ , b(can_b, clock_b, nid_second)
+ {
+ can_a.linkTogether(&can_b);
+ }
+
+ InterlinkedTestNodes()
+ : can_a(clock_a)
+ , can_b(clock_b)
+ , a(can_a, clock_a, 1)
+ , b(can_b, clock_b, 2)
+ {
+ can_a.linkTogether(&can_b);
+ }
+
+ int spinBoth(uavcan::MonotonicDuration duration)
+ {
+ assert(!duration.isNegative());
+ unsigned nspins2 = unsigned(duration.toMSec() / 2);
+ nspins2 = nspins2 ? nspins2 : 1;
+ while (nspins2 --> 0)
+ {
+ int ret = a.spin(uavcan::MonotonicDuration::fromMSec(1));
+ if (ret < 0)
+ {
+ return ret;
+ }
+ ret = b.spin(uavcan::MonotonicDuration::fromMSec(1));
+ if (ret < 0)
+ {
+ return ret;
+ }
+ }
+ return 0;
+ }
+};
+
+
+typedef InterlinkedTestNodes<SystemClockDriver> InterlinkedTestNodesWithSysClock;
+typedef InterlinkedTestNodes<SystemClockMock> InterlinkedTestNodesWithClockMock;
+
+
+template <unsigned NumNodes>
+struct TestNetwork
+{
+ struct NodeEnvironment
+ {
+ SystemClockDriver clock;
+ PairableCanDriver can_driver;
+ TestNode node;
+
+ NodeEnvironment(uavcan::NodeID node_id)
+ : can_driver(clock)
+ , node(can_driver, clock, node_id)
+ { }
+ };
+
+ std::auto_ptr<NodeEnvironment> nodes[NumNodes];
+
+ TestNetwork(uavcan::uint8_t first_node_id = 1)
+ {
+ for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+ {
+ nodes[i].reset(new NodeEnvironment(uint8_t(first_node_id + i)));
+ }
+
+ for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+ {
+ for (uavcan::uint8_t k = 0; k < NumNodes; k++)
+ {
+ nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver);
+ }
+ }
+
+ for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+ {
+ assert(nodes[i]->can_driver.others.size() == (NumNodes - 1));
+ }
+ }
+
+ int spinAll(uavcan::MonotonicDuration duration)
+ {
+ assert(!duration.isNegative());
+ unsigned nspins = unsigned(duration.toMSec() / NumNodes);
+ nspins = nspins ? nspins : 1;
+ while (nspins --> 0)
+ {
+ for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+ {
+ int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1));
+ if (ret < 0)
+ {
+ return ret;
+ }
+ }
+ }
+ return 0;
+ }
+
+ TestNode& operator[](unsigned index)
+ {
+ if (index >= NumNodes)
+ {
+ throw std::out_of_range("No such test node");
+ }
+ return nodes[index]->node;
+ }
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/node/test_node_test.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "test_node.hpp"
+
+
+TEST(TestNode, TestNetwork)
+{
+ TestNetwork<4> nwk;
+
+ uavcan::CanFrame frame;
+ for (uint8_t i = 0; i < 8; i++)
+ {
+ frame.data[i] = i;
+ }
+ frame.id = 1234U;
+
+ ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags()));
+
+ for (int i = 1; i < 4; i++)
+ {
+ uavcan::CanFrame rx;
+ uavcan::MonotonicTime ts_mono;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags = 0;
+ ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags));
+
+ ASSERT_TRUE(rx == frame);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/data_type_info_provider.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/protocol/data_type_info_provider.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+#include "helpers.hpp"
+
+using uavcan::protocol::GetDataTypeInfo;
+using uavcan::protocol::NodeStatus;
+using uavcan::protocol::DataTypeKind;
+using uavcan::ServiceCallResult;
+using uavcan::DataTypeInfoProvider;
+using uavcan::GlobalDataTypeRegistry;
+using uavcan::DefaultDataTypeRegistrator;
+using uavcan::MonotonicDuration;
+
+template <typename DataType>
+static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResultCollector<GetDataTypeInfo>::Result>& resp,
+ unsigned flags)
+{
+ if (!resp.get())
+ {
+ std::cout << "Null response" << std::endl;
+ return false;
+ }
+ if (!resp->isSuccessful())
+ {
+ std::cout << "Request was not successful" << std::endl;
+ return false;
+ }
+ if (resp->getResponse().name != DataType::getDataTypeFullName())
+ {
+ std::cout << "Type name mismatch: '"
+ << resp->getResponse().name.c_str() << "' '"
+ << DataType::getDataTypeFullName() << "'" << std::endl;
+ return false;
+ }
+ if (DataType::getDataTypeSignature().get() != resp->getResponse().signature)
+ {
+ std::cout << "Signature mismatch" << std::endl;
+ return false;
+ }
+ if (resp->getResponse().flags != flags)
+ {
+ std::cout << "Mask mismatch" << std::endl;
+ return false;
+ }
+ if (resp->getResponse().kind.value != DataType::DataTypeKind)
+ {
+ std::cout << "Kind mismatch" << std::endl;
+ return false;
+ }
+ if (resp->getResponse().id != DataType::DefaultDataTypeID)
+ {
+ std::cout << "DTID mismatch" << std::endl;
+ return false;
+ }
+ return true;
+}
+
+
+TEST(DataTypeInfoProvider, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ DataTypeInfoProvider dtip(nodes.a);
+
+ GlobalDataTypeRegistry::instance().reset();
+ DefaultDataTypeRegistrator<GetDataTypeInfo> _reg1;
+ DefaultDataTypeRegistrator<NodeStatus> _reg3;
+
+ ASSERT_LE(0, dtip.start());
+
+ ServiceClientWithCollector<GetDataTypeInfo> gdti_cln(nodes.b);
+
+ /*
+ * GetDataTypeInfo request for GetDataTypeInfo
+ */
+ GetDataTypeInfo::Request gdti_request;
+ gdti_request.id = GetDataTypeInfo::DefaultDataTypeID;
+ gdti_request.kind.value = DataTypeKind::SERVICE;
+ ASSERT_LE(0, gdti_cln.call(1, gdti_request));
+ nodes.spinBoth(MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(validateDataTypeInfoResponse<GetDataTypeInfo>(gdti_cln.collector.result,
+ GetDataTypeInfo::Response::FLAG_KNOWN |
+ GetDataTypeInfo::Response::FLAG_SERVING));
+ ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
+
+ /*
+ * GetDataTypeInfo request for GetDataTypeInfo by name
+ */
+ gdti_request = GetDataTypeInfo::Request();
+ gdti_request.id = 999; // Intentionally wrong
+ gdti_request.kind.value = DataTypeKind::MESSAGE; // Intentionally wrong
+ gdti_request.name = "uavcan.protocol.GetDataTypeInfo";
+ ASSERT_LE(0, gdti_cln.call(1, gdti_request));
+ nodes.spinBoth(MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(validateDataTypeInfoResponse<GetDataTypeInfo>(gdti_cln.collector.result,
+ GetDataTypeInfo::Response::FLAG_KNOWN |
+ GetDataTypeInfo::Response::FLAG_SERVING));
+ ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
+
+ /*
+ * GetDataTypeInfo request for NodeStatus - not used yet
+ */
+ gdti_request = GetDataTypeInfo::Request();
+ gdti_request.id = NodeStatus::DefaultDataTypeID;
+ gdti_request.kind.value = DataTypeKind::MESSAGE;
+ ASSERT_LE(0, gdti_cln.call(1, gdti_request));
+ nodes.spinBoth(MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(validateDataTypeInfoResponse<NodeStatus>(gdti_cln.collector.result,
+ GetDataTypeInfo::Response::FLAG_KNOWN));
+
+ /*
+ * Starting publisher and subscriber for NodeStatus, requesting info again
+ */
+ uavcan::Publisher<NodeStatus> ns_pub(nodes.a);
+ SubscriberWithCollector<NodeStatus> ns_sub(nodes.a);
+
+ ASSERT_LE(0, ns_pub.broadcast(NodeStatus()));
+ ASSERT_LE(0, ns_sub.start());
+
+ // Request again
+ ASSERT_LE(0, gdti_cln.call(1, gdti_request));
+ nodes.spinBoth(MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(validateDataTypeInfoResponse<NodeStatus>(gdti_cln.collector.result,
+ GetDataTypeInfo::Response::FLAG_KNOWN |
+ GetDataTypeInfo::Response::FLAG_PUBLISHING |
+ GetDataTypeInfo::Response::FLAG_SUBSCRIBED));
+
+ /*
+ * Requesting a non-existent type
+ */
+ gdti_request = GetDataTypeInfo::Request();
+ gdti_request.id = 20000;
+ gdti_request.kind.value = 3; // INVALID VALUE
+ ASSERT_LE(0, gdti_cln.call(1, gdti_request));
+ nodes.spinBoth(MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(gdti_cln.collector.result.get());
+ ASSERT_TRUE(gdti_cln.collector.result->isSuccessful());
+ ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
+ ASSERT_EQ(0, gdti_cln.collector.result->getResponse().flags);
+ ASSERT_TRUE(gdti_cln.collector.result->getResponse().name.empty()); // Empty name
+ ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->getResponse().id);
+ ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->getResponse().kind.value);
+
+ /*
+ * Requesting a non-existent type by name
+ */
+ gdti_request = GetDataTypeInfo::Request();
+ gdti_request.id = 999; // Intentionally wrong
+ gdti_request.kind.value = 3; // Intentionally wrong
+ gdti_request.name = "uavcan.equipment.gnss.Fix";
+ ASSERT_LE(0, gdti_cln.call(1, gdti_request));
+ nodes.spinBoth(MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(gdti_cln.collector.result.get());
+ ASSERT_TRUE(gdti_cln.collector.result->isSuccessful());
+ ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
+ ASSERT_EQ(0, gdti_cln.collector.result->getResponse().flags);
+ ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->getResponse().name);
+ ASSERT_EQ(0, gdti_cln.collector.result->getResponse().id);
+ ASSERT_EQ(0, gdti_cln.collector.result->getResponse().kind.value);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_client.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "helpers.hpp"
+
+
+TEST(DynamicNodeIDClient, Basic)
+{
+ // Node A is Allocator, Node B is Allocatee
+ InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+ uavcan::DynamicNodeIDClient dnidac(nodes.b);
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
+ (void)_reg1;
+
+ /*
+ * Client initialization
+ */
+ uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
+
+ ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id)); // Empty hardware version is not allowed
+
+ for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
+ {
+ unique_id[i] = i;
+ }
+
+ ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id, uavcan::NodeID()));
+
+ const uavcan::NodeID PreferredNodeID = 42;
+ ASSERT_LE(0, dnidac.start(unique_id, PreferredNodeID));
+
+ ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+ ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+ ASSERT_FALSE(dnidac.isAllocationComplete());
+
+ /*
+ * Subscriber (server emulation)
+ */
+ SubscriberWithCollector<uavcan::protocol::dynamic_node_id::Allocation> dynid_sub(nodes.a);
+ ASSERT_LE(0, dynid_sub.start());
+ dynid_sub.subscriber.allowAnonymousTransfers();
+
+ /*
+ * Monitoring requests
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
+ ASSERT_TRUE(dynid_sub.collector.msg.get());
+ std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl;
+ ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+ ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id);
+ ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
+ dynid_sub.collector.msg->unique_id.end(),
+ unique_id.begin()));
+ dynid_sub.collector.msg.reset();
+
+ // Second - rate is no lower than 0.5 Hz
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
+ ASSERT_TRUE(dynid_sub.collector.msg.get());
+ dynid_sub.collector.msg.reset();
+
+ ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+ ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+ ASSERT_FALSE(dnidac.isAllocationComplete());
+
+ /*
+ * Publisher (server emulation)
+ */
+ uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation> dynid_pub(nodes.a);
+ ASSERT_LE(0, dynid_pub.init());
+
+ /*
+ * Sending some some Allocation messages - the timer will keep restarting
+ */
+ for (int i = 0; i < 10; i++)
+ {
+ uavcan::protocol::dynamic_node_id::Allocation msg; // Contents of the message doesn't matter
+ ASSERT_LE(0, dynid_pub.broadcast(msg));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210));
+ ASSERT_FALSE(dynid_sub.collector.msg.get());
+ }
+
+ /*
+ * Responding with partially matching unique ID - the client will respond with second-stage request immediately
+ */
+ const uint8_t BytesPerRequest = uavcan::protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
+ {
+ uavcan::protocol::dynamic_node_id::Allocation msg;
+ msg.unique_id.resize(BytesPerRequest);
+ uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest, msg.unique_id.begin());
+
+ std::cout << "First-stage offer:\n" << msg << std::endl;
+
+ ASSERT_FALSE(dynid_sub.collector.msg.get());
+ ASSERT_LE(0, dynid_pub.broadcast(msg));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
+
+ ASSERT_TRUE(dynid_sub.collector.msg.get());
+ std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl;
+ ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+ ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id);
+ ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
+ dynid_sub.collector.msg->unique_id.end(),
+ unique_id.begin() + BytesPerRequest));
+ dynid_sub.collector.msg.reset();
+ }
+
+ /*
+ * Responding with second-stage offer, expecting the last request back
+ */
+ {
+ uavcan::protocol::dynamic_node_id::Allocation msg;
+ msg.unique_id.resize(BytesPerRequest * 2);
+ uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest * 2, msg.unique_id.begin());
+
+ std::cout << "Second-stage offer:\n" << msg << std::endl;
+
+ ASSERT_FALSE(dynid_sub.collector.msg.get());
+ ASSERT_LE(0, dynid_pub.broadcast(msg));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
+
+ ASSERT_TRUE(dynid_sub.collector.msg.get());
+ std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl;
+ ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+ ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id);
+ ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
+ dynid_sub.collector.msg->unique_id.end(),
+ unique_id.begin() + BytesPerRequest * 2));
+ dynid_sub.collector.msg.reset();
+ }
+
+ ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+ ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+ ASSERT_FALSE(dnidac.isAllocationComplete());
+
+ /*
+ * Now we have full unique ID for this client received, and it is possible to grant allocation
+ */
+ {
+ uavcan::protocol::dynamic_node_id::Allocation msg;
+ msg.unique_id.resize(16);
+ msg.node_id = 72;
+ uavcan::copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
+
+ ASSERT_FALSE(dynid_sub.collector.msg.get());
+ ASSERT_LE(0, dynid_pub.broadcast(msg));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+ ASSERT_FALSE(dynid_sub.collector.msg.get());
+ }
+
+ ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID());
+ ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID());
+ ASSERT_TRUE(dnidac.isAllocationComplete());
+}
+
+
+TEST(DynamicNodeIDClient, NonPassiveMode)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::DynamicNodeIDClient dnidac(nodes.b);
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
+ (void)_reg1;
+
+ uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
+ for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
+ {
+ unique_id[i] = i;
+ }
+
+ ASSERT_LE(-uavcan::ErrLogic, dnidac.start(unique_id));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,117 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "event_tracer.hpp"
+#include "../helpers.hpp"
+
+
+using uavcan::dynamic_node_id_server::UniqueID;
+
+class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler
+{
+ std::vector<std::pair<UniqueID, uavcan::NodeID> > requests_;
+
+public:
+ bool can_followup;
+
+ AllocationRequestHandler() : can_followup(false) { }
+
+ virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id)
+ {
+ requests_.push_back(std::pair<UniqueID, uavcan::NodeID>(unique_id, preferred_node_id));
+ }
+
+ virtual bool canPublishFollowupAllocationResponse() const
+ {
+ return can_followup;
+ }
+
+ bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id)
+ {
+ if (requests_.empty())
+ {
+ std::cout << "No pending requests" << std::endl;
+ return false;
+ }
+
+ const std::pair<UniqueID, uavcan::NodeID> pair = requests_.at(requests_.size() - 1U);
+ requests_.pop_back();
+
+ if (pair.first != unique_id)
+ {
+ std::cout << "Unique ID mismatch" << std::endl;
+ return false;
+ }
+
+ if (pair.second != preferred_node_id)
+ {
+ std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")"
+ << std::endl;
+ return false;
+ }
+
+ return true;
+ }
+
+ void reset() { requests_.clear(); }
+};
+
+
+TEST(dynamic_node_id_server_AllocationRequestManager, Basic)
+{
+ using namespace uavcan::protocol::dynamic_node_id;
+ using namespace uavcan::protocol::dynamic_node_id::server;
+ using namespace uavcan::dynamic_node_id_server;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<Allocation> _reg1;
+
+ // Node A is Allocator, Node B is Allocatee
+ InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+ uavcan::DynamicNodeIDClient client(nodes.b);
+
+ /*
+ * Client initialization
+ */
+ uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
+ for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
+ {
+ unique_id[i] = i;
+ }
+ const uavcan::NodeID PreferredNodeID = 42;
+ ASSERT_LE(0, client.start(unique_id, PreferredNodeID));
+
+ /*
+ * Request manager initialization
+ */
+ EventTracer tracer;
+ AllocationRequestHandler handler;
+ handler.can_followup = true;
+
+ AllocationRequestManager manager(nodes.a, tracer, handler);
+
+ ASSERT_LE(0, manager.init(uavcan::TransferPriority::OneHigherThanLowest));
+
+ /*
+ * Allocation
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+
+ ASSERT_TRUE(handler.matchAndPopLastRequest(unique_id, PreferredNodeID));
+
+ ASSERT_LE(0, manager.broadcastAllocationResponse(unique_id, PreferredNodeID));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+
+ /*
+ * Checking the client
+ */
+ ASSERT_TRUE(client.isAllocationComplete());
+
+ ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "../../helpers.hpp"
+#include "../event_tracer.hpp"
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+using uavcan::dynamic_node_id_server::UniqueID;
+
+
+TEST(dynamic_node_id_server_centralized_Server, Basic)
+{
+ using namespace uavcan::dynamic_node_id_server;
+ using namespace uavcan::protocol::dynamic_node_id;
+ using namespace uavcan::protocol::dynamic_node_id::server;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<Allocation> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+
+ // Node A is Allocator, Node B is Allocatee
+ InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+ UniqueID own_unique_id;
+ own_unique_id[0] = 0xAA;
+ own_unique_id[3] = 0xCC;
+ own_unique_id[7] = 0xEE;
+ own_unique_id[9] = 0xBB;
+
+ /*
+ * Server
+ */
+ uavcan::dynamic_node_id_server::CentralizedServer server(nodes.a, storage, tracer);
+
+ ASSERT_LE(0, server.init(own_unique_id));
+
+ ASSERT_EQ(1, server.getNumAllocations()); // Server's own node ID
+
+ /*
+ * Client
+ */
+ uavcan::DynamicNodeIDClient client(nodes.b);
+ uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
+ for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
+ {
+ unique_id[i] = i;
+ }
+ const uavcan::NodeID PreferredNodeID = 42;
+ ASSERT_LE(0, client.start(unique_id, PreferredNodeID));
+
+ /*
+ * Fire
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000));
+
+ ASSERT_TRUE(client.isAllocationComplete());
+ ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
+
+ ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client
+}
+
+
+TEST(dynamic_node_id_server_centralized, ObjectSizes)
+{
+ using namespace uavcan::dynamic_node_id_server;
+ std::cout << "centralized::Storage: " << sizeof(centralized::Storage) << std::endl;
+ std::cout << "centralized::Server: " << sizeof(centralized::Server) << std::endl;
+ std::cout << "NodeDiscoverer: " << sizeof(NodeDiscoverer) << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp>
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+
+TEST(dynamic_node_id_server_centralized_Storage, Initialization)
+{
+ using namespace uavcan::dynamic_node_id_server::centralized;
+ using namespace uavcan::dynamic_node_id_server;
+
+ // No data in the storage - initializing empty
+ {
+ MemoryStorageBackend storage;
+ Storage stor(storage);
+
+ ASSERT_EQ(0, storage.getNumKeys());
+ ASSERT_LE(0, stor.init());
+
+ ASSERT_EQ(0, storage.getNumKeys());
+ ASSERT_EQ(0, stor.getSize());
+
+ ASSERT_FALSE(stor.isNodeIDOccupied(1));
+ ASSERT_FALSE(stor.isNodeIDOccupied(0));
+ }
+ // Nonempty storage, many items
+ {
+ MemoryStorageBackend storage;
+ Storage stor(storage);
+
+ storage.set("occupation_mask", "0e000000000000000000000000000000"); // node ID 1, 2, 3
+ ASSERT_LE(0, stor.init()); // OK
+
+ ASSERT_EQ(1, storage.getNumKeys());
+ ASSERT_EQ(3, stor.getSize());
+
+ ASSERT_TRUE(stor.isNodeIDOccupied(1));
+ ASSERT_TRUE(stor.isNodeIDOccupied(2));
+ ASSERT_TRUE(stor.isNodeIDOccupied(3));
+ ASSERT_FALSE(stor.isNodeIDOccupied(0));
+ ASSERT_FALSE(stor.isNodeIDOccupied(4));
+
+ UniqueID uid_1;
+ uid_1[0] = 1;
+
+ UniqueID uid_2;
+ uid_2[0] = 2;
+
+ UniqueID uid_3;
+ uid_3[0] = 3;
+
+ ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_1).isValid());
+ ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_2).isValid());
+ ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_3).isValid());
+ ASSERT_FALSE(stor.isNodeIDOccupied(127));
+
+ storage.set("01000000000000000000000000000000", "1");
+ storage.set("02000000000000000000000000000000", "2");
+ storage.set("03000000000000000000000000000000", "3");
+
+ ASSERT_EQ(1, stor.getNodeIDForUniqueID(uid_1).get());
+ ASSERT_EQ(2, stor.getNodeIDForUniqueID(uid_2).get());
+ ASSERT_EQ(3, stor.getNodeIDForUniqueID(uid_3).get());
+ ASSERT_FALSE(stor.isNodeIDOccupied(127));
+ }
+}
+
+
+TEST(dynamic_node_id_server_centralized_Storage, Basic)
+{
+ using namespace uavcan::dynamic_node_id_server::centralized;
+
+ MemoryStorageBackend storage;
+ Storage stor(storage);
+
+ ASSERT_EQ(0, storage.getNumKeys());
+ ASSERT_LE(0, stor.init());
+ storage.print();
+ ASSERT_EQ(0, storage.getNumKeys());
+
+ /*
+ * Adding one entry to the log, making sure it appears in the storage
+ */
+ uavcan::dynamic_node_id_server::UniqueID unique_id;
+ unique_id[0] = 1;
+ ASSERT_LE(0, stor.add(1, unique_id));
+
+ ASSERT_EQ("02000000000000000000000000000000", storage.get("occupation_mask"));
+ ASSERT_EQ("1", storage.get("01000000000000000000000000000000"));
+
+ ASSERT_EQ(2, storage.getNumKeys());
+ ASSERT_EQ(1, stor.getSize());
+
+ /*
+ * Adding another entry while storage is failing
+ */
+ storage.failOnSetCalls(true);
+
+ ASSERT_EQ(2, storage.getNumKeys());
+
+ unique_id[0] = 2;
+ ASSERT_GT(0, stor.add(2, unique_id));
+
+ ASSERT_EQ(2, storage.getNumKeys()); // No new entries, we failed
+
+ ASSERT_EQ(1, stor.getSize());
+
+ /*
+ * Adding a lot of entries
+ */
+ storage.failOnSetCalls(false);
+
+ uavcan::NodeID node_id(2);
+ while (stor.getSize() < 127)
+ {
+ ASSERT_LE(0, stor.add(node_id, unique_id));
+
+ ASSERT_TRUE(stor.getNodeIDForUniqueID(unique_id).isValid());
+ ASSERT_TRUE(stor.isNodeIDOccupied(node_id));
+
+ node_id = uint8_t(uavcan::min(node_id.get() + 1U, 127U));
+ unique_id[0] = uint8_t(unique_id[0] + 1U);
+ }
+
+ storage.print();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,262 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp>
+#include "../event_tracer.hpp"
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+TEST(dynamic_node_id_server_ClusterManager, Initialization)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ const unsigned MaxClusterSize =
+ uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::server::Discovery> _reg1;
+
+ EventTracer tracer;
+
+ /*
+ * Simple initialization
+ */
+ {
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+ InterlinkedTestNodesWithSysClock nodes;
+
+ ClusterManager mgr(nodes.a, storage, log, tracer);
+
+ // Too big
+ ASSERT_GT(0, mgr.init(MaxClusterSize + 1, uavcan::TransferPriority::OneHigherThanLowest));
+ ASSERT_EQ(0, storage.getNumKeys());
+
+ // OK
+ ASSERT_LE(0, mgr.init(5, uavcan::TransferPriority::OneHigherThanLowest));
+ ASSERT_EQ(1, storage.getNumKeys());
+ ASSERT_EQ("5", storage.get("cluster_size"));
+
+ // Testing other states
+ ASSERT_EQ(0, mgr.getNumKnownServers());
+ ASSERT_EQ(5, mgr.getClusterSize());
+ ASSERT_EQ(3, mgr.getQuorumSize());
+ ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid());
+ }
+ /*
+ * Recovery from the storage
+ */
+ {
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+ InterlinkedTestNodesWithSysClock nodes;
+
+ ClusterManager mgr(nodes.a, storage, log, tracer);
+
+ // Not configured
+ ASSERT_GT(0, mgr.init(0, uavcan::TransferPriority::OneHigherThanLowest));
+ ASSERT_EQ(0, storage.getNumKeys());
+
+ // OK
+ storage.set("cluster_size", "5");
+ ASSERT_LE(0, mgr.init(0, uavcan::TransferPriority::OneHigherThanLowest));
+ ASSERT_EQ(1, storage.getNumKeys());
+ }
+}
+
+
+TEST(dynamic_node_id_server_ClusterManager, OneServer)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::server::Discovery> _reg1;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+ InterlinkedTestNodesWithSysClock nodes;
+
+ ClusterManager mgr(nodes.a, storage, log, tracer);
+
+ /*
+ * Pub and sub
+ */
+ SubscriberWithCollector<uavcan::protocol::dynamic_node_id::server::Discovery> sub(nodes.b);
+ uavcan::Publisher<uavcan::protocol::dynamic_node_id::server::Discovery> pub(nodes.b);
+
+ ASSERT_LE(0, sub.start());
+ ASSERT_LE(0, pub.init());
+
+ /*
+ * Starting
+ */
+ ASSERT_LE(0, mgr.init(1, uavcan::TransferPriority::OneHigherThanLowest));
+
+ ASSERT_EQ(0, mgr.getNumKnownServers());
+ ASSERT_TRUE(mgr.isClusterDiscovered());
+
+ ASSERT_EQ(0, nodes.a.internal_failure_count);
+
+ /*
+ * Broadcasting discovery with wrong cluster size, it will be reported as internal failure
+ */
+ uavcan::protocol::dynamic_node_id::server::Discovery msg;
+ msg.configured_cluster_size = 2;
+ msg.known_nodes.push_back(2U);
+ ASSERT_LE(0, pub.broadcast(msg));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_EQ(1, nodes.a.internal_failure_count);
+
+ /*
+ * Discovery rate limiting test
+ */
+ ASSERT_FALSE(sub.collector.msg.get());
+
+ msg = uavcan::protocol::dynamic_node_id::server::Discovery();
+ msg.configured_cluster_size = 1; // Correct value
+ ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+ ASSERT_FALSE(sub.collector.msg.get());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_EQ(1, sub.collector.msg->configured_cluster_size);
+ ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
+ ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
+ sub.collector.msg.reset();
+
+ // Rinse repeat
+ ASSERT_LE(0, pub.broadcast(msg));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+ ASSERT_FALSE(sub.collector.msg.get());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_EQ(1, sub.collector.msg->configured_cluster_size);
+ ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
+ ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
+ sub.collector.msg.reset();
+}
+
+
+TEST(dynamic_node_id_server_ClusterManager, ThreeServers)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::server::Discovery> _reg1;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+ InterlinkedTestNodesWithSysClock nodes;
+
+ ClusterManager mgr(nodes.a, storage, log, tracer);
+
+ /*
+ * Pub and sub
+ */
+ SubscriberWithCollector<uavcan::protocol::dynamic_node_id::server::Discovery> sub(nodes.b);
+ uavcan::Publisher<uavcan::protocol::dynamic_node_id::server::Discovery> pub(nodes.b);
+
+ ASSERT_LE(0, sub.start());
+ ASSERT_LE(0, pub.init());
+
+ /*
+ * Starting
+ */
+ ASSERT_LE(0, mgr.init(3, uavcan::TransferPriority::OneHigherThanLowest));
+
+ ASSERT_EQ(0, mgr.getNumKnownServers());
+ ASSERT_FALSE(mgr.isClusterDiscovered());
+
+ /*
+ * Discovery publishing rate check
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+ ASSERT_FALSE(sub.collector.msg.get());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
+ ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
+ ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
+ sub.collector.msg.reset();
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+ ASSERT_FALSE(sub.collector.msg.get());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
+ ASSERT_EQ(1, sub.collector.msg->known_nodes.size());
+ ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
+ sub.collector.msg.reset();
+
+ /*
+ * Discovering other nodes
+ */
+ uavcan::protocol::dynamic_node_id::server::Discovery msg;
+ msg.configured_cluster_size = 3;
+ msg.known_nodes.push_back(2U);
+ ASSERT_LE(0, pub.broadcast(msg));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
+ ASSERT_EQ(2, sub.collector.msg->known_nodes.size());
+ ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
+ ASSERT_EQ(2, sub.collector.msg->known_nodes[1]);
+ sub.collector.msg.reset();
+
+ ASSERT_FALSE(mgr.isClusterDiscovered());
+
+ // This will complete the discovery
+ msg.known_nodes.push_back(127U);
+ ASSERT_LE(0, pub.broadcast(msg));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_EQ(3, sub.collector.msg->configured_cluster_size);
+ ASSERT_EQ(3, sub.collector.msg->known_nodes.size());
+ ASSERT_EQ(1, sub.collector.msg->known_nodes[0]);
+ ASSERT_EQ(2, sub.collector.msg->known_nodes[1]);
+ ASSERT_EQ(127, sub.collector.msg->known_nodes[2]);
+ sub.collector.msg.reset();
+
+ // Making sure discovery is now terminated
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
+ ASSERT_FALSE(sub.collector.msg.get());
+
+ /*
+ * Checking Raft states
+ */
+ ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0));
+ ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1));
+ ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2));
+
+ ASSERT_EQ(0, mgr.getServerMatchIndex(2));
+ ASSERT_EQ(0, mgr.getServerMatchIndex(127));
+
+ ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2));
+ ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127));
+
+ mgr.setServerMatchIndex(2, 10);
+ ASSERT_EQ(10, mgr.getServerMatchIndex(2));
+
+ mgr.incrementServerNextIndexBy(2, 5);
+ ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2));
+ mgr.decrementServerNextIndex(2);
+ ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2));
+
+ mgr.resetAllServerIndices();
+
+ ASSERT_EQ(0, mgr.getServerMatchIndex(2));
+ ASSERT_EQ(0, mgr.getServerMatchIndex(127));
+
+ ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2));
+ ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/log.hpp>
+#include "../event_tracer.hpp"
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+
+static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry
+
+
+TEST(dynamic_node_id_server_Log, Initialization)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ EventTracer tracer;
+ // No log data in the storage - initializing empty log
+ {
+ MemoryStorageBackend storage;
+ uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer);
+
+ ASSERT_EQ(0, storage.getNumKeys());
+ ASSERT_LE(0, log.init());
+ ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys());
+ ASSERT_EQ(0, log.getLastIndex());
+ ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
+ ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id);
+ ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(),
+ log.getEntryAtIndex(0)->unique_id);
+ }
+ // Nonempty storage, one item
+ {
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+
+ storage.set("log_last_index", "0");
+ ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found
+ ASSERT_EQ(1, storage.getNumKeys());
+
+ storage.set("log0_term", "0");
+ storage.set("log0_unique_id", "00000000000000000000000000000000");
+ storage.set("log0_node_id", "0");
+ ASSERT_LE(0, log.init()); // OK now
+ ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys());
+ ASSERT_EQ(0, log.getLastIndex());
+ ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
+ }
+ // Nonempty storage, broken data
+ {
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+
+ storage.set("log_last_index", "foobar");
+ ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value
+
+ storage.set("log_last_index", "128");
+ ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value
+
+ storage.set("log_last_index", "0");
+ ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items
+ ASSERT_EQ(1, storage.getNumKeys());
+
+ storage.set("log0_term", "0");
+ storage.set("log0_unique_id", "00000000000000000000000000000000");
+ storage.set("log0_node_id", "128"); // Bad value (127 max)
+ ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed
+ ASSERT_EQ(0, log.getLastIndex());
+ ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
+ ASSERT_EQ(4, storage.getNumKeys());
+ }
+ // Nonempty storage, many items
+ {
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+
+ storage.set("log_last_index", "1"); // 2 items - 0, 1
+ storage.set("log0_term", "0");
+ storage.set("log0_unique_id", "00000000000000000000000000000000");
+ storage.set("log0_node_id", "0");
+ storage.set("log1_term", "1");
+ storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef");
+ storage.set("log1_node_id", "127");
+
+ ASSERT_LE(0, log.init()); // OK now
+ ASSERT_EQ(7, storage.getNumKeys());
+ ASSERT_EQ(1, log.getLastIndex());
+
+ ASSERT_EQ(0, log.getEntryAtIndex(0)->term);
+ ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id);
+ ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(),
+ log.getEntryAtIndex(0)->unique_id);
+
+ ASSERT_EQ(1, log.getEntryAtIndex(1)->term);
+ ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id);
+ uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid;
+ uid[0] = 0x01;
+ uid[1] = 0x23;
+ uid[2] = 0x45;
+ uid[3] = 0x67;
+ uid[4] = 0x89;
+ uid[5] = 0xab;
+ uid[6] = 0xcd;
+ uid[7] = 0xef;
+ uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8);
+ ASSERT_EQ(uid, log.getEntryAtIndex(1)->unique_id);
+ }
+}
+
+
+TEST(dynamic_node_id_server_Log, Append)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+
+ ASSERT_EQ(0, storage.getNumKeys());
+ ASSERT_LE(0, log.init());
+ storage.print();
+ ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys());
+
+ /*
+ * Entry at the index 0 always exists, and it's always zero-initialized.
+ */
+ ASSERT_EQ("0", storage.get("log_last_index"));
+ ASSERT_EQ("0", storage.get("log0_term"));
+ ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id"));
+ ASSERT_EQ("0", storage.get("log0_node_id"));
+
+ /*
+ * Adding one entry to the log, making sure it appears in the storage
+ */
+ uavcan::protocol::dynamic_node_id::server::Entry entry;
+ entry.term = 1;
+ entry.node_id = 1;
+ entry.unique_id[0] = 1;
+ ASSERT_LE(0, log.append(entry));
+
+ ASSERT_EQ("1", storage.get("log_last_index"));
+ ASSERT_EQ("1", storage.get("log1_term"));
+ ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id"));
+ ASSERT_EQ("1", storage.get("log1_node_id"));
+
+ ASSERT_EQ(1, log.getLastIndex());
+ ASSERT_TRUE(entry == *log.getEntryAtIndex(1));
+
+ /*
+ * Adding another entry while storage is failing
+ */
+ storage.failOnSetCalls(true);
+
+ ASSERT_EQ(7, storage.getNumKeys());
+
+ entry.term = 2;
+ entry.node_id = 2;
+ entry.unique_id[0] = 2;
+ ASSERT_GT(0, log.append(entry));
+
+ ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed
+
+ ASSERT_EQ(1, log.getLastIndex());
+
+ /*
+ * Making sure append() fails when the log is full
+ */
+ storage.failOnSetCalls(false);
+
+ while (log.getLastIndex() < (log.Capacity - 1))
+ {
+ ASSERT_LE(0, log.append(entry));
+ ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex()));
+
+ entry.term += 1;
+ entry.node_id = uint8_t(entry.node_id + 1U);
+ entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U);
+ }
+
+ ASSERT_GT(0, log.append(entry)); // Failing because full
+
+ storage.print();
+}
+
+
+TEST(dynamic_node_id_server_Log, Remove)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+ Log log(storage, tracer);
+
+ /*
+ * Filling the log fully
+ */
+ uavcan::protocol::dynamic_node_id::server::Entry entry;
+ entry.term = 1;
+ entry.node_id = 1;
+ entry.unique_id[0] = 1;
+
+ while (log.getLastIndex() < (log.Capacity - 1))
+ {
+ ASSERT_LE(0, log.append(entry));
+ ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex()));
+
+ entry.term += 1;
+ entry.node_id = uint8_t(entry.node_id + 1U);
+ entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U);
+ }
+
+ /*
+ * Removal will fail as the storage is failing to update
+ */
+ storage.failOnSetCalls(true);
+
+ ASSERT_EQ(log.Capacity - 1, log.getLastIndex());
+ ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing
+ ASSERT_EQ(log.Capacity - 1, log.getLastIndex());
+
+ /*
+ * Now removal must work
+ */
+ storage.failOnSetCalls(false);
+
+ ASSERT_EQ(log.Capacity - 1, log.getLastIndex());
+
+ ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60));
+ ASSERT_EQ(59, log.getLastIndex());
+ ASSERT_EQ("59", storage.get("log_last_index"));
+
+ ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30));
+ ASSERT_EQ(30, log.getLastIndex());
+ ASSERT_EQ("30", storage.get("log_last_index"));
+
+ ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1));
+ ASSERT_EQ(0, log.getLastIndex());
+ ASSERT_EQ("0", storage.get("log_last_index"));
+
+ storage.print();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,209 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp>
+#include "../event_tracer.hpp"
+#include "../memory_storage_backend.hpp"
+
+
+TEST(dynamic_node_id_server_PersistentState, Initialization)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ EventTracer tracer;
+ /*
+ * First initialization
+ */
+ {
+ MemoryStorageBackend storage;
+ PersistentState pers(storage, tracer);
+
+ ASSERT_EQ(0, storage.getNumKeys());
+ ASSERT_LE(0, pers.init());
+
+ ASSERT_LE(3, storage.getNumKeys());
+ ASSERT_EQ("0", storage.get("log_last_index"));
+ ASSERT_EQ("0", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+ }
+ /*
+ * Partial recovery - only empty log is recovered
+ */
+ {
+ MemoryStorageBackend storage;
+
+ {
+ // This log is used to initialize the storage
+ Log log(storage, tracer);
+ ASSERT_LE(0, log.init());
+ }
+ ASSERT_LE(1, storage.getNumKeys());
+
+ PersistentState pers(storage, tracer);
+
+ ASSERT_LE(0, pers.init());
+
+ ASSERT_LE(3, storage.getNumKeys());
+ ASSERT_EQ("0", storage.get("log_last_index"));
+ ASSERT_EQ("0", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+ }
+ /*
+ * Partial recovery - log and current term are recovered
+ */
+ {
+ MemoryStorageBackend storage;
+
+ {
+ // This log is used to initialize the storage
+ Log log(storage, tracer);
+ ASSERT_LE(0, log.init());
+ }
+ ASSERT_LE(1, storage.getNumKeys());
+
+ storage.set("current_term", "1");
+
+ PersistentState pers(storage, tracer);
+
+ ASSERT_GT(0, pers.init()); // Fails because current term is not zero
+
+ storage.set("current_term", "0");
+
+ ASSERT_LE(0, pers.init()); // OK now
+
+ ASSERT_LE(3, storage.getNumKeys());
+ ASSERT_EQ("0", storage.get("log_last_index"));
+ ASSERT_EQ("0", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+ }
+ /*
+ * Full recovery
+ */
+ {
+ MemoryStorageBackend storage;
+
+ {
+ // This log is used to initialize the storage
+ Log log(storage, tracer);
+ ASSERT_LE(0, log.init());
+
+ uavcan::protocol::dynamic_node_id::server::Entry entry;
+ entry.term = 1;
+ entry.node_id = 1;
+ entry.unique_id[0] = 1;
+ ASSERT_LE(0, log.append(entry));
+ }
+ ASSERT_LE(4, storage.getNumKeys());
+
+ PersistentState pers(storage, tracer);
+
+ ASSERT_GT(0, pers.init()); // Fails because log is not empty
+
+ storage.set("current_term", "0");
+ storage.set("voted_for", "0");
+ ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm
+
+ storage.set("current_term", "1"); // OK
+ storage.set("voted_for", "128"); // Invalid value
+ ASSERT_GT(0, pers.init()); // Fails because of bad votedFor
+
+ storage.set("voted_for", "0"); // OK now
+ ASSERT_LE(0, pers.init());
+
+ ASSERT_LE(3, storage.getNumKeys());
+ ASSERT_EQ("1", storage.get("log_last_index"));
+ ASSERT_EQ("1", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+ }
+}
+
+
+TEST(dynamic_node_id_server_PersistentState, Basic)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+ PersistentState pers(storage, tracer);
+
+ /*
+ * Initializing
+ */
+ ASSERT_LE(0, pers.init());
+
+ ASSERT_EQ("0", storage.get("log_last_index"));
+ ASSERT_EQ("0", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+
+ /*
+ * Inserting some log entries
+ */
+ uavcan::protocol::dynamic_node_id::server::Entry entry;
+ entry.term = 1;
+ entry.node_id = 1;
+ entry.unique_id[0] = 1;
+ ASSERT_LE(0, pers.getLog().append(entry));
+
+ ASSERT_EQ("1", storage.get("log_last_index"));
+ ASSERT_EQ("0", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+
+ /*
+ * Changing current term
+ */
+ ASSERT_EQ(0, pers.getCurrentTerm());
+ ASSERT_LE(0, pers.setCurrentTerm(2));
+ ASSERT_EQ(2, pers.getCurrentTerm());
+
+ ASSERT_EQ("1", storage.get("log_last_index"));
+ ASSERT_EQ("2", storage.get("current_term"));
+ ASSERT_EQ("0", storage.get("voted_for"));
+
+ /*
+ * Changing votedFor
+ */
+ ASSERT_FALSE(pers.isVotedForSet());
+ ASSERT_EQ(0, pers.getVotedFor().get());
+ ASSERT_LE(0, pers.setVotedFor(0));
+ ASSERT_EQ(0, pers.getVotedFor().get());
+ ASSERT_LE(0, pers.setVotedFor(45));
+ ASSERT_EQ(45, pers.getVotedFor().get());
+ ASSERT_TRUE(pers.isVotedForSet());
+
+ ASSERT_EQ("1", storage.get("log_last_index"));
+ ASSERT_EQ("2", storage.get("current_term"));
+ ASSERT_EQ("45", storage.get("voted_for"));
+
+ ASSERT_LE(0, pers.resetVotedFor());
+ ASSERT_EQ(0, pers.getVotedFor().get());
+ ASSERT_FALSE(pers.isVotedForSet());
+ ASSERT_EQ("0", storage.get("voted_for"));
+
+ ASSERT_LE(0, pers.setVotedFor(45));
+ ASSERT_TRUE(pers.isVotedForSet());
+ ASSERT_EQ("45", storage.get("voted_for"));
+
+ /*
+ * Handling errors
+ */
+ storage.failOnSetCalls(true);
+
+ ASSERT_EQ(2, pers.getCurrentTerm());
+ ASSERT_GT(0, pers.setCurrentTerm(7893));
+ ASSERT_EQ(2, pers.getCurrentTerm());
+
+ ASSERT_EQ(45, pers.getVotedFor().get());
+ ASSERT_GT(0, pers.setVotedFor(78));
+ ASSERT_EQ(45, pers.getVotedFor().get());
+
+ ASSERT_EQ("1", storage.get("log_last_index"));
+ ASSERT_EQ("2", storage.get("current_term"));
+ ASSERT_EQ("45", storage.get("voted_for"));
+
+ /*
+ * Final checks
+ */
+ ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,195 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <uavcan/protocol/dynamic_node_id_server/distributed.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "../event_tracer.hpp"
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+using uavcan::dynamic_node_id_server::UniqueID;
+
+
+class CommitHandler : public uavcan::dynamic_node_id_server::distributed::IRaftLeaderMonitor
+{
+ const std::string id_;
+
+ virtual void handleLogCommitOnLeader(const uavcan::protocol::dynamic_node_id::server::Entry& entry)
+ {
+ std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl;
+ }
+
+ virtual void handleLocalLeadershipChange(bool local_node_is_leader)
+ {
+ std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl;
+ }
+
+public:
+ CommitHandler(const std::string& id) : id_(id) { }
+};
+
+
+TEST(dynamic_node_id_server_RaftCore, Basic)
+{
+ using namespace uavcan::dynamic_node_id_server::distributed;
+ using namespace uavcan::protocol::dynamic_node_id::server;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<Discovery> _reg1;
+ uavcan::DefaultDataTypeRegistrator<AppendEntries> _reg2;
+ uavcan::DefaultDataTypeRegistrator<RequestVote> _reg3;
+
+ EventTracer tracer_a("a");
+ EventTracer tracer_b("b");
+ MemoryStorageBackend storage_a;
+ MemoryStorageBackend storage_b;
+ CommitHandler commit_handler_a("a");
+ CommitHandler commit_handler_b("b");
+
+ InterlinkedTestNodesWithSysClock nodes;
+
+ std::auto_ptr<RaftCore> raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a));
+ std::auto_ptr<RaftCore> raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b));
+
+ /*
+ * Initialization
+ */
+ ASSERT_LE(0, raft_a->init(2, uavcan::TransferPriority::OneHigherThanLowest));
+ ASSERT_LE(0, raft_b->init(2, uavcan::TransferPriority::OneHigherThanLowest));
+
+ /*
+ * Running and trying not to fall
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000));
+
+ // Either must become a leader
+ ASSERT_TRUE(raft_a->isLeader() || raft_b->isLeader());
+
+ ASSERT_EQ(0, raft_a->getCommitIndex());
+ ASSERT_EQ(0, raft_b->getCommitIndex());
+
+ /*
+ * Adding some stuff
+ */
+ Entry::FieldTypes::unique_id unique_id;
+ uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA));
+
+ (raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000));
+
+ ASSERT_EQ(1, raft_a->getCommitIndex());
+ ASSERT_EQ(1, raft_b->getCommitIndex());
+
+ /*
+ * Terminating the leader
+ */
+ raft_a.reset();
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000));
+
+ /*
+ * Reinitializing the leader - current Follower will become the new Leader
+ */
+ storage_a.reset();
+
+ raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a));
+ ASSERT_LE(0, raft_a->init(2, uavcan::TransferPriority::OneHigherThanLowest));
+ ASSERT_EQ(0, raft_a->getCommitIndex());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000));
+
+ ASSERT_FALSE(raft_a->isLeader());
+ ASSERT_TRUE(raft_b->isLeader());
+
+ ASSERT_EQ(1, raft_a->getCommitIndex());
+ ASSERT_EQ(1, raft_b->getCommitIndex());
+}
+
+
+TEST(dynamic_node_id_server_Server, Basic)
+{
+ using namespace uavcan::dynamic_node_id_server;
+ using namespace uavcan::protocol::dynamic_node_id;
+ using namespace uavcan::protocol::dynamic_node_id::server;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<Discovery> _reg1;
+ uavcan::DefaultDataTypeRegistrator<AppendEntries> _reg2;
+ uavcan::DefaultDataTypeRegistrator<RequestVote> _reg3;
+ uavcan::DefaultDataTypeRegistrator<Allocation> _reg4;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg5;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg6;
+
+ EventTracer tracer;
+ MemoryStorageBackend storage;
+
+ // Node A is Allocator, Node B is Allocatee
+ InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+ UniqueID own_unique_id;
+ own_unique_id[0] = 0xAA;
+ own_unique_id[3] = 0xCC;
+ own_unique_id[7] = 0xEE;
+ own_unique_id[9] = 0xBB;
+
+ /*
+ * Server
+ */
+ DistributedServer server(nodes.a, storage, tracer);
+
+ ASSERT_LE(0, server.init(own_unique_id, 1));
+
+ ASSERT_EQ(0, server.getNumAllocations());
+
+ /*
+ * Client
+ */
+ uavcan::DynamicNodeIDClient client(nodes.b);
+ uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id;
+ for (uavcan::uint8_t i = 0; i < unique_id.size(); i++)
+ {
+ unique_id[i] = i;
+ }
+ const uavcan::NodeID PreferredNodeID = 42;
+ ASSERT_LE(0, client.start(unique_id, PreferredNodeID));
+
+ /*
+ * Fire
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000));
+
+ ASSERT_TRUE(client.isAllocationComplete());
+ ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
+
+ ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client
+}
+
+
+TEST(dynamic_node_id_server, ObjectSizes)
+{
+ using namespace uavcan;
+ using namespace uavcan::protocol::dynamic_node_id::server;
+ using namespace uavcan::dynamic_node_id_server;
+
+ std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl;
+ std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl;
+ std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl;
+ std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl;
+ std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl;
+ std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl;
+
+ std::cout << "ServiceServer<AppendEntries>: " << sizeof(ServiceServer<AppendEntries>) << std::endl;
+ std::cout << "ServiceClient<AppendEntries>: " << sizeof(ServiceClient<AppendEntries>) << std::endl;
+ std::cout << "ServiceServer<RequestVote>: " << sizeof(ServiceServer<RequestVote>) << std::endl;
+ std::cout << "ServiceClient<RequestVote>: " << sizeof(ServiceClient<RequestVote>) << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+#include "event_tracer.hpp"
+
+
+TEST(dynamic_node_id_server_EventTracer, EventCodeToString)
+{
+ using namespace uavcan::dynamic_node_id_server;
+
+ // Simply checking some error codes
+ ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError));
+ ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure));
+ ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived));
+ ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected));
+ ASSERT_STREQ("AllocationUnexpectedStage", IEventTracer::getEventName(TraceAllocationUnexpectedStage));
+}
+
+
+TEST(dynamic_node_id_server_EventTracer, EnvironmentSelfTest)
+{
+ using namespace uavcan::dynamic_node_id_server;
+
+ EventTracer tracer;
+
+ ASSERT_EQ(0, tracer.getNumEvents());
+
+ tracer.onEvent(TraceRaftAppendEntriesCallFailure, 123);
+ ASSERT_EQ(1, tracer.getNumEvents());
+ tracer.onEvent(TraceRaftAppendEntriesCallFailure, -456);
+ ASSERT_EQ(2, tracer.getNumEvents());
+ tracer.onEvent(TraceError, -0xFFFFFFFFFFFFFFFLL);
+ ASSERT_EQ(3, tracer.getNumEvents());
+
+ ASSERT_EQ(0, tracer.countEvents(TraceAllocationActivity));
+ ASSERT_EQ(2, tracer.countEvents(TraceRaftAppendEntriesCallFailure));
+ ASSERT_EQ(1, tracer.countEvents(TraceError));
+
+ ASSERT_EQ(-456, tracer.getLastEventArgumentOrFail(TraceRaftAppendEntriesCallFailure));
+
+ ASSERT_EQ(-0xFFFFFFFFFFFFFFFLL, tracer.getLastEventArgumentOrFail(TraceError));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <iostream>
+#include <string>
+#include <list>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+#include <uavcan/time.hpp>
+#include "../../clock.hpp"
+
+
+class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer
+{
+ struct EventLogEntry
+ {
+ const uavcan::dynamic_node_id_server::TraceCode code;
+ const uavcan::int64_t argument;
+
+ EventLogEntry(uavcan::dynamic_node_id_server::TraceCode arg_code, uavcan::int64_t arg_argument)
+ : code(arg_code)
+ , argument(arg_argument)
+ { }
+ };
+
+ const std::string id_;
+ const uavcan::MonotonicTime startup_ts_;
+ std::list<EventLogEntry> event_log_;
+
+public:
+ EventTracer() :
+ startup_ts_(SystemClockDriver().getMonotonic())
+ { }
+
+ EventTracer(const std::string& id) :
+ id_(id),
+ startup_ts_(SystemClockDriver().getMonotonic())
+ { }
+
+ virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument)
+ {
+ const uavcan::MonotonicDuration ts = SystemClockDriver().getMonotonic() - startup_ts_;
+ std::cout << "EVENT [" << id_ << "]\t" << ts.toString() << "\t"
+ << int(code) << "\t" << getEventName(code) << "\t" << argument << std::endl;
+ event_log_.push_back(EventLogEntry(code, argument));
+ }
+
+ unsigned countEvents(const uavcan::dynamic_node_id_server::TraceCode code) const
+ {
+ unsigned count = 0;
+ for (std::list<EventLogEntry>::const_iterator it = event_log_.begin(); it != event_log_.end(); ++it)
+ {
+ count += (it->code == code) ? 1U : 0U;
+ }
+ return count;
+ }
+
+ uavcan::int64_t getLastEventArgumentOrFail(const uavcan::dynamic_node_id_server::TraceCode code) const
+ {
+ for (std::list<EventLogEntry>::const_reverse_iterator it = event_log_.rbegin(); it != event_log_.rend(); ++it)
+ {
+ if (it->code == code)
+ {
+ return it->argument;
+ }
+ }
+
+ std::cout << "No such event in the event log, code " << int(code) << ", log length " << event_log_.size()
+ << std::endl;
+
+ throw std::runtime_error("EventTracer::getLastEventArgumentOrFail()");
+ }
+
+ unsigned getNumEvents() const { return static_cast<unsigned>(event_log_.size()); }
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <iostream>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+
+class GetNodeInfoMockServer
+{
+ typedef uavcan::MethodBinder<GetNodeInfoMockServer*,
+ void (GetNodeInfoMockServer::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::GetNodeInfo::Request>&,
+ uavcan::protocol::GetNodeInfo::Response&) const>
+ GetNodeInfoCallback;
+
+ uavcan::ServiceServer<uavcan::protocol::GetNodeInfo, GetNodeInfoCallback> server_;
+
+ void handleRequest(const uavcan::ReceivedDataStructure<uavcan::protocol::GetNodeInfo::Request>& req,
+ uavcan::protocol::GetNodeInfo::Response& res) const
+ {
+ res = response;
+ std::cout << "GET NODE INFO:\nREQUEST\n" << req << "RESPONSE\n" << res << std::endl;
+ }
+
+public:
+ uavcan::protocol::GetNodeInfo::Response response;
+
+ GetNodeInfoMockServer(uavcan::INode& node) : server_(node) { }
+
+ int start() { return server_.start(GetNodeInfoCallback(this, &GetNodeInfoMockServer::handleRequest)); }
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <map>
+#include <uavcan/protocol/dynamic_node_id_server/storage_backend.hpp>
+
+class MemoryStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend
+{
+ typedef std::map<String, String> Container;
+ Container container_;
+
+ bool fail_;
+
+public:
+ MemoryStorageBackend()
+ : fail_(false)
+ { }
+
+ virtual String get(const String& key) const
+ {
+ const Container::const_iterator it = container_.find(key);
+ if (it == container_.end())
+ {
+ return String();
+ }
+ return it->second;
+ }
+
+ virtual void set(const String& key, const String& value)
+ {
+ if (!fail_)
+ {
+ container_[key] = value;
+ }
+ }
+
+ void failOnSetCalls(bool really) { fail_ = really; }
+
+ void reset() { container_.clear(); }
+
+ unsigned getNumKeys() const { return unsigned(container_.size()); }
+
+ void print() const
+ {
+ for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it)
+ {
+ std::cout << it->first.c_str() << "\t" << it->second.c_str() << std::endl;
+ }
+ }
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,286 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <gtest/gtest.h>
+#include <vector>
+#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
+#include <uavcan/node/publisher.hpp>
+#include "event_tracer.hpp"
+#include "get_node_info_mock_server.hpp"
+#include "../helpers.hpp"
+
+using namespace uavcan::dynamic_node_id_server;
+
+
+class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler
+{
+public:
+ struct NodeInfo
+ {
+ UniqueID unique_id;
+ uavcan::NodeID node_id;
+ bool committed;
+
+ NodeInfo() : committed(false) { }
+ };
+
+ bool can_discover;
+ std::vector<NodeInfo> nodes;
+
+ NodeDiscoveryHandler() : can_discover(false) { }
+
+ virtual bool canDiscoverNewNodes() const
+ {
+ return can_discover;
+ }
+
+ virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const
+ {
+ const NodeInfo* const ni = const_cast<NodeDiscoveryHandler*>(this)->findNode(node_id);
+ if (ni == UAVCAN_NULLPTR)
+ {
+ return NodeAwarenessUnknown;
+ }
+ return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
+ }
+
+ virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id)
+ {
+ NodeInfo info;
+ if (unique_id_or_null != UAVCAN_NULLPTR)
+ {
+ info.unique_id = *unique_id_or_null;
+ }
+ info.node_id = node_id;
+ nodes.push_back(info);
+ }
+
+ NodeInfo* findNode(const UniqueID& unique_id)
+ {
+ for (unsigned i = 0; i < nodes.size(); i++)
+ {
+ if (nodes.at(i).unique_id == unique_id)
+ {
+ return &nodes.at(i);
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+
+ NodeInfo* findNode(const uavcan::NodeID& node_id)
+ {
+ for (unsigned i = 0; i < nodes.size(); i++)
+ {
+ if (nodes.at(i).node_id == node_id)
+ {
+ return &nodes.at(i);
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+};
+
+
+TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
+{
+ using namespace uavcan::protocol::dynamic_node_id::server;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+
+ EventTracer tracer;
+ InterlinkedTestNodesWithSysClock nodes;
+ NodeDiscoveryHandler handler;
+
+ NodeDiscoverer disc(nodes.a, tracer, handler);
+
+ /*
+ * Initialization
+ */
+ ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest));
+
+ ASSERT_FALSE(disc.hasUnknownNodes());
+
+ /*
+ * Publishing NodeStatus, discovery is disabled
+ */
+ std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl;
+ handler.can_discover = false;
+
+ uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
+ ASSERT_LE(0, node_status_pub.init());
+
+ uavcan::protocol::NodeStatus node_status;
+ node_status.uptime_sec = 0;
+ ASSERT_LE(0, node_status_pub.broadcast(node_status));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); // The timer runs as long as there are unknown nodes
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled!
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_TRUE(disc.hasUnknownNodes());
+
+ /*
+ * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving
+ */
+ std::cout << "!!! Enabling discovery" << std::endl;
+ handler.can_discover = true;
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1150));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
+ ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_TRUE(disc.hasUnknownNodes());
+
+ /*
+ * Publishing NodeStatus
+ */
+ std::cout << "!!! Publishing NodeStatus" << std::endl;
+
+ node_status.uptime_sec += 5U;
+ ASSERT_LE(0, node_status_pub.broadcast(node_status));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1250));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
+ ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+ ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_TRUE(disc.hasUnknownNodes());
+
+ /*
+ * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized
+ */
+ std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl;
+
+ GetNodeInfoMockServer get_node_info_server(nodes.b);
+ get_node_info_server.response.hardware_version.unique_id[0] = 123; // Arbitrary data
+ get_node_info_server.response.hardware_version.unique_id[6] = 213;
+ get_node_info_server.response.hardware_version.unique_id[14] = 52;
+ ASSERT_LE(0, get_node_info_server.start());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
+ ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+ ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
+ ASSERT_FALSE(disc.hasUnknownNodes());
+
+ /*
+ * Checking the results
+ */
+ ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id));
+ ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get());
+}
+
+
+TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
+{
+ using namespace uavcan::protocol::dynamic_node_id::server;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+
+ EventTracer tracer;
+ InterlinkedTestNodesWithSysClock nodes;
+ NodeDiscoveryHandler handler;
+
+ NodeDiscoverer disc(nodes.a, tracer, handler);
+
+ /*
+ * Initialization
+ */
+ ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest));
+
+ ASSERT_FALSE(disc.hasUnknownNodes());
+
+ /*
+ * Publishing NodeStatus once to trigger querying
+ * Querying for 2 seconds, no responses will be sent (there's no server)
+ */
+ handler.can_discover = true;
+
+ uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
+ ASSERT_LE(0, node_status_pub.init());
+
+ uavcan::protocol::NodeStatus node_status;
+ node_status.uptime_sec = 10; // Nonzero
+ ASSERT_LE(0, node_status_pub.broadcast(node_status));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
+ ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+ ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
+ ASSERT_TRUE(disc.hasUnknownNodes());
+
+ /*
+ * Emulating node restart
+ */
+ node_status.uptime_sec = 9; // Less than previous
+ ASSERT_LE(0, node_status_pub.broadcast(node_status));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
+ ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+ ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
+ ASSERT_TRUE(disc.hasUnknownNodes());
+
+ /*
+ * Waiting for timeout
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
+
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
+ ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+ ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
+ ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
+ ASSERT_FALSE(disc.hasUnknownNodes());
+
+ /*
+ * Checking the results
+ */
+ ASSERT_TRUE(handler.findNode(UniqueID()));
+ ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get());
+}
+
+
+TEST(dynamic_node_id_server_NodeDiscoverer, Sizes)
+{
+ using namespace uavcan;
+
+ std::cout << "BitSet<NodeID::Max + 1>: " << sizeof(BitSet<NodeID::Max + 1>) << std::endl;
+ std::cout << "ServiceClient<protocol::GetNodeInfo>: " << sizeof(ServiceClient<protocol::GetNodeInfo>) << std::endl;
+ std::cout << "protocol::GetNodeInfo::Response: " << sizeof(protocol::GetNodeInfo::Response) << std::endl;
+ std::cout << "Subscriber<protocol::NodeStatus>: " << sizeof(Subscriber<protocol::NodeStatus>) << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include "memory_storage_backend.hpp"
+
+TEST(dynamic_node_id_server_StorageMarshaller, Basic)
+{
+ MemoryStorageBackend st;
+
+ uavcan::dynamic_node_id_server::StorageMarshaller marshaler(st);
+
+ uavcan::dynamic_node_id_server::IStorageBackend::String key;
+
+ /*
+ * uint32
+ */
+ uint32_t u32 = 0;
+
+ key = "foo";
+ u32 = 0;
+ ASSERT_LE(0, marshaler.setAndGetBack(key, u32));
+ ASSERT_EQ(0, u32);
+
+ key = "bar";
+ u32 = 0xFFFFFFFF;
+ ASSERT_LE(0, marshaler.setAndGetBack(key, u32));
+ ASSERT_EQ(0xFFFFFFFF, u32);
+ ASSERT_LE(0, marshaler.get(key, u32));
+ ASSERT_EQ(0xFFFFFFFF, u32);
+
+ key = "foo";
+ ASSERT_LE(0, marshaler.get(key, u32));
+ ASSERT_EQ(0, u32);
+
+ key = "the_cake_is_a_lie";
+ ASSERT_GT(0, marshaler.get(key, u32));
+ ASSERT_EQ(0, u32);
+
+ /*
+ * uint8[16]
+ */
+ uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id array;
+
+ key = "a";
+ // Set zero
+ ASSERT_LE(0, marshaler.setAndGetBack(key, array));
+ for (uint8_t i = 0; i < 16; i++)
+ {
+ ASSERT_EQ(0, array[i]);
+ }
+
+ // Make sure this will not be interpreted as uint32
+ ASSERT_GT(0, marshaler.get(key, u32));
+ ASSERT_EQ(0, u32);
+
+ // Set pattern
+ for (uint8_t i = 0; i < 16; i++)
+ {
+ array[i] = uint8_t(i + 1);
+ }
+ ASSERT_LE(0, marshaler.setAndGetBack(key, array));
+ for (uint8_t i = 0; i < 16; i++)
+ {
+ ASSERT_EQ(i + 1, array[i]);
+ }
+
+ // Set another pattern
+ for (uint8_t i = 0; i < 16; i++)
+ {
+ array[i] = uint8_t(i | (i << 4));
+ }
+ ASSERT_LE(0, marshaler.setAndGetBack(key, array));
+ for (uint8_t i = 0; i < 16; i++)
+ {
+ ASSERT_EQ(uint8_t(i | (i << 4)), array[i]);
+ }
+
+ // Make sure uint32 cannot be interpreted as an array
+ key = "foo";
+ ASSERT_GT(0, marshaler.get(key, array));
+
+ // Nonexistent key
+ key = "the_cake_is_a_lie";
+ ASSERT_GT(0, marshaler.get(key, array));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/file_server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,136 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/file_server.hpp>
+#include "helpers.hpp"
+
+
+class TestFileServerBackend : public uavcan::IFileServerBackend
+{
+public:
+ static const std::string file_name;
+ static const std::string file_data;
+
+ virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type)
+ {
+ if (path == file_name)
+ {
+ out_size = uint16_t(file_data.length());
+ out_type.flags |= EntryType::FLAG_FILE;
+ out_type.flags |= EntryType::FLAG_READABLE;
+ return 0;
+ }
+ else
+ {
+ return Error::NOT_FOUND;
+ }
+ }
+
+ virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size)
+ {
+ if (path == file_name)
+ {
+ if (offset < file_data.length())
+ {
+ inout_size = uint16_t(file_data.length() - offset);
+ std::memcpy(out_buffer, file_data.c_str() + offset, inout_size);
+ }
+ else
+ {
+ inout_size = 0;
+ }
+ return 0;
+ }
+ else
+ {
+ return Error::NOT_FOUND;
+ }
+ }
+};
+
+const std::string TestFileServerBackend::file_name = "test";
+const std::string TestFileServerBackend::file_data = "123456789";
+
+TEST(BasicFileServer, Basic)
+{
+ using namespace uavcan::protocol::file;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<GetInfo> _reg1;
+ uavcan::DefaultDataTypeRegistrator<Read> _reg2;
+
+ InterlinkedTestNodesWithSysClock nodes;
+
+ TestFileServerBackend backend;
+
+ uavcan::BasicFileServer serv(nodes.a, backend);
+ std::cout << "sizeof(uavcan::BasicFileServer): " << sizeof(uavcan::BasicFileServer) << std::endl;
+
+ ASSERT_LE(0, serv.start());
+
+ /*
+ * GetInfo, existing file
+ */
+ {
+ ServiceClientWithCollector<GetInfo> get_info(nodes.b);
+
+ GetInfo::Request get_info_req;
+ get_info_req.path.path = "test";
+
+ ASSERT_LE(0, get_info.call(1, get_info_req));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(get_info.collector.result.get());
+ ASSERT_TRUE(get_info.collector.result->isSuccessful());
+
+ ASSERT_EQ(0, get_info.collector.result->getResponse().error.value);
+ ASSERT_EQ(9, get_info.collector.result->getResponse().size);
+ ASSERT_EQ(EntryType::FLAG_FILE | EntryType::FLAG_READABLE,
+ get_info.collector.result->getResponse().entry_type.flags);
+ }
+
+ /*
+ * Read, existing file
+ */
+ {
+ ServiceClientWithCollector<Read> read(nodes.b);
+
+ Read::Request read_req;
+ read_req.path.path = "test";
+
+ ASSERT_LE(0, read.call(1, read_req));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(read.collector.result.get());
+ ASSERT_TRUE(read.collector.result->isSuccessful());
+
+ ASSERT_EQ("123456789", read.collector.result->getResponse().data);
+ ASSERT_EQ(0, read.collector.result->getResponse().error.value);
+ }
+}
+
+
+TEST(FileServer, Basic)
+{
+ using namespace uavcan::protocol::file;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<GetInfo> _reg1;
+ uavcan::DefaultDataTypeRegistrator<Read> _reg2;
+ uavcan::DefaultDataTypeRegistrator<Write> _reg3;
+ uavcan::DefaultDataTypeRegistrator<Delete> _reg4;
+ uavcan::DefaultDataTypeRegistrator<GetDirectoryEntryInfo> _reg5;
+
+ InterlinkedTestNodesWithSysClock nodes;
+
+ TestFileServerBackend backend;
+
+ uavcan::FileServer serv(nodes.a, backend);
+ std::cout << "sizeof(uavcan::FileServer): " << sizeof(uavcan::FileServer) << std::endl;
+
+ ASSERT_LE(0, serv.start());
+
+ // TODO TEST
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/firmware_update_trigger.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,336 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+using namespace uavcan::protocol::file;
+
+struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
+{
+ unsigned should_request_cnt;
+ unsigned should_retry_cnt;
+ unsigned confirmation_cnt;
+
+ std::string firmware_path;
+
+ int retry_quota;
+ std::string expected_node_name_to_update;
+
+ BeginFirmwareUpdate::Response last_error_response;
+
+ FirmwareVersionChecker()
+ : should_request_cnt(0)
+ , should_retry_cnt(0)
+ , confirmation_cnt(0)
+ , retry_quota(0)
+ { }
+
+ virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id,
+ const uavcan::protocol::GetNodeInfo::Response& node_info,
+ FirmwareFilePath& out_firmware_file_path)
+ {
+ should_request_cnt++;
+ std::cout << "REQUEST? " << int(node_id.get()) << "\n" << node_info << std::endl;
+ out_firmware_file_path = firmware_path.c_str();
+ return node_info.name == expected_node_name_to_update;
+ }
+
+ virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id,
+ const BeginFirmwareUpdate::Response& error_response,
+ FirmwareFilePath& out_firmware_file_path)
+ {
+ last_error_response = error_response;
+ std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl;
+ should_retry_cnt++;
+
+ EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str());
+
+ if (retry_quota > 0)
+ {
+ retry_quota--;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id,
+ const BeginFirmwareUpdate::Response& response)
+ {
+ confirmation_cnt++;
+ std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl;
+ }
+};
+
+struct BeginFirmwareUpdateServer
+{
+ uint8_t response_error_code;
+
+ BeginFirmwareUpdateServer() : response_error_code(0) { }
+
+ void handleRequest(const uavcan::ReceivedDataStructure<typename BeginFirmwareUpdate::Request>& req,
+ uavcan::ServiceResponseDataStructure<typename BeginFirmwareUpdate::Response>& res) const
+ {
+ std::cout << "REQUEST\n" << req << std::endl;
+ res.error = response_error_code;
+ res.optional_error_message = "foobar";
+ }
+
+ typedef uavcan::MethodBinder<BeginFirmwareUpdateServer*,
+ void (BeginFirmwareUpdateServer::*)(
+ const uavcan::ReceivedDataStructure<typename BeginFirmwareUpdate::Request>&,
+ uavcan::ServiceResponseDataStructure<typename BeginFirmwareUpdate::Response>&) const> Callback;
+
+ Callback makeCallback() { return Callback(this, &BeginFirmwareUpdateServer::handleRequest); }
+};
+
+
+TEST(FirmwareUpdateTrigger, Basic)
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<BeginFirmwareUpdate> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
+
+ InterlinkedTestNodesWithSysClock nodes;
+
+ FirmwareVersionChecker checker;
+
+ uavcan::NodeInfoRetriever node_info_retriever(nodes.a); // On the same node
+
+ uavcan::FirmwareUpdateTrigger trigger(nodes.a, checker);
+ std::cout << "sizeof(uavcan::FirmwareUpdateTrigger): " << sizeof(uavcan::FirmwareUpdateTrigger) << std::endl;
+
+ std::auto_ptr<uavcan::NodeStatusProvider> provider(new uavcan::NodeStatusProvider(nodes.b)); // Other node
+
+ /*
+ * Initializing
+ */
+ ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/"));
+
+ ASSERT_LE(0, node_info_retriever.start());
+ ASSERT_EQ(1, node_info_retriever.getNumListeners());
+
+ uavcan::protocol::HardwareVersion hwver;
+ hwver.unique_id[0] = 123;
+ hwver.unique_id[4] = 213;
+ hwver.unique_id[8] = 45;
+
+ provider->setName("Ivan");
+ provider->setHardwareVersion(hwver);
+
+ ASSERT_LE(0, provider->startAndPublish());
+
+ ASSERT_FALSE(trigger.isTimerRunning());
+ ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+ /*
+ * Updating one node
+ * The server that can confirm the request is not running yet
+ */
+ checker.firmware_path = "firmware_path";
+ checker.expected_node_name_to_update = "Ivan";
+ checker.retry_quota = 1000;
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+
+ ASSERT_TRUE(trigger.isTimerRunning());
+ ASSERT_EQ(1, trigger.getNumPendingNodes());
+
+ ASSERT_EQ(1, checker.should_request_cnt);
+ ASSERT_EQ(0, checker.should_retry_cnt);
+ ASSERT_EQ(0, checker.confirmation_cnt);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+
+ // Still running!
+ ASSERT_TRUE(trigger.isTimerRunning());
+ ASSERT_EQ(1, trigger.getNumPendingNodes());
+
+ /*
+ * Starting the firmware update server that returns an error
+ * The checker will instruct the trigger to repeat
+ */
+ uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv(nodes.b);
+ BeginFirmwareUpdateServer srv_impl;
+
+ ASSERT_LE(0, srv.start(srv_impl.makeCallback()));
+
+ srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN;
+ checker.retry_quota = 1000;
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+
+ ASSERT_EQ(1, checker.should_request_cnt);
+ ASSERT_EQ(1, checker.should_retry_cnt);
+ ASSERT_EQ(0, checker.confirmation_cnt);
+
+ // Still running!
+ ASSERT_TRUE(trigger.isTimerRunning());
+ ASSERT_EQ(1, trigger.getNumPendingNodes());
+
+ /*
+ * Trying again, this time with ERROR_IN_PROGRESS
+ */
+ srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS;
+ checker.retry_quota = 0;
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100));
+
+ ASSERT_EQ(1, checker.should_request_cnt);
+ ASSERT_EQ(1, checker.should_retry_cnt);
+ ASSERT_EQ(1, checker.confirmation_cnt);
+
+ // Stopped!
+ ASSERT_FALSE(trigger.isTimerRunning());
+ ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+ /*
+ * Restarting the node info provider
+ * Now it doesn't need an update
+ */
+ provider.reset(new uavcan::NodeStatusProvider(nodes.b));
+
+ provider->setName("Dmitry");
+ provider->setHardwareVersion(hwver);
+
+ ASSERT_LE(0, provider->startAndPublish());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100));
+
+ ASSERT_EQ(2, checker.should_request_cnt);
+ ASSERT_EQ(1, checker.should_retry_cnt);
+ ASSERT_EQ(1, checker.confirmation_cnt);
+
+ // Stopped!
+ ASSERT_FALSE(trigger.isTimerRunning());
+ ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+ /*
+ * Final checks
+ */
+ ASSERT_EQ(0, nodes.a.internal_failure_count);
+ ASSERT_EQ(0, nodes.b.internal_failure_count);
+}
+
+
+TEST(FirmwareUpdateTrigger, MultiNode)
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<BeginFirmwareUpdate> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
+
+ TestNetwork<5> nodes;
+
+ // The trigger node
+ FirmwareVersionChecker checker;
+ uavcan::NodeInfoRetriever node_info_retriever(nodes[0]);
+ uavcan::FirmwareUpdateTrigger trigger(nodes[0], checker);
+
+ // The client nodes
+ std::auto_ptr<uavcan::NodeStatusProvider> provider_a(new uavcan::NodeStatusProvider(nodes[1]));
+ std::auto_ptr<uavcan::NodeStatusProvider> provider_b(new uavcan::NodeStatusProvider(nodes[2]));
+ std::auto_ptr<uavcan::NodeStatusProvider> provider_c(new uavcan::NodeStatusProvider(nodes[3]));
+ std::auto_ptr<uavcan::NodeStatusProvider> provider_d(new uavcan::NodeStatusProvider(nodes[4]));
+
+ uavcan::protocol::HardwareVersion hwver;
+
+ /*
+ * Initializing
+ */
+ ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/"));
+
+ ASSERT_LE(0, node_info_retriever.start());
+ ASSERT_EQ(1, node_info_retriever.getNumListeners());
+
+ hwver.unique_id[0] = 0xAA;
+ provider_a->setHardwareVersion(hwver);
+ provider_a->setName("Victor");
+ ASSERT_LE(0, provider_a->startAndPublish());
+
+ hwver.unique_id[0] = 0xBB;
+ provider_b->setHardwareVersion(hwver);
+ provider_b->setName("Victor");
+ ASSERT_LE(0, provider_b->startAndPublish());
+
+ hwver.unique_id[0] = 0xCC;
+ provider_c->setHardwareVersion(hwver);
+ provider_c->setName("Alexey");
+ ASSERT_LE(0, provider_c->startAndPublish());
+
+ hwver.unique_id[0] = 0xDD;
+ provider_d->setHardwareVersion(hwver);
+ provider_d->setName("Victor");
+ ASSERT_LE(0, provider_d->startAndPublish());
+
+ checker.expected_node_name_to_update = "Victor"; // Victors will get updated, others will not
+ checker.firmware_path = "abc";
+
+ /*
+ * Running - 3 will timout, 1 will be ignored
+ */
+ ASSERT_FALSE(trigger.isTimerRunning());
+ ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+ nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100));
+
+ ASSERT_TRUE(trigger.isTimerRunning());
+ ASSERT_EQ(3, trigger.getNumPendingNodes());
+
+ ASSERT_EQ(4, checker.should_request_cnt);
+ ASSERT_EQ(0, checker.should_retry_cnt);
+ ASSERT_EQ(0, checker.confirmation_cnt);
+
+ /*
+ * Initializing the BeginFirmwareUpdate servers
+ */
+ uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_a(nodes[1]);
+ uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_b(nodes[2]);
+ uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_c(nodes[3]);
+ uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_d(nodes[4]);
+
+ BeginFirmwareUpdateServer srv_a_impl;
+ BeginFirmwareUpdateServer srv_b_impl;
+ BeginFirmwareUpdateServer srv_c_impl;
+ BeginFirmwareUpdateServer srv_d_impl;
+
+ ASSERT_LE(0, srv_a.start(srv_a_impl.makeCallback()));
+ ASSERT_LE(0, srv_b.start(srv_b_impl.makeCallback()));
+ ASSERT_LE(0, srv_c.start(srv_c_impl.makeCallback()));
+ ASSERT_LE(0, srv_d.start(srv_d_impl.makeCallback()));
+
+ srv_a_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry
+ srv_b_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry
+ srv_c_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // ignore (see below)
+ srv_d_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_OK; // OK
+
+ /*
+ * Spinning, now we're getting some errors
+ * This also checks correctness of the round-robin selector
+ */
+ checker.retry_quota = 2;
+ nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200)); // Two will retry, one drop, one confirm
+
+ ASSERT_TRUE(trigger.isTimerRunning());
+
+ nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed now
+
+ EXPECT_EQ(4, checker.should_request_cnt);
+ EXPECT_EQ(4, checker.should_retry_cnt);
+ EXPECT_EQ(1, checker.confirmation_cnt);
+
+ /*
+ * Waiting for the timer to stop
+ */
+ nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1100));
+
+ ASSERT_FALSE(trigger.isTimerRunning());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/global_time_sync_master.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include "helpers.hpp"
+
+struct GlobalTimeSyncMasterTestNode
+{
+ SystemClockDriver clock;
+ PairableCanDriver can;
+ TestNode node;
+
+ GlobalTimeSyncMasterTestNode(uavcan::NodeID nid)
+ : can(clock)
+ , node(can, clock, nid)
+ { }
+};
+
+struct GlobalTimeSyncTestNetwork
+{
+ GlobalTimeSyncMasterTestNode slave;
+ GlobalTimeSyncMasterTestNode master_low;
+ GlobalTimeSyncMasterTestNode master_high;
+
+ GlobalTimeSyncTestNetwork()
+ : slave(64)
+ , master_low(120)
+ , master_high(8)
+ {
+ slave.can.others.insert(&master_low.can);
+ master_low.can.others.insert(&slave.can);
+ master_high.can.others.insert(&slave.can);
+ }
+
+ void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9))
+ {
+ assert(!duration.isNegative());
+ unsigned nspins3 = unsigned(duration.toMSec() / 3);
+ nspins3 = nspins3 ? nspins3 : 2;
+ while (nspins3 --> 0)
+ {
+ ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+ ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+ ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+ }
+ }
+};
+
+TEST(GlobalTimeSyncMaster, Basic)
+{
+ GlobalTimeSyncTestNetwork nwk;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
+
+ uavcan::GlobalTimeSyncSlave slave(nwk.slave.node);
+ uavcan::GlobalTimeSyncMaster master_low(nwk.master_low.node);
+ uavcan::GlobalTimeSyncMaster master_high(nwk.master_high.node);
+
+ ASSERT_FALSE(master_low.isInitialized());
+
+ ASSERT_LE(0, slave.start());
+ ASSERT_LE(0, master_low.init());
+ ASSERT_LE(0, master_high.init());
+
+ ASSERT_TRUE(master_low.isInitialized());
+ ASSERT_FALSE(slave.isActive());
+
+ /*
+ * Simple synchronization
+ */
+ ASSERT_LE(0, master_low.publish()); // Update
+ nwk.spinAll();
+
+ usleep(400000);
+ ASSERT_LE(0, master_low.publish()); // Adjustment
+ nwk.spinAll();
+
+ // Synchronization complete.
+ ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
+ ASSERT_TRUE(slave.isActive());
+ ASSERT_EQ(nwk.master_low.node.getNodeID(), slave.getMasterNodeID());
+
+ /*
+ * Moving clocks forward and re-syncing with another master
+ */
+ static const uavcan::UtcDuration OneDay = uavcan::UtcDuration::fromMSec(24 * 3600 * 1000);
+ nwk.master_high.clock.utc_adjustment = OneDay;
+
+ usleep(400000);
+ ASSERT_LE(0, master_low.publish()); // Update from the old master
+ nwk.spinAll();
+
+ ASSERT_LE(0, master_high.publish()); // Update from the new master
+ nwk.spinAll();
+
+ usleep(400000);
+ ASSERT_LE(0, master_low.publish()); // Adjustment from the old master (ignored now)
+ ASSERT_LE(0, master_high.publish()); // Adjustment from the new master (accepted)
+ nwk.spinAll();
+
+ // Synchronization complete.
+ ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
+ ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
+ ASSERT_TRUE(slave.isActive());
+ ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
+
+ /*
+ * Frequent calls to publish()
+ */
+ ASSERT_LE(0, master_low.publish()); // Dropped
+ ASSERT_LE(0, master_low.publish()); // Dropped
+ ASSERT_LE(0, master_low.publish()); // Dropped
+
+ ASSERT_TRUE(nwk.slave.can.read_queue.empty());
+
+ usleep(400000);
+ ASSERT_LE(0, master_low.publish()); // Accepted
+ ASSERT_FALSE(nwk.slave.can.read_queue.empty());
+
+ nwk.spinAll();
+
+ // Synchronization did not change
+ ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
+ ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
+ ASSERT_TRUE(slave.isActive());
+ ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/global_time_sync_slave.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,314 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include "helpers.hpp"
+
+
+TEST(GlobalTimeSyncSlave, Basic)
+{
+ InterlinkedTestNodesWithClockMock nodes(64, 65);
+
+ SystemClockMock& slave_clock = nodes.clock_a;
+ SystemClockMock& master_clock = nodes.clock_b;
+
+ slave_clock.advance(1000000);
+ master_clock.advance(1000000);
+
+ master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000;
+ master_clock.preserve_utc = slave_clock.preserve_utc = true;
+ slave_clock.utc = 0; // Not set yet
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
+
+ uavcan::GlobalTimeSyncSlave gtss(nodes.a);
+ uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(nodes.b);
+
+ ASSERT_LE(0, gtss.start());
+ ASSERT_FALSE(gtss.isActive());
+ ASSERT_FALSE(gtss.getMasterNodeID().isValid());
+
+ /*
+ * Empty broadcast
+ * The slave must only register the timestamp and adjust nothing
+ */
+ uavcan::protocol::GlobalTimeSync gts;
+ gts.previous_transmission_timestamp_usec = 0;
+ gts_pub.broadcast(gts);
+ gts.previous_transmission_timestamp_usec = master_clock.utc;
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_EQ(0, slave_clock.utc);
+ ASSERT_EQ(1000000, master_clock.utc);
+ std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
+ std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl;
+
+ ASSERT_FALSE(gtss.isActive());
+ ASSERT_FALSE(gtss.getMasterNodeID().isValid());
+
+ /*
+ * Follow-up broadcast with proper time
+ * Slave must adjust now
+ */
+ gts_pub.broadcast(gts);
+ gts.previous_transmission_timestamp_usec = master_clock.utc;
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(1000000, slave_clock.utc);
+ ASSERT_EQ(1000000, master_clock.utc);
+ std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
+ std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl;
+
+ master_clock.utc += 1000000;
+ slave_clock.utc += 1000000;
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
+
+ /*
+ * Next follow-up, slave is synchronized now
+ * Will update
+ */
+ gts_pub.broadcast(gts);
+ gts.previous_transmission_timestamp_usec = master_clock.utc;
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(2000000, slave_clock.utc);
+ ASSERT_EQ(2000000, master_clock.utc);
+
+ master_clock.utc += 1000000;
+ slave_clock.utc += 1000000;
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
+
+ /*
+ * Next follow-up, slave is synchronized now
+ * Will adjust
+ */
+ gts_pub.broadcast(gts);
+ gts.previous_transmission_timestamp_usec = master_clock.utc;
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(3000000, slave_clock.utc);
+ ASSERT_EQ(3000000, master_clock.utc);
+
+ master_clock.utc += 1000000;
+ slave_clock.utc += 1000000;
+ ASSERT_EQ(4000000, slave_clock.utc);
+ ASSERT_EQ(4000000, master_clock.utc);
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
+
+ /*
+ * Another master
+ * This one has higher priority, so it will be preferred
+ */
+ SystemClockMock master2_clock(100);
+ master2_clock.monotonic_auto_advance = 1000;
+ master2_clock.preserve_utc = true;
+ PairableCanDriver master2_can(master2_clock);
+ master2_can.others.insert(&nodes.can_a);
+ TestNode master2_node(master2_can, master2_clock, 8);
+
+ uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub2(master2_node);
+
+ /*
+ * Update step, no adjustment yet
+ */
+ gts.previous_transmission_timestamp_usec = 0;
+ gts_pub2.broadcast(gts);
+ gts.previous_transmission_timestamp_usec = master2_clock.utc;
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(4000000, slave_clock.utc);
+ ASSERT_EQ(100, master2_clock.utc);
+
+ master2_clock.utc += 1000000;
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
+
+ /*
+ * Adjustment
+ */
+ gts_pub2.broadcast(gts);
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(100, slave_clock.utc);
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
+
+ /*
+ * Another master will be ignored now
+ */
+ gts.previous_transmission_timestamp_usec = 99999999;
+ // Update
+ gts_pub.broadcast(gts);
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(100, slave_clock.utc);
+ // Adjust
+ gts_pub.broadcast(gts);
+ nodes.spinBoth(uavcan::MonotonicDuration());
+ ASSERT_EQ(100, slave_clock.utc);
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
+
+ /*
+ * Timeout
+ */
+ slave_clock.advance(100000000);
+
+ ASSERT_FALSE(gtss.isActive());
+ ASSERT_FALSE(gtss.getMasterNodeID().isValid());
+}
+
+
+#if !defined(BYTE_ORDER) || !defined(LITTLE_ENDIAN) || (BYTE_ORDER != LITTLE_ENDIAN)
+# error "This test cannot be executed on this platform"
+#endif
+
+static uavcan::Frame makeSyncMsg(uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
+{
+ uavcan::Frame frame(uavcan::protocol::GlobalTimeSync::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+ snid, uavcan::NodeID::Broadcast, tid);
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+ EXPECT_EQ(7, frame.setPayload(reinterpret_cast<uint8_t*>(&usec), 7)); // Assuming little endian!!!
+ return frame;
+}
+
+static void broadcastSyncMsg(CanIfaceMock& iface, uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
+{
+ const uavcan::Frame frame = makeSyncMsg(usec, snid, tid);
+ uavcan::CanFrame can_frame;
+ ASSERT_TRUE(frame.compile(can_frame));
+ iface.pushRx(can_frame);
+}
+
+
+TEST(GlobalTimeSyncSlave, Validation)
+{
+ SystemClockMock slave_clock;
+ slave_clock.monotonic = 1000000;
+ slave_clock.preserve_utc = true;
+
+ CanDriverMock slave_can(3, slave_clock);
+ for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++)
+ {
+ slave_can.ifaces.at(i).enable_utc_timestamping = true;
+ }
+
+ TestNode node(slave_can, slave_clock, 64);
+
+ uavcan::GlobalTimeSyncSlave gtss(node);
+ uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
+
+ ASSERT_LE(0, gtss.start());
+ ASSERT_FALSE(gtss.isActive());
+ ASSERT_FALSE(gtss.getMasterNodeID().isValid());
+ ASSERT_EQ(0, slave_clock.utc);
+
+ /*
+ * Update/adjust/update
+ */
+ broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(8, gtss.getMasterNodeID().get());
+ ASSERT_EQ(1000, slave_clock.utc);
+
+ broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 2); // Update
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_EQ(1000, slave_clock.utc);
+ std::cout << slave_clock.monotonic << std::endl;
+
+ /*
+ * TID jump simulates a frame loss
+ */
+ broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 4); // Adjustment skipped - expected TID 3, update instead
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(8, gtss.getMasterNodeID().get());
+ ASSERT_EQ(1000, slave_clock.utc);
+ std::cout << slave_clock.monotonic << std::endl;
+
+ /*
+ * Valid adjustment - continuing from TID 4
+ */
+ broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 5); // Slave UTC was 1000, master reports 3000 --> shift ahead
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 5);
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(8, gtss.getMasterNodeID().get());
+ ASSERT_EQ(3000, slave_clock.utc);
+ std::cout << slave_clock.monotonic << std::endl;
+
+ /*
+ * Update, then very long delay with correct TID
+ */
+ broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 6); // Valid update, slave UTC is 3000
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 6);
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ slave_clock.monotonic += 5000000;
+
+ broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 7); // Adjustment skipped
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 7);
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 8); // Valid adjustment now
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 8);
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(8, gtss.getMasterNodeID().get());
+ ASSERT_EQ(5000, slave_clock.utc);
+ std::cout << slave_clock.monotonic << std::endl;
+}
+
+
+TEST(GlobalTimeSyncSlave, Suppression)
+{
+ SystemClockMock slave_clock;
+ slave_clock.monotonic = 1000000;
+ slave_clock.preserve_utc = true;
+
+ CanDriverMock slave_can(3, slave_clock);
+ for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++)
+ {
+ slave_can.ifaces.at(i).enable_utc_timestamping = true;
+ }
+
+ TestNode node(slave_can, slave_clock, 64);
+
+ uavcan::GlobalTimeSyncSlave gtss(node);
+ uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
+
+ ASSERT_LE(0, gtss.start());
+ ASSERT_EQ(0, slave_clock.utc);
+
+ gtss.suppress(true);
+
+ broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
+ broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(gtss.isActive());
+ ASSERT_EQ(8, gtss.getMasterNodeID().get());
+ ASSERT_EQ(0, slave_clock.utc); // The clock shall not be asjusted
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/helpers.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include "../node/test_node.hpp"
+
+
+template <typename DataType>
+class SubscriptionCollector : uavcan::Noncopyable
+{
+ void handler(const DataType& msg)
+ {
+ this->msg.reset(new DataType(msg));
+ }
+
+public:
+ std::auto_ptr<DataType> msg;
+
+ typedef uavcan::MethodBinder<SubscriptionCollector*,
+ void (SubscriptionCollector::*)(const DataType&)> Binder;
+
+ Binder bind() { return Binder(this, &SubscriptionCollector::handler); }
+};
+
+
+template <typename DataType>
+struct SubscriberWithCollector
+{
+ typedef SubscriptionCollector<DataType> Collector;
+ typedef uavcan::Subscriber<DataType, typename Collector::Binder> Subscriber;
+
+ Collector collector;
+ Subscriber subscriber;
+
+ SubscriberWithCollector(uavcan::INode& node)
+ : subscriber(node)
+ { }
+
+ int start() { return subscriber.start(collector.bind()); }
+};
+
+
+template <typename DataType>
+class ServiceCallResultCollector : uavcan::Noncopyable
+{
+ typedef uavcan::ServiceCallResult<DataType> ServiceCallResult;
+
+public:
+ class Result
+ {
+ const typename ServiceCallResult::Status status_;
+ uavcan::ServiceCallID call_id_;
+ typename DataType::Response response_;
+
+ public:
+ Result(typename ServiceCallResult::Status arg_status,
+ uavcan::ServiceCallID arg_call_id,
+ const typename DataType::Response& arg_response)
+ : status_(arg_status)
+ , call_id_(arg_call_id)
+ , response_(arg_response)
+ { }
+
+ bool isSuccessful() const { return status_ == ServiceCallResult::Success; }
+
+ typename ServiceCallResult::Status getStatus() const { return status_; }
+
+ uavcan::ServiceCallID getCallID() const { return call_id_; }
+
+ const typename DataType::Response& getResponse() const { return response_; }
+ typename DataType::Response& getResponse() { return response_; }
+ };
+
+private:
+ void handler(const uavcan::ServiceCallResult<DataType>& tmp_result)
+ {
+ std::cout << tmp_result << std::endl;
+ result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse()));
+ }
+
+public:
+ std::auto_ptr<Result> result;
+
+ typedef uavcan::MethodBinder<ServiceCallResultCollector*,
+ void (ServiceCallResultCollector::*)(const uavcan::ServiceCallResult<DataType>&)>
+ Binder;
+
+ Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); }
+};
+
+
+template <typename DataType>
+struct ServiceClientWithCollector
+{
+ typedef ServiceCallResultCollector<DataType> Collector;
+ typedef uavcan::ServiceClient<DataType, typename Collector::Binder> ServiceClient;
+
+ Collector collector;
+ ServiceClient client;
+
+ ServiceClientWithCollector(uavcan::INode& node)
+ : client(node)
+ { }
+
+ int call(uavcan::NodeID node_id, const typename DataType::Request& request)
+ {
+ client.setCallback(collector.bind());
+ return client.call(node_id, request);
+ }
+};
+
+
+struct BackgroundSpinner : uavcan::TimerBase
+{
+ uavcan::INode& spinning_node;
+
+ BackgroundSpinner(uavcan::INode& spinning_node, uavcan::INode& running_node)
+ : uavcan::TimerBase(running_node)
+ , spinning_node(spinning_node)
+ { }
+
+ virtual void handleTimerEvent(const uavcan::TimerEvent&)
+ {
+ spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1));
+ }
+};
+
+template <typename CanDriver, typename MessageType>
+static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id,
+ const MessageType& message, uavcan::TransferID tid)
+{
+ uavcan::StaticTransferBuffer<100> buffer;
+ uavcan::BitStream bitstream(buffer);
+ uavcan::ScalarCodec codec(bitstream);
+
+ // Manual message publication
+ ASSERT_LT(0, MessageType::encode(message, codec));
+ ASSERT_GE(8, buffer.getMaxWritePos());
+
+ // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
+ uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+ node_id, uavcan::NodeID::Broadcast, tid);
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+
+ ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos()));
+
+ uavcan::CanFrame can_frame;
+ ASSERT_TRUE(frame.compile(can_frame));
+
+ can.pushRxToAllIfaces(can_frame);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/logger.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/logger.hpp>
+#include "helpers.hpp"
+
+
+struct LogSink : public uavcan::ILogSink
+{
+ std::queue<uavcan::protocol::debug::LogMessage> msgs;
+ LogLevel level;
+
+ LogSink()
+ : level(uavcan::protocol::debug::LogLevel::ERROR)
+ { }
+
+ LogLevel getLogLevel() const { return level; }
+
+ void log(const uavcan::protocol::debug::LogMessage& message)
+ {
+ msgs.push(message);
+ std::cout << message << std::endl;
+ }
+
+ uavcan::protocol::debug::LogMessage pop()
+ {
+ if (msgs.empty())
+ {
+ std::cout << "LogSink is empty" << std::endl;
+ std::abort();
+ }
+ const uavcan::protocol::debug::LogMessage m = msgs.front();
+ msgs.pop();
+ return m;
+ }
+
+ bool popMatchByLevelAndText(int level, const std::string& source, const std::string& text)
+ {
+ if (msgs.empty())
+ {
+ std::cout << "LogSink is empty" << std::endl;
+ return false;
+ }
+ const uavcan::protocol::debug::LogMessage m = pop();
+ return
+ level == m.level.value &&
+ source == m.source &&
+ text == m.text;
+ }
+};
+
+
+TEST(Logger, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::Logger logger(nodes.a);
+
+ ASSERT_EQ(uavcan::protocol::debug::LogLevel::ERROR, logger.getLevel());
+
+ LogSink sink;
+
+ // Will fail - types are not registered
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ ASSERT_GT(0, logger.logError("foo", "Error (fail - type is not registered)"));
+ ASSERT_EQ(0, logger.logDebug("foo", "Debug (ignored - low logging level)"));
+
+ ASSERT_FALSE(logger.getExternalSink());
+ logger.setExternalSink(&sink);
+ ASSERT_EQ(&sink, logger.getExternalSink());
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::debug::LogMessage> _reg1;
+
+ SubscriberWithCollector<uavcan::protocol::debug::LogMessage> log_sub(nodes.b);
+ ASSERT_LE(0, log_sub.start());
+
+ // Sink test
+ ASSERT_EQ(0, logger.logDebug("foo", "Debug (ignored due to low logging level)"));
+ ASSERT_TRUE(sink.msgs.empty());
+
+ sink.level = uavcan::protocol::debug::LogLevel::DEBUG;
+ ASSERT_EQ(0, logger.logDebug("foo", "Debug (sink only)"));
+ ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::DEBUG, "foo", "Debug (sink only)"));
+
+ ASSERT_LE(0, logger.logError("foo", "Error"));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_TRUE(log_sub.collector.msg.get());
+ ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::ERROR);
+ ASSERT_EQ(log_sub.collector.msg->source, "foo");
+ ASSERT_EQ(log_sub.collector.msg->text, "Error");
+
+ logger.setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+ ASSERT_LE(0, logger.logWarning("foo", "Warning"));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::WARNING);
+ ASSERT_EQ(log_sub.collector.msg->source, "foo");
+ ASSERT_EQ(log_sub.collector.msg->text, "Warning");
+
+ ASSERT_LE(0, logger.logInfo("foo", "Info"));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::INFO);
+ ASSERT_EQ(log_sub.collector.msg->source, "foo");
+ ASSERT_EQ(log_sub.collector.msg->text, "Info");
+
+ ASSERT_LE(0, logger.logDebug("foo", "Debug"));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::DEBUG);
+ ASSERT_EQ(log_sub.collector.msg->source, "foo");
+ ASSERT_EQ(log_sub.collector.msg->text, "Debug");
+
+ ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::ERROR, "foo", "Error"));
+ ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::WARNING, "foo", "Warning"));
+ ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::INFO, "foo", "Info"));
+ ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::DEBUG, "foo", "Debug"));
+}
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+TEST(Logger, Cpp11Formatting)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::Logger logger(nodes.a);
+ logger.setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+ SubscriberWithCollector<uavcan::protocol::debug::LogMessage> log_sub(nodes.b);
+ ASSERT_LE(0, log_sub.start());
+
+ ASSERT_LE(0, logger.logWarning("foo", "char='%*', %* is %*", '$', "double", 12.34));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_TRUE(log_sub.collector.msg.get());
+ ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::WARNING);
+ ASSERT_EQ(log_sub.collector.msg->source, "foo");
+ ASSERT_EQ(log_sub.collector.msg->text, "char='$', double is 12.34");
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/node_info_retriever.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,293 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <memory>
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id,
+ uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
+{
+ uavcan::protocol::NodeStatus msg;
+ msg.health = uavcan::protocol::NodeStatus::HEALTH_OK;
+ msg.mode = uavcan::protocol::NodeStatus::MODE_OPERATIONAL;
+ msg.uptime_sec = uptime_sec;
+ emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
+}
+
+
+struct NodeInfoListener : public uavcan::INodeInfoListener
+{
+ std::auto_ptr<uavcan::protocol::GetNodeInfo::Response> last_node_info;
+ uavcan::NodeID last_node_id;
+ unsigned status_message_cnt;
+ unsigned status_change_cnt;
+ unsigned info_unavailable_cnt;
+
+ NodeInfoListener()
+ : status_message_cnt(0)
+ , status_change_cnt(0)
+ , info_unavailable_cnt(0)
+ { }
+
+ virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id,
+ const uavcan::protocol::GetNodeInfo::Response& node_info)
+ {
+ last_node_id = node_id;
+ std::cout << node_info << std::endl;
+ last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info));
+ }
+
+ virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id)
+ {
+ std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl;
+ last_node_id = node_id;
+ info_unavailable_cnt++;
+ }
+
+ virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event)
+ {
+ std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: "
+ << event.old_status.toString() << " --> " << event.status.toString() << std::endl;
+ status_change_cnt++;
+ }
+
+ virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
+ {
+ std::cout << msg << std::endl;
+ status_message_cnt++;
+ }
+};
+
+
+TEST(NodeInfoRetriever, Basic)
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::NodeInfoRetriever retr(nodes.a);
+ std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
+ std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
+ << sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
+
+ std::auto_ptr<uavcan::NodeStatusProvider> provider(new uavcan::NodeStatusProvider(nodes.b));
+
+ NodeInfoListener listener;
+
+ /*
+ * Initialization
+ */
+ ASSERT_LE(0, retr.start());
+
+ retr.removeListener(&listener); // Does nothing
+ retr.addListener(&listener);
+ retr.addListener(&listener);
+ retr.addListener(&listener);
+ ASSERT_EQ(1, retr.getNumListeners());
+
+ uavcan::protocol::HardwareVersion hwver;
+ hwver.unique_id[0] = 123;
+ hwver.unique_id[4] = 213;
+ hwver.unique_id[8] = 45;
+
+ provider->setName("Ivan");
+ provider->setHardwareVersion(hwver);
+
+ ASSERT_LE(0, provider->startAndPublish());
+
+ ASSERT_FALSE(retr.isRetrievingInProgress());
+ ASSERT_EQ(0, retr.getNumPendingRequests());
+
+ EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default
+
+ /*
+ * Waiting for discovery
+ */
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50));
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
+ ASSERT_FALSE(retr.isRetrievingInProgress());
+
+ ASSERT_EQ(2, listener.status_message_cnt);
+ ASSERT_EQ(1, listener.status_change_cnt);
+ ASSERT_EQ(0, listener.info_unavailable_cnt);
+ ASSERT_TRUE(listener.last_node_info.get());
+ ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id);
+ ASSERT_EQ("Ivan", listener.last_node_info->name);
+ ASSERT_TRUE(hwver == listener.last_node_info->hardware_version);
+
+ provider.reset(); // Moving the provider out of the way; its entry will timeout meanwhile
+
+ /*
+ * Declaring a bunch of different nodes that don't support GetNodeInfo
+ */
+ ASSERT_FALSE(retr.isRetrievingInProgress());
+
+ retr.setNumRequestAttempts(3);
+
+ uavcan::TransferID tid;
+
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 10, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 10, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_LE(1, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_LE(2, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_EQ(3, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+
+ tid.increment();
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 11, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 11, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_LE(1, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_LE(2, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_EQ(3, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+
+ tid.increment();
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 12, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 12, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid); // Reset
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_LE(1, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_LE(2, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_EQ(3, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+
+ EXPECT_EQ(11, listener.status_message_cnt);
+ EXPECT_EQ(5, listener.status_change_cnt); // node 2 online/offline + 3 test nodes above
+ EXPECT_EQ(2, listener.info_unavailable_cnt);
+
+ tid.increment();
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_EQ(1, retr.getNumPendingRequests());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+ ASSERT_EQ(1, retr.getNumPendingRequests()); // Still one because two went offline
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200));
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+
+ tid.increment();
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 12, tid);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200));
+ ASSERT_FALSE(retr.isRetrievingInProgress()); // Out of attempts, stopping
+ ASSERT_EQ(0, retr.getNumPendingRequests());
+
+ EXPECT_EQ(13, listener.status_message_cnt);
+ EXPECT_EQ(7, listener.status_change_cnt); // node 2 online/offline + 2 test nodes above online/offline + 1
+ EXPECT_EQ(3, listener.info_unavailable_cnt);
+
+ /*
+ * Forcing the class to forget everything
+ */
+ std::cout << "Invalidation" << std::endl;
+
+ retr.invalidateAll();
+
+ ASSERT_FALSE(retr.isRetrievingInProgress());
+ ASSERT_EQ(0, retr.getNumPendingRequests());
+
+ tid.increment();
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 60, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 60, tid);
+ publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 60, tid);
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
+
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+ ASSERT_EQ(3, retr.getNumPendingRequests());
+}
+
+
+TEST(NodeInfoRetriever, MaxConcurrentRequests)
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::NodeInfoRetriever retr(nodes.a);
+ std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
+ std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
+ << sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
+
+ NodeInfoListener listener;
+
+ /*
+ * Initialization
+ */
+ ASSERT_LE(0, retr.start());
+
+ retr.addListener(&listener);
+ ASSERT_EQ(1, retr.getNumListeners());
+
+ ASSERT_FALSE(retr.isRetrievingInProgress());
+ ASSERT_EQ(0, retr.getNumPendingRequests());
+
+ ASSERT_EQ(40, retr.getRequestInterval().toMSec());
+
+ const unsigned MaxPendingRequests = 26; // See class docs
+ const unsigned MinPendingRequestsAtFullLoad = 12;
+
+ /*
+ * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit.
+ */
+ for (uint8_t node_id = 1U; node_id <= 127U; node_id++)
+ {
+ publishNodeStatus(nodes.can_a, node_id, 0, uavcan::TransferID());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+ }
+
+ ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
+ ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests());
+
+ for (int i = 0; i < 8; i++) // Approximate
+ {
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35));
+ std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl;
+
+ ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
+ ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests());
+
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+ }
+
+ ASSERT_LT(0, retr.getNumPendingRequests());
+ ASSERT_TRUE(retr.isRetrievingInProgress());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000));
+
+ ASSERT_EQ(0, retr.getNumPendingRequests());
+ ASSERT_FALSE(retr.isRetrievingInProgress());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/node_status_monitor.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id,
+ uavcan::uint8_t health, uavcan::uint8_t mode,
+ uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
+{
+ uavcan::protocol::NodeStatus msg;
+ msg.health = health;
+ msg.mode = mode;
+ msg.uptime_sec = uptime_sec;
+ emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
+}
+
+
+static void shortSpin(TestNode& node)
+{
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+}
+
+
+TEST(NodeStatusMonitor, Basic)
+{
+ using uavcan::protocol::NodeStatus;
+ using uavcan::NodeID;
+
+ SystemClockMock clock_mock(100);
+ clock_mock.monotonic_auto_advance = 1000;
+
+ CanDriverMock can(2, clock_mock);
+
+ TestNode node(can, clock_mock, 64);
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+
+ uavcan::NodeStatusMonitor nsm(node);
+ ASSERT_LE(0, nsm.start());
+
+ ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+ /*
+ * Empty NSM, no nodes were registered yet
+ */
+ ASSERT_FALSE(nsm.findNodeWithWorstHealth().isValid());
+
+ ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(123)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode);
+
+ /*
+ * Some new status messages
+ */
+ publishNodeStatus(can, 10, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 12, 0);
+ shortSpin(node);
+ ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstHealth());
+
+ publishNodeStatus(can, 9, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_INITIALIZATION, 0, 0);
+ shortSpin(node);
+ ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth());
+
+ publishNodeStatus(can, 11, NodeStatus::HEALTH_CRITICAL, NodeStatus::MODE_MAINTENANCE, 999, 0);
+ shortSpin(node);
+ ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth());
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
+ ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
+ ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
+ ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
+
+ /*
+ * Timeout
+ */
+ std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl;
+
+ clock_mock.advance(500000);
+ shortSpin(node);
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
+ ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
+
+ clock_mock.advance(500000);
+ shortSpin(node);
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
+ ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
+
+ clock_mock.advance(500000);
+ shortSpin(node);
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
+ ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
+
+ /*
+ * Will timeout now
+ */
+ clock_mock.advance(4000000);
+ shortSpin(node);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
+
+ /*
+ * Recovering one node, adding two extra
+ */
+ publishNodeStatus(can, 11, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 999, 1);
+ shortSpin(node);
+
+ publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1);
+ shortSpin(node);
+
+ publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1);
+ shortSpin(node);
+
+ /*
+ * Making sure OFFLINE is still worst status
+ */
+ ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth());
+
+ /*
+ * Final validation
+ */
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
+ ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127)));
+ ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health);
+
+ ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1)));
+ ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health);
+
+ /*
+ * Forgetting
+ */
+ nsm.forgetNode(127);
+ ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(127)).health);
+
+ nsm.forgetNode(9);
+ ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9)));
+ ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
+ ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(9)).health);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/node_status_provider.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+
+struct AdHocNodeStatusUpdater : public uavcan::IAdHocNodeStatusUpdater
+{
+ uavcan::uint64_t invokations;
+
+ AdHocNodeStatusUpdater() : invokations(0) { }
+
+ virtual void updateNodeStatus()
+ {
+ invokations++;
+ }
+};
+
+
+TEST(NodeStatusProvider, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::NodeStatusProvider nsp(nodes.a);
+
+ /*
+ * Initialization
+ */
+ uavcan::protocol::HardwareVersion hwver;
+ hwver.major = 3;
+ hwver.minor = 14;
+
+ uavcan::protocol::SoftwareVersion swver;
+ swver.major = 2;
+ swver.minor = 18;
+ swver.vcs_commit = 0x600DF00D;
+
+ nsp.setHardwareVersion(hwver);
+ nsp.setSoftwareVersion(swver);
+
+ ASSERT_TRUE(nsp.getName().empty());
+ nsp.setName("superluminal_communication_unit");
+ ASSERT_STREQ("superluminal_communication_unit", nsp.getName().c_str());
+
+ ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_OK, nsp.getHealth());
+ ASSERT_EQ(uavcan::protocol::NodeStatus::MODE_INITIALIZATION, nsp.getMode());
+ nsp.setHealthError();
+ nsp.setModeOperational();
+ ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, nsp.getHealth());
+ ASSERT_EQ(uavcan::protocol::NodeStatus::MODE_OPERATIONAL, nsp.getMode());
+
+ // Will fail - types are not registered
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ ASSERT_GT(0, nsp.startAndPublish());
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+ ASSERT_LE(0, nsp.startAndPublish());
+
+ // Checking the publishing rate settings
+ ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS),
+ nsp.getStatusPublicationPeriod());
+
+ nsp.setStatusPublicationPeriod(uavcan::MonotonicDuration());
+ ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS),
+ nsp.getStatusPublicationPeriod());
+
+ nsp.setStatusPublicationPeriod(uavcan::MonotonicDuration::fromMSec(3600 * 1000 * 24));
+ ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS),
+ nsp.getStatusPublicationPeriod());
+
+ AdHocNodeStatusUpdater ad_hoc;
+ ASSERT_EQ(UAVCAN_NULLPTR, nsp.getAdHocNodeStatusUpdater());
+ nsp.setAdHocNodeStatusUpdater(&ad_hoc);
+ ASSERT_EQ(&ad_hoc, nsp.getAdHocNodeStatusUpdater());
+
+ /*
+ * Initial status publication
+ */
+ SubscriberWithCollector<uavcan::protocol::NodeStatus> status_sub(nodes.b);
+
+ ASSERT_LE(0, status_sub.start());
+ ASSERT_FALSE(status_sub.collector.msg.get()); // No data yet
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(status_sub.collector.msg.get()); // Was published at startup
+ ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, status_sub.collector.msg->health);
+ ASSERT_EQ(0, status_sub.collector.msg->vendor_specific_status_code);
+ ASSERT_GE(1, status_sub.collector.msg->uptime_sec);
+
+ ASSERT_EQ(0, ad_hoc.invokations); // Not invoked from startAndPublish()
+
+ /*
+ * Altering the vendor-specific status code, forcePublish()-ing it and checking the result
+ */
+ ASSERT_EQ(0, nsp.getVendorSpecificStatusCode());
+ nsp.setVendorSpecificStatusCode(1234);
+ ASSERT_EQ(1234, nsp.getVendorSpecificStatusCode());
+
+ ASSERT_LE(0, nsp.forcePublish());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, status_sub.collector.msg->health);
+ ASSERT_EQ(1234, status_sub.collector.msg->vendor_specific_status_code);
+ ASSERT_GE(1, status_sub.collector.msg->uptime_sec);
+
+ ASSERT_EQ(0, ad_hoc.invokations); // Not invoked from forcePublish()
+
+ /*
+ * Explicit node info request
+ */
+ ServiceClientWithCollector<uavcan::protocol::GetNodeInfo> gni_cln(nodes.b);
+
+ nsp.setHealthCritical();
+
+ ASSERT_FALSE(gni_cln.collector.result.get()); // No data yet
+ ASSERT_LE(0, gni_cln.call(1, uavcan::protocol::GetNodeInfo::Request()));
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_TRUE(gni_cln.collector.result.get()); // Response must have been delivered
+
+ ASSERT_TRUE(gni_cln.collector.result->isSuccessful());
+ ASSERT_EQ(1, gni_cln.collector.result->getCallID().server_node_id.get());
+
+ ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_CRITICAL,
+ gni_cln.collector.result->getResponse().status.health);
+
+ ASSERT_TRUE(hwver == gni_cln.collector.result->getResponse().hardware_version);
+ ASSERT_TRUE(swver == gni_cln.collector.result->getResponse().software_version);
+
+ ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->getResponse().name);
+
+ ASSERT_EQ(0, ad_hoc.invokations); // No timer-triggered publications happened yet
+
+ /*
+ * Timer triggered publication
+ */
+ EXPECT_EQ(3, nodes.a.getDispatcher().getTransferPerfCounter().getTxTransferCount());
+
+ nodes.spinBoth(nsp.getStatusPublicationPeriod());
+
+ EXPECT_EQ(1, ad_hoc.invokations); // No timer-triggered publications happened yet
+ EXPECT_EQ(4, nodes.a.getDispatcher().getTransferPerfCounter().getTxTransferCount());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/panic_broadcaster.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/panic_broadcaster.hpp>
+#include "helpers.hpp"
+
+
+TEST(PanicBroadcaster, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::PanicBroadcaster panicker(nodes.a);
+
+ SubscriberWithCollector<uavcan::protocol::Panic> sub(nodes.b);
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::Panic> _reg1;
+
+ ASSERT_LE(0, sub.start());
+
+ panicker.panic("I lost my towel!"); // Only the first 7 chars allowed
+
+ ASSERT_STREQ("I lost ", panicker.getReason().c_str());
+ ASSERT_TRUE(panicker.isPanicking());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str());
+ sub.collector.msg.reset();
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str());
+ sub.collector.msg.reset();
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300));
+ ASSERT_TRUE(sub.collector.msg.get());
+ ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str());
+ sub.collector.msg.reset();
+
+ panicker.dontPanic();
+ ASSERT_FALSE(panicker.isPanicking());
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300));
+ ASSERT_FALSE(sub.collector.msg.get());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/panic_listener.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/panic_listener.hpp>
+#include <uavcan/protocol/panic_broadcaster.hpp>
+#include "helpers.hpp"
+
+
+struct PanicHandler
+{
+ uavcan::protocol::Panic msg;
+
+ PanicHandler() : msg() { }
+
+ void handle(const uavcan::protocol::Panic& msg)
+ {
+ std::cout << msg << std::endl;
+ this->msg = msg;
+ }
+
+ typedef uavcan::MethodBinder<PanicHandler*, void (PanicHandler::*)(const uavcan::protocol::Panic& msg)> Binder;
+
+ Binder bind() { return Binder(this, &PanicHandler::handle); }
+};
+
+
+TEST(PanicListener, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::Panic> _reg1;
+
+ uavcan::PanicListener<PanicHandler::Binder> pl(nodes.a);
+ uavcan::PanicBroadcaster pbr(nodes.b);
+ PanicHandler handler;
+ ASSERT_LE(0, pl.start(handler.bind()));
+
+ pbr.panic("PANIC!!!");
+ ASSERT_TRUE(handler.msg == uavcan::protocol::Panic()); // One message published, panic is not registered yet
+
+ pbr.dontPanic();
+ ASSERT_FALSE(pbr.isPanicking());
+ std::cout << "Not panicking" << std::endl;
+
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); // Will reset
+ ASSERT_TRUE(handler.msg == uavcan::protocol::Panic());
+
+ pbr.panic("PANIC2!!!"); // Message text doesn't matter
+ ASSERT_TRUE(pbr.isPanicking());
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+ ASSERT_STREQ("PANIC2!", handler.msg.reason_text.c_str()); // Registered, only 7 chars preserved
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/param_server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <map>
+#include <gtest/gtest.h>
+#include <uavcan/protocol/param_server.hpp>
+#include "helpers.hpp"
+
+struct ParamServerTestManager : public uavcan::IParamManager
+{
+ typedef std::map<std::string, double> KeyValue;
+ KeyValue kv;
+
+ virtual void getParamNameByIndex(Index index, Name& out_name) const
+ {
+ Index current_idx = 0;
+ for (KeyValue::const_iterator it = kv.begin(); it != kv.end(); ++it, ++current_idx)
+ {
+ if (current_idx == index)
+ {
+ out_name = it->first.c_str();
+ break;
+ }
+ }
+ }
+
+ virtual void assignParamValue(const Name& name, const Value& value)
+ {
+ assert(!name.empty());
+ std::cout << "ASSIGN [" << name.c_str() << "]\n" << value << "\n---" << std::endl;
+ KeyValue::iterator it = kv.find(name.c_str());
+ if (it != kv.end())
+ {
+ if (value.is(Value::Tag::boolean_value))
+ {
+ assert(value.getTag() == Value::Tag::boolean_value);
+ it->second = double(value.boolean_value);
+ }
+ else if (value.is(Value::Tag::integer_value))
+ {
+ assert(value.getTag() == Value::Tag::integer_value);
+ it->second = double(value.integer_value);
+ }
+ else if (value.is(Value::Tag::real_value))
+ {
+ assert(value.getTag() == Value::Tag::real_value);
+ it->second = double(value.real_value);
+ }
+ else if (value.is(Value::Tag::string_value))
+ {
+ assert(value.getTag() == Value::Tag::string_value);
+ it->second = std::atof(value.string_value.c_str());
+ }
+ else
+ {
+ assert(0);
+ }
+ }
+ }
+
+ virtual void readParamValue(const Name& name, Value& out_value) const
+ {
+ assert(!name.empty());
+ KeyValue::const_iterator it = kv.find(name.c_str());
+ if (it != kv.end())
+ {
+ out_value.to<Value::Tag::real_value>() = float(it->second);
+ assert(out_value.getTag() == Value::Tag::real_value);
+ }
+ std::cout << "READ [" << name.c_str() << "]\n" << out_value << "\n---" << std::endl;
+ }
+
+ virtual int saveAllParams()
+ {
+ std::cout << "SAVE" << std::endl;
+ return 0;
+ }
+
+ virtual int eraseAllParams()
+ {
+ std::cout << "ERASE" << std::endl;
+ return 0;
+ }
+};
+
+
+template <typename Client, typename Message>
+static void doCall(Client& client, const Message& request, InterlinkedTestNodesWithSysClock& nodes)
+{
+ ASSERT_LE(0, client.call(1, request));
+ ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
+ ASSERT_TRUE(client.collector.result.get());
+ ASSERT_TRUE(client.collector.result->isSuccessful());
+}
+
+
+TEST(ParamServer, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::ParamServer server(nodes.a);
+
+ ParamServerTestManager mgr;
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::param::GetSet> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::param::ExecuteOpcode> _reg2;
+
+ ASSERT_LE(0, server.start(&mgr));
+
+ ServiceClientWithCollector<uavcan::protocol::param::GetSet> get_set_cln(nodes.b);
+ ServiceClientWithCollector<uavcan::protocol::param::ExecuteOpcode> save_erase_cln(nodes.b);
+
+ /*
+ * Save/erase
+ */
+ uavcan::protocol::param::ExecuteOpcode::Request save_erase_rq;
+ save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
+ doCall(save_erase_cln, save_erase_rq, nodes);
+ ASSERT_TRUE(save_erase_cln.collector.result.get());
+ ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok);
+
+ save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
+ doCall(save_erase_cln, save_erase_rq, nodes);
+ ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok);
+
+ // Invalid opcode
+ save_erase_rq.opcode = 0xFF;
+ doCall(save_erase_cln, save_erase_rq, nodes);
+ ASSERT_FALSE(save_erase_cln.collector.result->getResponse().ok);
+
+ /*
+ * Get/set
+ */
+ uavcan::protocol::param::GetSet::Request get_set_rq;
+ get_set_rq.name = "nonexistent_parameter";
+ doCall(get_set_cln, get_set_rq, nodes);
+ ASSERT_TRUE(get_set_cln.collector.result.get());
+ ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty());
+
+ // No such variable, shall return empty name/value
+ get_set_rq.index = 0;
+ get_set_rq.name.clear();
+ get_set_rq.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = 0xDEADBEEF;
+ doCall(get_set_cln, get_set_rq, nodes);
+ ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty());
+ ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.is(uavcan::protocol::param::Value::Tag::empty));
+
+ mgr.kv["foobar"] = 123.456; // New param
+
+ // Get by name
+ get_set_rq = uavcan::protocol::param::GetSet::Request();
+ get_set_rq.name = "foobar";
+ doCall(get_set_cln, get_set_rq, nodes);
+ ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str());
+ ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.is(uavcan::protocol::param::Value::Tag::real_value));
+ ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->getResponse().value.
+ to<uavcan::protocol::param::Value::Tag::real_value>());
+
+ // Set by index
+ get_set_rq = uavcan::protocol::param::GetSet::Request();
+ get_set_rq.index = 0;
+ get_set_rq.value.to<uavcan::protocol::param::Value::Tag::string_value>() = "424242";
+ doCall(get_set_cln, get_set_rq, nodes);
+ ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str());
+ ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value.
+ to<uavcan::protocol::param::Value::Tag::real_value>());
+
+ // Get by index
+ get_set_rq = uavcan::protocol::param::GetSet::Request();
+ get_set_rq.index = 0;
+ doCall(get_set_cln, get_set_rq, nodes);
+ ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str());
+ ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value.
+ to<uavcan::protocol::param::Value::Tag::real_value>());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/restart_request_server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/restart_request_server.hpp>
+#include "helpers.hpp"
+
+
+struct RestartHandler : public uavcan::IRestartRequestHandler
+{
+ bool accept;
+
+ bool handleRestartRequest(uavcan::NodeID request_source)
+ {
+ std::cout << "Restart request from " << int(request_source.get()) << " will be "
+ << (accept ? "accepted" : "rejected") << std::endl;
+ return accept;
+ }
+};
+
+
+TEST(RestartRequestServer, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::RestartRequestServer rrs(nodes.a);
+
+ ServiceClientWithCollector<uavcan::protocol::RestartNode> rrs_cln(nodes.b);
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::RestartNode> _reg1;
+
+ ASSERT_LE(0, rrs.start());
+
+ uavcan::protocol::RestartNode::Request request;
+ request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER;
+
+ /*
+ * Rejected - handler was not set
+ */
+ ASSERT_LE(0, rrs_cln.call(1, request));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(rrs_cln.collector.result.get());
+ ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+ ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok);
+
+ /*
+ * Accepted
+ */
+ RestartHandler handler;
+ handler.accept = true;
+ rrs.setHandler(&handler);
+
+ ASSERT_LE(0, rrs_cln.call(1, request));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+ ASSERT_TRUE(rrs_cln.collector.result->getResponse().ok);
+
+ /*
+ * Rejected by handler
+ */
+ handler.accept = false;
+
+ ASSERT_LE(0, rrs_cln.call(1, request));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+ ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok);
+
+ /*
+ * Rejected because of invalid magic number
+ */
+ handler.accept = true;
+
+ ASSERT_LE(0, rrs_cln.call(1, uavcan::protocol::RestartNode::Request()));
+ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+ ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+ ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/protocol/transport_stats_provider.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/transport_stats_provider.hpp>
+#include "helpers.hpp"
+
+
+TEST(TransportStatsProvider, Basic)
+{
+ InterlinkedTestNodesWithSysClock nodes;
+
+ uavcan::TransportStatsProvider tsp(nodes.a);
+
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetTransportStats> _reg1;
+
+ ASSERT_LE(0, tsp.start());
+
+ ServiceClientWithCollector<uavcan::protocol::GetTransportStats> tsp_cln(nodes.b);
+
+ /*
+ * First request
+ */
+ ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request()));
+ ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(tsp_cln.collector.result.get());
+ ASSERT_TRUE(tsp_cln.collector.result->isSuccessful());
+ ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors);
+ ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_rx);
+ ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfers_tx);
+ ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size());
+ ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors);
+ ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx);
+ ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx);
+
+ /*
+ * Second request
+ */
+ ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request()));
+ ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(tsp_cln.collector.result.get());
+ ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors);
+ ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_rx);
+ ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_tx);
+ ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size());
+ ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors);
+ ASSERT_EQ(2, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx);
+ ASSERT_EQ(6, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx);
+
+ /*
+ * Sending a malformed frame, it must be registered as tranfer error
+ */
+ uavcan::Frame frame(uavcan::protocol::GetTransportStats::DefaultDataTypeID, uavcan::TransferTypeServiceRequest,
+ 2, 1, 1);
+ frame.setStartOfTransfer(true);
+ frame.setEndOfTransfer(true);
+ uavcan::CanFrame can_frame;
+ ASSERT_TRUE(frame.compile(can_frame));
+ nodes.can_a.read_queue.push(can_frame);
+
+ ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
+
+ /*
+ * Introducing a CAN driver error
+ */
+ nodes.can_a.error_count = 72;
+
+ /*
+ * Third request
+ */
+ ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request()));
+ ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
+
+ ASSERT_TRUE(tsp_cln.collector.result.get());
+ EXPECT_EQ(1, tsp_cln.collector.result->getResponse().transfer_errors); // That broken frame
+ EXPECT_EQ(3, tsp_cln.collector.result->getResponse().transfers_rx);
+ EXPECT_EQ(2, tsp_cln.collector.result->getResponse().transfers_tx);
+ EXPECT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size());
+ EXPECT_EQ(72, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors);
+ EXPECT_EQ(4, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); // Same here
+ EXPECT_EQ(12, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/test_main.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/build_config.hpp>
+#include <cstdio>
+#include <cstdlib>
+#include <execinfo.h>
+#include <signal.h>
+#include <unistd.h>
+
+static void sigsegv_handler(int sig)
+{
+ const int BacktraceSize = 32;
+ void* array[BacktraceSize];
+ const int size = backtrace(array, BacktraceSize);
+
+ std::fprintf(stderr, "SIGNAL %d RECEIVED; STACKTRACE:\n", sig);
+ backtrace_symbols_fd(array, size, STDERR_FILENO);
+ std::exit(1);
+}
+
+int main(int argc, char **argv)
+{
+ signal(SIGSEGV, sigsegv_handler);
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+#if UAVCAN_CPP_VERSION == UAVCAN_CPP11
+ std::cout << "C++11" << std::endl;
+#elif UAVCAN_CPP_VERSION == UAVCAN_CPP03
+ std::cout << "C++03" << std::endl;
+#else
+# error UAVCAN_CPP_VERSION
+#endif
+
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/time.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/time.hpp>
+#include <uavcan/Timestamp.hpp>
+
+
+TEST(Time, Monotonic)
+{
+ using uavcan::MonotonicDuration;
+ using uavcan::MonotonicTime;
+
+ MonotonicTime m1;
+ MonotonicTime m2 = MonotonicTime::fromMSec(1);
+ MonotonicDuration md1 = m2 - m1; // 1000
+ MonotonicDuration md2 = m1 - m2; // -1000
+
+ ASSERT_EQ(0, m1.toUSec());
+ ASSERT_EQ(1000, m2.toUSec());
+ ASSERT_EQ(1000, md1.toUSec());
+ ASSERT_EQ(-1000, md2.toUSec());
+
+ ASSERT_LT(m1, m2);
+ ASSERT_LE(m1, m2);
+ ASSERT_NE(m1, m2);
+ ASSERT_TRUE(m1.isZero());
+ ASSERT_FALSE(m2.isZero());
+
+ ASSERT_GT(md1, md2);
+ ASSERT_GE(md1, md2);
+ ASSERT_NE(md1, md2);
+ ASSERT_FALSE(md1.isZero());
+ ASSERT_TRUE(md1.isPositive());
+ ASSERT_TRUE(md2.isNegative());
+
+ ASSERT_EQ(0, (md1 + md2).toUSec());
+ ASSERT_EQ(2000, (md1 - md2).toUSec());
+
+ md1 *= 2; // 2000
+ ASSERT_EQ(2000, md1.toUSec());
+
+ md2 += md1; // md2 = -1000 + 2000
+ ASSERT_EQ(1000, md2.toUSec());
+
+ ASSERT_EQ(-1000, (-md2).toUSec());
+
+ /*
+ * To string
+ */
+ ASSERT_EQ("0.000000", m1.toString());
+ ASSERT_EQ("0.001000", m2.toString());
+
+ ASSERT_EQ("0.002000", md1.toString());
+ ASSERT_EQ("-0.001000", (-md2).toString());
+
+ ASSERT_EQ("1001.000001", MonotonicTime::fromUSec(1001000001).toString());
+ ASSERT_EQ("-1001.000001", MonotonicDuration::fromUSec(-1001000001).toString());
+}
+
+
+TEST(Time, Utc)
+{
+ using uavcan::UtcDuration;
+ using uavcan::UtcTime;
+ using uavcan::Timestamp;
+
+ Timestamp ts;
+ ts.usec = 9000;
+
+ UtcTime u1(ts);
+ ASSERT_EQ(9000, u1.toUSec());
+
+ ts.usec *= 2;
+ u1 = ts;
+ ASSERT_EQ(18000, u1.toUSec());
+
+ ts = UtcTime::fromUSec(12345678900);
+ ASSERT_EQ(12345678900, ts.usec);
+
+ /*
+ * To string
+ */
+ ASSERT_EQ("0.018000", u1.toString());
+ ASSERT_EQ("12345.678900", UtcTime(ts).toString());
+}
+
+
+TEST(Time, Overflow)
+{
+ using uavcan::MonotonicDuration;
+ using uavcan::MonotonicTime;
+
+ MonotonicTime max_4 = MonotonicTime::fromUSec(MonotonicTime::getMax().toUSec() / 4);
+ MonotonicDuration max_4_duration = max_4 - MonotonicTime();
+
+ std::cout << max_4 << std::endl;
+ ASSERT_EQ(max_4_duration.toUSec(), max_4.toUSec());
+
+ MonotonicTime max = (((max_4 + max_4_duration) + max_4_duration) + max_4_duration) + max_4_duration;
+ ASSERT_EQ(max, MonotonicTime::getMax()); // Must not overflow
+
+ MonotonicTime min;
+ min -= max_4_duration;
+ ASSERT_EQ(min, MonotonicTime()); // Must not underflow
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/can/can.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,282 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <queue>
+#include <vector>
+#include <gtest/gtest.h>
+#include <uavcan/transport/can_io.hpp>
+#include <uavcan/transport/frame.hpp>
+#include <uavcan/driver/can.hpp>
+#include <uavcan/driver/system_clock.hpp>
+#include "../../clock.hpp"
+
+
+class CanIfaceMock : public uavcan::ICanIface
+{
+public:
+ struct FrameWithTime
+ {
+ uavcan::CanFrame frame;
+ uavcan::MonotonicTime time;
+ uavcan::UtcTime time_utc;
+ uavcan::CanIOFlags flags;
+
+ FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time)
+ : frame(frame)
+ , time(time)
+ , flags(0)
+ { }
+
+ FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time, uavcan::UtcTime time_utc)
+ : frame(frame)
+ , time(time)
+ , time_utc(time_utc)
+ , flags(0)
+ { }
+
+ FrameWithTime(const uavcan::CanFrame& frame, uint64_t time_usec)
+ : frame(frame)
+ , time(uavcan::MonotonicTime::fromUSec(time_usec))
+ , flags(0)
+ { }
+ };
+
+ std::queue<FrameWithTime> tx; ///< Queue of outgoing frames (bus <-- library)
+ std::queue<FrameWithTime> rx; ///< Queue of incoming frames (bus --> library)
+ std::queue<FrameWithTime> loopback; ///< Loopback
+ bool writeable;
+ bool tx_failure;
+ bool rx_failure;
+ uint64_t num_errors;
+ uavcan::ISystemClock& iclock;
+ bool enable_utc_timestamping;
+ uavcan::CanFrame pending_tx;
+
+ CanIfaceMock(uavcan::ISystemClock& iclock)
+ : writeable(true)
+ , tx_failure(false)
+ , rx_failure(false)
+ , num_errors(0)
+ , iclock(iclock)
+ , enable_utc_timestamping(false)
+ { }
+
+ void pushRx(const uavcan::CanFrame& frame)
+ {
+ rx.push(FrameWithTime(frame, iclock.getMonotonic()));
+ }
+
+ void pushRx(const uavcan::RxFrame& frame)
+ {
+ uavcan::CanFrame can_frame;
+ EXPECT_TRUE(frame.compile(can_frame));
+ rx.push(FrameWithTime(can_frame, frame.getMonotonicTimestamp(), frame.getUtcTimestamp()));
+ }
+
+ bool matchAndPopTx(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline)
+ {
+ if (tx.empty())
+ {
+ std::cout << "Tx buffer is empty" << std::endl;
+ return false;
+ }
+ const FrameWithTime frame_time = tx.front();
+ tx.pop();
+ return (frame_time.frame == frame) && (frame_time.time == tx_deadline);
+ }
+
+ bool matchPendingTx(const uavcan::CanFrame& frame) const
+ {
+ if (pending_tx != frame)
+ {
+ std::cout << "Pending TX mismatch: \n"
+ << " Expected: " << frame.toString(uavcan::CanFrame::StrAligned) << "\n"
+ << " Actual: " << pending_tx.toString(uavcan::CanFrame::StrAligned) << std::endl;
+ }
+ return pending_tx == frame;
+ }
+
+ bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline_usec)
+ {
+ return matchAndPopTx(frame, uavcan::MonotonicTime::fromUSec(tx_deadline_usec));
+ }
+
+ uavcan::CanFrame popTxFrame()
+ {
+ if (tx.empty())
+ {
+ std::cout << "Tx buffer is empty" << std::endl;
+ std::abort();
+ }
+ const FrameWithTime frame_time = tx.front();
+ tx.pop();
+ return frame_time.frame;
+ }
+
+ virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+ uavcan::CanIOFlags flags)
+ {
+ assert(this);
+ EXPECT_TRUE(writeable); // Shall never be called when not writeable
+ if (tx_failure)
+ {
+ return -1;
+ }
+ if (!writeable)
+ {
+ return 0;
+ }
+ tx.push(FrameWithTime(frame, tx_deadline));
+ tx.back().flags = flags;
+ if (flags & uavcan::CanIOFlagLoopback)
+ {
+ loopback.push(FrameWithTime(frame, iclock.getMonotonic()));
+ }
+ return 1;
+ }
+
+ virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
+ {
+ assert(this);
+ if (loopback.empty())
+ {
+ EXPECT_TRUE(rx.size()); // Shall never be called when not readable
+ if (rx_failure)
+ {
+ return -1;
+ }
+ if (rx.empty())
+ {
+ return 0;
+ }
+ const FrameWithTime frame = rx.front();
+ rx.pop();
+ out_frame = frame.frame;
+ out_ts_monotonic = frame.time;
+ out_ts_utc = frame.time_utc;
+ out_flags = frame.flags;
+ }
+ else
+ {
+ out_flags |= uavcan::CanIOFlagLoopback;
+ const FrameWithTime frame = loopback.front();
+ loopback.pop();
+ out_frame = frame.frame;
+ out_ts_monotonic = frame.time;
+ out_ts_utc = frame.time_utc;
+ }
+
+ // Let's just all pretend that this code is autogenerated, instead of being carefully designed by a human.
+ if (out_ts_utc.isZero())
+ {
+ out_ts_utc = enable_utc_timestamping ? iclock.getUtc() : uavcan::UtcTime();
+ }
+ return 1;
+ }
+
+ // cppcheck-suppress unusedFunction
+ // cppcheck-suppress functionConst
+ virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return 0; }
+ // cppcheck-suppress unusedFunction
+ virtual uavcan::uint16_t getNumFilters() const { return 4; } // decrease number of HW_filters from 9 to 4
+ virtual uavcan::uint64_t getErrorCount() const { return num_errors; }
+};
+
+class CanDriverMock : public uavcan::ICanDriver
+{
+public:
+ std::vector<CanIfaceMock> ifaces;
+ uavcan::ISystemClock& iclock;
+ bool select_failure;
+
+ CanDriverMock(unsigned num_ifaces, uavcan::ISystemClock& iclock)
+ : ifaces(num_ifaces, CanIfaceMock(iclock))
+ , iclock(iclock)
+ , select_failure(false)
+ { }
+
+ void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
+ {
+ for (uint8_t i = 0; i < getNumIfaces(); i++)
+ {
+ ifaces.at(i).pushRx(can_frame);
+ }
+ }
+
+ virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime deadline)
+ {
+ assert(this);
+ //std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
+
+ for (unsigned i = 0; i < ifaces.size(); i++)
+ {
+ ifaces.at(i).pending_tx = (pending_tx[i] == UAVCAN_NULLPTR) ? uavcan::CanFrame() : *pending_tx[i];
+ }
+
+ if (select_failure)
+ {
+ return -1;
+ }
+
+ const uavcan::uint8_t valid_iface_mask = uavcan::uint8_t((1 << getNumIfaces()) - 1);
+ EXPECT_FALSE(inout_masks.write & ~valid_iface_mask);
+ EXPECT_FALSE(inout_masks.read & ~valid_iface_mask);
+
+ uavcan::uint8_t out_write_mask = 0;
+ uavcan::uint8_t out_read_mask = 0;
+ for (unsigned i = 0; i < getNumIfaces(); i++)
+ {
+ const uavcan::uint8_t mask = uavcan::uint8_t(1 << i);
+ if ((inout_masks.write & mask) && ifaces.at(i).writeable)
+ {
+ out_write_mask |= mask;
+ }
+ if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size()))
+ {
+ out_read_mask |= mask;
+ }
+ }
+ inout_masks.write = out_write_mask;
+ inout_masks.read = out_read_mask;
+ if ((out_write_mask | out_read_mask) == 0)
+ {
+ const uavcan::MonotonicTime ts = iclock.getMonotonic();
+ const uavcan::MonotonicDuration diff = deadline - ts;
+ SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
+ if (mock)
+ {
+ if (diff.isPositive())
+ {
+ mock->advance(uint64_t(diff.toUSec())); // Emulating timeout
+ }
+ }
+ else
+ {
+ if (diff.isPositive())
+ {
+ usleep(unsigned(diff.toUSec()));
+ }
+ }
+ return 0;
+ }
+ return 1; // This value is not being checked anyway, it just has to be greater than zero
+ }
+
+ virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) { return &ifaces.at(iface_index); }
+ virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const { return &ifaces.at(iface_index); }
+ virtual uavcan::uint8_t getNumIfaces() const { return uavcan::uint8_t(ifaces.size()); }
+};
+
+enum FrameType { STD, EXT };
+inline uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type)
+{
+ id |= (type == EXT) ? uavcan::CanFrame::FlagEFF : 0;
+ return uavcan::CanFrame(id, reinterpret_cast<const uint8_t*>(str_data.c_str()), uavcan::uint8_t(str_data.length()));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/can/can_driver.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "can.hpp"
+#include <uavcan/driver/can.hpp>
+
+TEST(CanFrame, FrameProperties)
+{
+ EXPECT_TRUE(makeCanFrame(0, "", EXT).isExtended());
+ EXPECT_FALSE(makeCanFrame(0, "", STD).isExtended());
+ EXPECT_FALSE(makeCanFrame(0, "", EXT).isRemoteTransmissionRequest());
+ EXPECT_FALSE(makeCanFrame(0, "", STD).isRemoteTransmissionRequest());
+
+ uavcan::CanFrame frame = makeCanFrame(123, "", STD);
+ frame.id |= uavcan::CanFrame::FlagRTR;
+ EXPECT_TRUE(frame.isRemoteTransmissionRequest());
+ EXPECT_FALSE(frame.isErrorFrame());
+ frame.id |= uavcan::CanFrame::FlagERR;
+ EXPECT_TRUE(frame.isErrorFrame());
+}
+
+TEST(CanFrame, Arbitration)
+{
+ using uavcan::CanFrame;
+
+ CanFrame a;
+ CanFrame b;
+
+ /*
+ * Simple
+ */
+ a.id = 123;
+ b.id = 122;
+ ASSERT_TRUE(a.priorityLowerThan(b));
+ ASSERT_TRUE(b.priorityHigherThan(a));
+
+ a.id = 123 | CanFrame::FlagEFF;
+ b.id = 122 | CanFrame::FlagEFF;
+ ASSERT_TRUE(a.priorityLowerThan(b));
+ ASSERT_TRUE(b.priorityHigherThan(a));
+
+ a.id = 8;
+ b.id = 8;
+ ASSERT_FALSE(a.priorityLowerThan(b));
+ ASSERT_FALSE(b.priorityHigherThan(a));
+
+ /*
+ * EXT vs STD
+ */
+ a.id = 1000; // 1000
+ b.id = 2000 | CanFrame::FlagEFF; // 2000 >> 18, wins
+ ASSERT_TRUE(a.priorityLowerThan(b));
+ ASSERT_TRUE(b.priorityHigherThan(a));
+
+ a.id = 0x400;
+ b.id = 0x10000000 | CanFrame::FlagEFF; // (0x400 << 18), 11 most significant bits are the same --> EFF loses
+ ASSERT_TRUE(a.priorityHigherThan(b));
+ ASSERT_TRUE(b.priorityLowerThan(a));
+
+ /*
+ * RTR vs Data
+ */
+ a.id = 123 | CanFrame::FlagRTR; // On the same ID, RTR loses
+ b.id = 123;
+ ASSERT_TRUE(a.priorityLowerThan(b));
+ ASSERT_TRUE(b.priorityHigherThan(a));
+
+ a.id = CanFrame::MaskStdID | CanFrame::FlagRTR; // RTR is STD, so it wins
+ b.id = CanFrame::MaskExtID | CanFrame::FlagEFF; // Lowest possible priority for data frame
+ ASSERT_TRUE(a.priorityHigherThan(b));
+ ASSERT_TRUE(b.priorityLowerThan(a));
+
+ a.id = 123 | CanFrame::FlagRTR; // Both RTR arbitrate as usually
+ b.id = 122 | CanFrame::FlagRTR;
+ ASSERT_TRUE(a.priorityLowerThan(b));
+ ASSERT_TRUE(b.priorityHigherThan(a));
+}
+
+TEST(CanFrame, ToString)
+{
+ uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04" "1234", EXT);
+ EXPECT_EQ("0x0000007b 01 02 03 04 31 32 33 34 '....1234'", frame.toString());
+ EXPECT_TRUE(frame.toString() == frame.toString(uavcan::CanFrame::StrAligned));
+
+ frame = makeCanFrame(123, "z", EXT);
+ EXPECT_EQ("0x0000007b 7a 'z'", frame.toString(uavcan::CanFrame::StrAligned));
+ EXPECT_EQ("0x0000007b 7a 'z'", frame.toString());
+
+ EXPECT_EQ(" 0x141 61 62 63 64 aa bb cc dd 'abcd....'",
+ makeCanFrame(321, "abcd" "\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::StrAligned));
+
+ EXPECT_EQ(" 0x100 ''",
+ makeCanFrame(256, "", STD).toString(uavcan::CanFrame::StrAligned));
+
+ EXPECT_EQ("0x100 ''",
+ makeCanFrame(256, "", STD).toString());
+
+ EXPECT_EQ("0x141 61 62 63 64 aa bb cc dd 'abcd....'",
+ makeCanFrame(321, "abcd" "\xaa\xbb\xcc\xdd", STD).toString());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/can/iface_mock.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "can.hpp"
+
+TEST(CanDriverMock, Basic)
+{
+ using uavcan::CanFrame;
+ using uavcan::CanSelectMasks;
+
+ SystemClockMock clockmock;
+ CanDriverMock driver(3, clockmock);
+
+ const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { };
+
+ ASSERT_EQ(3, driver.getNumIfaces());
+
+ // All WR, no RD
+ CanSelectMasks masks;
+ masks.write = 7;
+ masks.read = 7;
+ EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
+ EXPECT_EQ(7, masks.write);
+ EXPECT_EQ(0, masks.read);
+
+ for (unsigned i = 0; i < 3; i++)
+ {
+ driver.ifaces.at(i).writeable = false;
+ }
+
+ // No WR, no RD
+ masks.write = 7;
+ masks.read = 7;
+ EXPECT_EQ(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
+ EXPECT_EQ(0, masks.write);
+ EXPECT_EQ(0, masks.read);
+ EXPECT_EQ(100, clockmock.monotonic);
+ EXPECT_EQ(100, clockmock.utc);
+
+ // No WR, #1 RD
+ const CanFrame fr1 = makeCanFrame(123, "foo", EXT);
+ driver.ifaces.at(1).pushRx(fr1);
+ masks.write = 7;
+ masks.read = 6;
+ EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
+ EXPECT_EQ(0, masks.write);
+ EXPECT_EQ(2, masks.read);
+ CanFrame fr2;
+ uavcan::MonotonicTime ts_monotonic;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags = 0;
+ EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc, flags));
+ EXPECT_EQ(0, flags);
+ EXPECT_EQ(fr1, fr2);
+ EXPECT_EQ(100, ts_monotonic.toUSec());
+ EXPECT_EQ(0, ts_utc.toUSec());
+
+ // #0 WR, #1 RD, Select failure
+ driver.ifaces.at(0).writeable = true;
+ driver.select_failure = true;
+ masks.write = 1;
+ masks.read = 7;
+ EXPECT_EQ(-1, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
+ EXPECT_EQ(1, masks.write); // Leaving masks unchanged - the library must ignore them
+ EXPECT_EQ(7, masks.read);
+}
+
+TEST(CanDriverMock, Loopback)
+{
+ using uavcan::CanFrame;
+ using uavcan::CanSelectMasks;
+
+ SystemClockMock clockmock;
+ CanDriverMock driver(1, clockmock);
+
+ const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { };
+
+ CanSelectMasks masks;
+ masks.write = 1;
+ masks.read = 1;
+ EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
+ EXPECT_EQ(1, masks.write);
+ EXPECT_EQ(0, masks.read);
+
+ clockmock.advance(200);
+
+ CanFrame fr1;
+ fr1.id = 123 | CanFrame::FlagEFF;
+ EXPECT_EQ(1, driver.getIface(0)->send(fr1, uavcan::MonotonicTime::fromUSec(10000), uavcan::CanIOFlagLoopback));
+
+ masks.write = 0;
+ masks.read = 1;
+ EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
+ EXPECT_EQ(0, masks.write);
+ EXPECT_EQ(1, masks.read);
+
+ CanFrame fr2;
+ uavcan::MonotonicTime ts_monotonic;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags = 0;
+ EXPECT_EQ(1, driver.getIface(0)->receive(fr2, ts_monotonic, ts_utc, flags));
+ EXPECT_EQ(uavcan::CanIOFlagLoopback, flags);
+ EXPECT_EQ(fr1, fr2);
+ EXPECT_EQ(200, ts_monotonic.toUSec());
+ EXPECT_EQ(0, ts_utc.toUSec());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/can/io.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,386 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "can.hpp"
+
+static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame,
+ uint64_t timestamp_usec, int iface_index)
+{
+ if (static_cast<const uavcan::CanFrame&>(rxframe) != frame)
+ {
+ std::cout << "Frame mismatch:\n"
+ << " " << rxframe.toString(uavcan::CanFrame::StrAligned) << "\n"
+ << " " << frame.toString(uavcan::CanFrame::StrAligned) << std::endl;
+ }
+ return (static_cast<const uavcan::CanFrame&>(rxframe) == frame) &&
+ (rxframe.ts_mono == uavcan::MonotonicTime::fromUSec(timestamp_usec)) &&
+ (rxframe.iface_index == iface_index);
+}
+
+TEST(CanIOManager, Reception)
+{
+ // Memory
+ uavcan::PoolAllocator<sizeof(uavcan::CanTxQueue::Entry) * 4, sizeof(uavcan::CanTxQueue::Entry)> pool;
+
+ // Platform interface
+ SystemClockMock clockmock;
+ CanDriverMock driver(2, clockmock);
+
+ // IO Manager
+ uavcan::CanIOManager iomgr(driver, pool, clockmock);
+ ASSERT_EQ(2, iomgr.getNumIfaces());
+
+ /*
+ * Empty, will time out
+ */
+ uavcan::CanRxFrame frame;
+ uavcan::CanIOFlags flags = uavcan::CanIOFlags();
+ EXPECT_EQ(0, iomgr.receive(frame, tsMono(100), flags));
+ EXPECT_EQ(0, flags);
+ EXPECT_EQ(100, clockmock.monotonic);
+ EXPECT_EQ(100, clockmock.utc);
+
+ /*
+ * Non empty from multiple ifaces
+ */
+ const uavcan::CanFrame frames[2][3] = {
+ { makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) },
+ { makeCanFrame(6341, "b0", EXT), makeCanFrame(196, "b1", STD), makeCanFrame(73, "b2", EXT) },
+ };
+
+ clockmock.advance(10);
+ driver.ifaces.at(0).pushRx(frames[0][0]); // Timestamp 110
+ driver.ifaces.at(1).pushRx(frames[1][0]);
+ clockmock.advance(10);
+ driver.ifaces.at(0).pushRx(frames[0][1]); // Timestamp 120
+ driver.ifaces.at(1).pushRx(frames[1][1]);
+ clockmock.advance(10);
+ driver.ifaces.at(0).pushRx(frames[0][2]); // Timestamp 130
+ driver.ifaces.at(1).pushRx(frames[1][2]);
+ clockmock.advance(10);
+
+ EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+ EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0));
+ EXPECT_EQ(0, flags);
+
+ EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+ EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0));
+ EXPECT_EQ(0, flags);
+
+ EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+ EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0));
+ EXPECT_EQ(0, flags);
+
+ EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+ EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1));
+ EXPECT_EQ(0, flags);
+
+ EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+ EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1));
+ EXPECT_EQ(0, flags);
+
+ EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+ EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1));
+ EXPECT_EQ(0, flags);
+
+ EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); // Will time out
+ EXPECT_EQ(0, flags);
+
+ /*
+ * Perf counters
+ */
+ driver.select_failure = true;
+ EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+
+ driver.select_failure = false;
+ driver.ifaces.at(1).pushRx(frames[0][0]);
+ driver.ifaces.at(1).rx_failure = true;
+ EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
+
+ driver.ifaces.at(0).num_errors = 9000;
+ driver.ifaces.at(1).num_errors = 100500;
+ EXPECT_EQ(9000, iomgr.getIfacePerfCounters(0).errors);
+ EXPECT_EQ(100500, iomgr.getIfacePerfCounters(1).errors);
+
+ EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_rx);
+ EXPECT_EQ(3, iomgr.getIfacePerfCounters(1).frames_rx);
+
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_tx);
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx);
+}
+
+TEST(CanIOManager, Transmission)
+{
+ using uavcan::CanIOManager;
+ using uavcan::CanTxQueue;
+
+ // Memory
+ uavcan::PoolAllocator<sizeof(CanTxQueue::Entry) * 4, sizeof(CanTxQueue::Entry)> pool;
+
+ // Platform interface
+ SystemClockMock clockmock;
+ CanDriverMock driver(2, clockmock);
+
+ // IO Manager
+ CanIOManager iomgr(driver, pool, clockmock, 9999);
+ ASSERT_EQ(2, iomgr.getNumIfaces());
+
+ const int ALL_IFACES_MASK = 3;
+
+ const uavcan::CanFrame frames[] = {
+ makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD)
+ };
+
+ uavcan::CanIOFlags flags = uavcan::CanIOFlags();
+
+ /*
+ * Simple transmission
+ */
+ EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
+ EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
+
+ EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent, flags)); // To #1 only
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame()));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
+
+ EXPECT_EQ(0, clockmock.monotonic);
+ EXPECT_EQ(0, clockmock.utc);
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).errors);
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors);
+
+ /*
+ * TX Queue basics
+ */
+ EXPECT_EQ(0, pool.getNumUsedBlocks());
+
+ // Sending to both, #0 blocked
+ driver.ifaces.at(0).writeable = false;
+ EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201));
+ EXPECT_EQ(200, clockmock.monotonic);
+ EXPECT_EQ(200, clockmock.utc);
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+ EXPECT_EQ(1, pool.getNumUsedBlocks()); // One frame went into TX queue, and will expire soon
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // This one will persist
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // This will drop off on the second select()
+
+ // Sending to both, both blocked
+ driver.ifaces.at(1).writeable = false;
+ EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
+ EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Still 0
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); // 1!!
+
+ // Sending to #0, both blocked
+ EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent, flags));
+ EXPECT_EQ(400, clockmock.monotonic);
+ EXPECT_EQ(400, clockmock.utc);
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+ EXPECT_EQ(4, pool.getNumUsedBlocks());
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
+
+ // At this time TX queues are containing the following data:
+ // iface 0: frames[0] (EXPIRED), frames[1], frames[2]
+ // iface 1: frames[1]
+
+ // Sending to #1, both writeable
+ driver.ifaces.at(0).writeable = true;
+ driver.ifaces.at(1).writeable = true;
+ // One frame per each iface will be sent:
+ EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent, flags));
+ EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777)); // Note that frame[0] on iface #0 has expired
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Expired but still will be reported
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
+
+ // Calling receive() to flush the rest two frames
+ uavcan::CanRxFrame dummy_rx_frame;
+ EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0), flags));
+ EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
+ ASSERT_EQ(0, flags);
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
+
+ // Final checks
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+ EXPECT_EQ(0, pool.getNumUsedBlocks()); // Make sure the memory was properly released
+ EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors); // This is because of expired frame[0]
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors);
+
+ /*
+ * TX Queue updates from receive() call
+ */
+ driver.ifaces.at(0).writeable = false;
+ driver.ifaces.at(1).writeable = false;
+
+ // Sending 5 frames, one will be rejected
+ EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
+ EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent, flags));
+ // One frame kicked here:
+ EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
+
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
+
+ // State checks
+ EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full
+ EXPECT_EQ(1200, clockmock.monotonic);
+ EXPECT_EQ(1200, clockmock.utc);
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+
+ // Preparing the driver mock for receive() call
+ driver.ifaces.at(0).writeable = true;
+ driver.ifaces.at(1).writeable = true;
+ const uavcan::CanFrame rx_frames[] = { makeCanFrame(123, "rx0", STD), makeCanFrame(321, "rx1", EXT) };
+ driver.ifaces.at(0).pushRx(rx_frames[0]);
+ driver.ifaces.at(1).pushRx(rx_frames[1]);
+
+ // This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK)
+ uavcan::CanRxFrame rx_frame;
+ EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags)); // Non-blocking
+ EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0));
+ EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
+ ASSERT_EQ(0, flags);
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
+
+ EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags));
+ EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
+ EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE)
+ ASSERT_EQ(0, flags);
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[2]));
+
+ // State checks
+ EXPECT_EQ(0, pool.getNumUsedBlocks()); // TX queue is empty
+ EXPECT_EQ(1200, clockmock.monotonic);
+ EXPECT_EQ(1200, clockmock.utc);
+ EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+ EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+ EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors);
+ EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).errors); // This is because of rejected frame[1]
+
+ /*
+ * Error handling
+ */
+ // Select failure
+ driver.select_failure = true;
+ EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(rx_frame, tsMono(2000), flags));
+ EXPECT_EQ(-uavcan::ErrDriver,
+ iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
+ EXPECT_EQ(1200, clockmock.monotonic);
+ EXPECT_EQ(1200, clockmock.utc);
+ ASSERT_EQ(0, flags);
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
+
+ // Transmission failure
+ driver.select_failure = false;
+ driver.ifaces.at(0).writeable = true;
+ driver.ifaces.at(1).writeable = true;
+ driver.ifaces.at(0).tx_failure = true;
+ driver.ifaces.at(1).tx_failure = true;
+ // Non-blocking - return < 0
+ EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
+
+ ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered
+
+ // Failure removed - transmission shall proceed
+ driver.ifaces.at(0).tx_failure = false;
+ driver.ifaces.at(1).tx_failure = false;
+ EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500), flags));
+ EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200));
+ EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
+ EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted
+ ASSERT_EQ(0, flags);
+ EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame())); // Last call will be receive-only,
+ EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // hence empty TX
+
+ /*
+ * Perf counters
+ */
+ EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).frames_rx);
+ EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).frames_rx);
+
+ EXPECT_EQ(6, iomgr.getIfacePerfCounters(0).frames_tx);
+ EXPECT_EQ(8, iomgr.getIfacePerfCounters(1).frames_tx);
+}
+
+TEST(CanIOManager, Loopback)
+{
+ using uavcan::CanIOManager;
+ using uavcan::CanTxQueue;
+ using uavcan::CanFrame;
+ using uavcan::CanRxFrame;
+
+ // Memory
+ uavcan::PoolAllocator<sizeof(CanTxQueue::Entry) * 4, sizeof(CanTxQueue::Entry)> pool;
+
+ // Platform interface
+ SystemClockMock clockmock;
+ CanDriverMock driver(2, clockmock);
+
+ // IO Manager
+ CanIOManager iomgr(driver, pool, clockmock);
+ ASSERT_EQ(2, iomgr.getNumIfaces());
+
+ CanFrame fr1;
+ fr1.id = 123 | CanFrame::FlagEFF;
+
+ CanFrame fr2;
+ fr2.id = 456 | CanFrame::FlagEFF;
+
+ CanRxFrame rfr1;
+ CanRxFrame rfr2;
+
+ uavcan::CanIOFlags flags = 0;
+ ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback));
+ ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags));
+ ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
+ ASSERT_TRUE(rfr1 == fr1);
+
+ flags = 0;
+ ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback));
+ ASSERT_EQ(1, iomgr.send(fr2, tsMono(1000), tsMono(0), 1, CanTxQueue::Persistent, uavcan::CanIOFlagLoopback));
+ ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags));
+ ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
+ ASSERT_LE(0, iomgr.receive(rfr2, tsMono(100), flags));
+ ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
+ ASSERT_TRUE(rfr1 == fr1);
+ ASSERT_TRUE(rfr2 == fr2);
+
+ /*
+ * Perf counters
+ * Loopback frames are not registered as RX
+ */
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_rx);
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_rx);
+
+ EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_tx);
+ EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx);
+}
+
+TEST(CanIOManager, Size)
+{
+ std::cout << sizeof(uavcan::CanIOManager) << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/can/tx_queue.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/transport/can_io.hpp>
+#include "can.hpp"
+
+
+static int getQueueLength(uavcan::CanTxQueue& queue)
+{
+ const uavcan::CanTxQueue::Entry* p = queue.peek();
+ int length = 0;
+ while (p)
+ {
+ length++;
+ p = p->getNextListNode();
+ }
+ return length;
+}
+
+static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
+{
+ const uavcan::CanTxQueue::Entry* p = queue.peek();
+ while (p)
+ {
+ if (frame == p->frame)
+ {
+ return true;
+ }
+ p = p->getNextListNode();
+ }
+ return false;
+}
+
+TEST(CanTxQueue, Qos)
+{
+ const uavcan::CanIOFlags flags = 0;
+ uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags);
+ uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags);
+
+ EXPECT_FALSE(e1.qosHigherThan(e2));
+ EXPECT_FALSE(e2.qosHigherThan(e1));
+ EXPECT_FALSE(e1.qosLowerThan(e2));
+ EXPECT_FALSE(e2.qosLowerThan(e1));
+
+ e2.qos = uavcan::CanTxQueue::Persistent;
+
+ EXPECT_FALSE(e1.qosHigherThan(e2));
+ EXPECT_TRUE(e2.qosHigherThan(e1));
+ EXPECT_TRUE(e1.qosLowerThan(e2));
+ EXPECT_FALSE(e2.qosLowerThan(e1));
+
+ e1.qos = uavcan::CanTxQueue::Persistent;
+ e1.frame.id -= 1;
+
+ EXPECT_TRUE(e1.qosHigherThan(e2));
+ EXPECT_FALSE(e2.qosHigherThan(e1));
+ EXPECT_FALSE(e1.qosLowerThan(e2));
+ EXPECT_TRUE(e2.qosLowerThan(e1));
+}
+
+TEST(CanTxQueue, TxQueue)
+{
+ using uavcan::CanTxQueue;
+ using uavcan::CanFrame;
+
+ ASSERT_GE(40, sizeof(CanTxQueue::Entry)); // should be true for any platforms, though not required
+
+ uavcan::PoolAllocator<40 * 4, 40> pool;
+
+ SystemClockMock clockmock;
+
+ CanTxQueue queue(pool, clockmock, 99999);
+ EXPECT_TRUE(queue.isEmpty());
+
+ const uavcan::CanIOFlags flags = 0;
+
+ // Descending priority
+ const CanFrame f0 = makeCanFrame(0, "f0", EXT);
+ const CanFrame f1 = makeCanFrame(10, "f1", EXT);
+ const CanFrame f2 = makeCanFrame(20, "f2", EXT);
+ const CanFrame f3 = makeCanFrame(100, "f3", EXT);
+ const CanFrame f4 = makeCanFrame(10000, "f4", EXT);
+ const CanFrame f5 = makeCanFrame(99999, "f5", EXT);
+ const CanFrame f5a = makeCanFrame(99999, "f5a", EXT);
+ const CanFrame f6 = makeCanFrame(999999, "f6", EXT);
+
+ /*
+ * Priority insertion
+ */
+ queue.push(f4, tsMono(100), CanTxQueue::Persistent, flags);
+ EXPECT_FALSE(queue.isEmpty());
+ EXPECT_EQ(1, pool.getNumUsedBlocks());
+ EXPECT_EQ(f4, queue.peek()->frame);
+ EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5));
+ EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal
+ EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3));
+
+ queue.push(f3, tsMono(200), CanTxQueue::Persistent, flags);
+ EXPECT_EQ(f3, queue.peek()->frame);
+
+ queue.push(f0, tsMono(300), CanTxQueue::Volatile, flags);
+ EXPECT_EQ(f0, queue.peek()->frame);
+
+ queue.push(f1, tsMono(400), CanTxQueue::Volatile, flags);
+ EXPECT_EQ(f0, queue.peek()->frame); // Still f0, since it is highest
+ EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal
+ EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1));
+
+ // Out of free memory now
+
+ EXPECT_EQ(0, queue.getRejectedFrameCount());
+ EXPECT_EQ(4, getQueueLength(queue));
+ EXPECT_TRUE(isInQueue(queue, f0));
+ EXPECT_TRUE(isInQueue(queue, f1));
+ EXPECT_TRUE(isInQueue(queue, f3));
+ EXPECT_TRUE(isInQueue(queue, f4));
+
+ const CanTxQueue::Entry* p = queue.peek();
+ while (p)
+ {
+ std::cout << p->toString() << std::endl;
+ p = p->getNextListNode();
+ }
+
+ /*
+ * QoS
+ */
+ EXPECT_FALSE(isInQueue(queue, f2));
+ queue.push(f2, tsMono(100), CanTxQueue::Volatile, flags); // Non preempting, will be rejected
+ EXPECT_FALSE(isInQueue(queue, f2));
+
+ queue.push(f2, tsMono(500), CanTxQueue::Persistent, flags); // Will override f1 (f3 and f4 are presistent)
+ EXPECT_TRUE(isInQueue(queue, f2));
+ EXPECT_FALSE(isInQueue(queue, f1));
+ EXPECT_EQ(4, getQueueLength(queue));
+ EXPECT_EQ(2, queue.getRejectedFrameCount());
+ EXPECT_EQ(f0, queue.peek()->frame); // Check the priority
+
+ queue.push(f5, tsMono(600), CanTxQueue::Persistent, flags); // Will override f0 (rest are presistent)
+ EXPECT_TRUE(isInQueue(queue, f5));
+ EXPECT_FALSE(isInQueue(queue, f0));
+ EXPECT_EQ(f2, queue.peek()->frame); // Check the priority
+
+ // No volatile frames left now
+
+ queue.push(f5a, tsMono(700), CanTxQueue::Persistent, flags); // Will override f5 (same frame, same QoS)
+ EXPECT_TRUE(isInQueue(queue, f5a));
+ EXPECT_FALSE(isInQueue(queue, f5));
+
+ queue.push(f6, tsMono(700), CanTxQueue::Persistent, flags); // Will be rejected (lowest QoS)
+ EXPECT_FALSE(isInQueue(queue, f6));
+
+ EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
+ EXPECT_TRUE(queue.topPriorityHigherOrEqual(f2)); // Equal
+ EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5a));
+ EXPECT_EQ(4, getQueueLength(queue));
+ EXPECT_EQ(4, pool.getNumUsedBlocks());
+ EXPECT_EQ(5, queue.getRejectedFrameCount());
+ EXPECT_TRUE(isInQueue(queue, f2));
+ EXPECT_TRUE(isInQueue(queue, f3));
+ EXPECT_TRUE(isInQueue(queue, f4));
+ EXPECT_TRUE(isInQueue(queue, f5a));
+ EXPECT_EQ(f2, queue.peek()->frame); // Check the priority
+
+ /*
+ * Expiration
+ */
+ clockmock.monotonic = 101;
+ queue.push(f0, tsMono(800), CanTxQueue::Volatile, flags); // Will replace f4 which is expired now
+ EXPECT_TRUE(isInQueue(queue, f0));
+ EXPECT_FALSE(isInQueue(queue, f4));
+ EXPECT_EQ(6, queue.getRejectedFrameCount());
+
+ clockmock.monotonic = 1001;
+ queue.push(f5, tsMono(2000), CanTxQueue::Volatile, flags); // Entire queue is expired
+ EXPECT_TRUE(isInQueue(queue, f5));
+ EXPECT_EQ(1, getQueueLength(queue)); // Just one entry left - f5
+ EXPECT_EQ(1, pool.getNumUsedBlocks()); // Make sure there is no leaks
+ EXPECT_EQ(10, queue.getRejectedFrameCount());
+
+ queue.push(f0, tsMono(1000), CanTxQueue::Persistent, flags); // This entry is already expired
+ EXPECT_EQ(1, getQueueLength(queue));
+ EXPECT_EQ(1, pool.getNumUsedBlocks());
+ EXPECT_EQ(11, queue.getRejectedFrameCount());
+
+ /*
+ * Removing
+ */
+ queue.push(f4, tsMono(5000), CanTxQueue::Volatile, flags);
+ EXPECT_EQ(2, getQueueLength(queue));
+ EXPECT_TRUE(isInQueue(queue, f4));
+ EXPECT_EQ(f4, queue.peek()->frame);
+
+ CanTxQueue::Entry* entry = queue.peek();
+ EXPECT_TRUE(entry);
+ queue.remove(entry);
+ EXPECT_FALSE(entry);
+
+ EXPECT_FALSE(isInQueue(queue, f4));
+ EXPECT_TRUE(isInQueue(queue, f5));
+
+ entry = queue.peek();
+ EXPECT_TRUE(entry);
+ queue.remove(entry);
+ EXPECT_FALSE(entry);
+
+ EXPECT_FALSE(isInQueue(queue, f5));
+
+ EXPECT_EQ(0, getQueueLength(queue)); // Final state checks
+ EXPECT_EQ(0, pool.getNumUsedBlocks());
+ EXPECT_EQ(11, queue.getRejectedFrameCount());
+ EXPECT_FALSE(queue.peek());
+ EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,219 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ * Ilia Sheremet <illia.sheremet@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <cassert>
+
+#include <uavcan/transport/can_acceptance_filter_configurator.hpp>
+#include "../node/test_node.hpp"
+#include "uavcan/node/subscriber.hpp"
+#include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
+#include <uavcan/equipment/air_data/Sideslip.hpp>
+#include <uavcan/equipment/air_data/TrueAirspeed.hpp>
+#include <uavcan/equipment/air_data/AngleOfAttack.hpp>
+#include <uavcan/equipment/ahrs/Solution.hpp>
+#include <uavcan/equipment/air_data/StaticPressure.hpp>
+#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <iostream>
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+
+template <typename DataType>
+struct SubscriptionListener
+{
+ typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructure;
+
+ struct ReceivedDataStructureCopy
+ {
+ uavcan::MonotonicTime ts_monotonic;
+ uavcan::UtcTime ts_utc;
+ uavcan::TransferType transfer_type;
+ uavcan::TransferID transfer_id;
+ uavcan::NodeID src_node_id;
+ uavcan::uint8_t iface_index;
+ DataType msg;
+
+ ReceivedDataStructureCopy(const ReceivedDataStructure& s) :
+ ts_monotonic(s.getMonotonicTimestamp()),
+ ts_utc(s.getUtcTimestamp()),
+ transfer_type(s.getTransferType()),
+ transfer_id(s.getTransferID()),
+ src_node_id(s.getSrcNodeID()),
+ iface_index(s.getIfaceIndex()),
+ msg(s)
+ { }
+ };
+
+ std::vector<DataType> simple;
+ std::vector<ReceivedDataStructureCopy> extended;
+
+ void receiveExtended(ReceivedDataStructure& msg)
+ {
+ extended.push_back(msg);
+ }
+
+ void receiveSimple(DataType& msg)
+ {
+ simple.push_back(msg);
+ }
+
+ typedef SubscriptionListener<DataType> SelfType;
+ typedef uavcan::MethodBinder<SelfType*, void(SelfType::*) (ReceivedDataStructure&)> ExtendedBinder;
+ typedef uavcan::MethodBinder<SelfType*, void(SelfType::*) (DataType&)> SimpleBinder;
+
+ ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); }
+ SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); }
+};
+
+static void writeServiceServerCallback(
+ const uavcan::ReceivedDataStructure<uavcan::protocol::file::BeginFirmwareUpdate::Request>& req,
+ uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp)
+{
+ std::cout << req << std::endl;
+ rsp.error = rsp.ERROR_UNKNOWN;
+}
+
+TEST(CanAcceptanceFilter, Basic_test)
+{
+ uavcan::GlobalDataTypeRegistry::instance().reset();
+ uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1;
+ uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2;
+ uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3;
+ uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4;
+ uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::Solution> _reg5;
+ uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6;
+ uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7;
+
+ SystemClockDriver clock_driver;
+ CanDriverMock can_driver(1, clock_driver);
+ TestNode node(can_driver, clock_driver, 24);
+
+ uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand,
+ SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node);
+ uavcan::Subscriber<uavcan::equipment::air_data::Sideslip,
+ SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node);
+ uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed,
+ SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node);
+ uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack,
+ SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node);
+ uavcan::Subscriber<uavcan::equipment::ahrs::Solution,
+ SubscriptionListener<uavcan::equipment::ahrs::Solution>::ExtendedBinder> sub_5(node);
+ uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
+ SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node);
+ uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
+ SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node);
+ uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node);
+
+ SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1;
+ SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2;
+ SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3;
+ SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4;
+ SubscriptionListener<uavcan::equipment::ahrs::Solution> listener_5;
+ SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6;
+
+ sub_1.start(listener_1.bindExtended());
+ sub_2.start(listener_2.bindExtended());
+ sub_3.start(listener_3.bindExtended());
+ sub_4.start(listener_4.bindExtended());
+ sub_5.start(listener_5.bindExtended());
+ sub_6.start(listener_6.bindExtended());
+ sub_6_1.start(listener_6.bindExtended());
+ server.start(writeServiceServerCallback);
+ std::cout << "Subscribers are initialized." << std::endl;
+
+ uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10);
+
+ int configure_filters_assert = anon_test_configuration.computeConfiguration();
+ ASSERT_EQ(configure_filters_assert, 0);
+ std::cout << "Filters are calculated with anonymous configuration." << std::endl;
+
+ const auto& configure_array = anon_test_configuration.getConfiguration();
+ uint32_t configure_array_size = configure_array.getSize();
+ std::cout << "Number of configurations after first time computeConfiguration() invoked: "
+ << configure_array_size << std::endl;
+ ASSERT_EQ(configure_array_size, 9);
+
+ std::cout << "Adding two additional configurations ... " << std::endl;
+ uavcan::CanFilterConfig aux_config_1, aux_config_2;
+ aux_config_1.id = 911;
+ aux_config_1.mask = 1488;
+ aux_config_2.id = 999999;
+ aux_config_2.mask = 849128412;
+ configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1);
+ ASSERT_EQ(configure_filters_assert, 0);
+ configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2);
+ ASSERT_EQ(configure_filters_assert, 0);
+ configure_array_size = configure_array.getSize();
+ std::cout << "New configuration anon_container size: " << configure_array_size << std::endl;
+ ASSERT_EQ(configure_array_size, 11);
+
+ std::cout << "Applying configuration ... " << std::endl;
+ configure_filters_assert = anon_test_configuration.applyConfiguration();
+ ASSERT_EQ(configure_filters_assert, 0);
+ std::cout << "Filters are configured." << std::endl;
+ configure_array_size = configure_array.getSize();
+ std::cout << "Final configuration anon_container size: " << configure_array_size << std::endl;
+ ASSERT_EQ(configure_array_size, 10);
+
+ for (uint16_t i = 0; i<configure_array_size; i++)
+ {
+ std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl;
+ std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
+ }
+
+ ASSERT_EQ(configure_array.getByIndex(0)->id, 911);
+ ASSERT_EQ(configure_array.getByIndex(0)->mask, 1488);
+ ASSERT_EQ(configure_array.getByIndex(1)->id, 999999);
+ ASSERT_EQ(configure_array.getByIndex(1)->mask, 849128412);
+ ASSERT_EQ(configure_array.getByIndex(2)->id, 2147746048);
+ ASSERT_EQ(configure_array.getByIndex(2)->mask, 3774873472);
+ ASSERT_EQ(configure_array.getByIndex(3)->id, 2147744768);
+ ASSERT_EQ(configure_array.getByIndex(3)->mask, 3774873472);
+ ASSERT_EQ(configure_array.getByIndex(4)->id, 2147739648);
+ ASSERT_EQ(configure_array.getByIndex(4)->mask, 3774873472);
+ ASSERT_EQ(configure_array.getByIndex(5)->id, 2147746816);
+ ASSERT_EQ(configure_array.getByIndex(5)->mask, 3774873472);
+ ASSERT_EQ(configure_array.getByIndex(6)->id, 2147746304);
+ ASSERT_EQ(configure_array.getByIndex(6)->mask, 3774873472);
+ ASSERT_EQ(configure_array.getByIndex(7)->id, 2147483648);
+ ASSERT_EQ(configure_array.getByIndex(7)->mask, 3758096639);
+ ASSERT_EQ(configure_array.getByIndex(8)->id, 2147489920);
+ ASSERT_EQ(configure_array.getByIndex(8)->mask, 3758129024);
+ ASSERT_EQ(configure_array.getByIndex(9)->id, 2147749888);
+ ASSERT_EQ(configure_array.getByIndex(9)->mask, 3774873472);
+
+ uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node, 4);
+ configure_filters_assert = no_anon_test_confiruration.computeConfiguration
+ (uavcan::CanAcceptanceFilterConfigurator::IgnoreAnonymousMessages);
+ ASSERT_EQ(configure_filters_assert, 0);
+ std::cout << "Filters are configured without anonymous configuration." << std::endl;
+
+ configure_filters_assert = no_anon_test_confiruration.applyConfiguration();
+ ASSERT_EQ(configure_filters_assert, 0);
+ std::cout << "Filters are configured without anonymous configuration." << std::endl;
+
+ const auto& configure_array_2 = no_anon_test_confiruration.getConfiguration();
+ configure_array_size = configure_array_2.getSize();
+ ASSERT_EQ(configure_filters_assert, 0);
+ ASSERT_EQ(configure_array_size, 4);
+
+ for (uint16_t i = 0; i<configure_array_size; i++)
+ {
+ std::cout << "config.ID [" << i << "] = " << configure_array_2.getByIndex(i)->id << std::endl;
+ std::cout << "config.MK [" << i << "] = " << configure_array_2.getByIndex(i)->mask << std::endl;
+ }
+
+ ASSERT_EQ(configure_array_2.getByIndex(0)->id, 2147739648);
+ ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 3774868352);
+ ASSERT_EQ(configure_array_2.getByIndex(1)->id, 2147745792);
+ ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 3774872704);
+ ASSERT_EQ(configure_array_2.getByIndex(2)->id, 2147489920);
+ ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 3758129024);
+ ASSERT_EQ(configure_array_2.getByIndex(3)->id, 2147745792);
+ ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 3774868352);
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/crc.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/transport/crc.hpp>
+
+/*
+ import crcmod
+ crc = crcmod.predefined.Crc('crc-ccitt-false')
+ crc.update('123')
+ crc.hexdigest()
+ '5BCE'
+ crc.update('456789')
+ crc.hexdigest()
+ '29B1'
+ */
+
+TEST(TransferCRC, Correctness)
+{
+ uavcan::TransferCRC crc;
+
+ ASSERT_EQ(0xFFFF, crc.get());
+
+ crc.add('1');
+ crc.add('2');
+ crc.add('3');
+ ASSERT_EQ(0x5BCE, crc.get());
+
+ crc.add(reinterpret_cast<const uint8_t*>("456789"), 6);
+ ASSERT_EQ(0x29B1, crc.get());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/dispatcher.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,392 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <memory>
+#include <vector>
+#include <gtest/gtest.h>
+#include "transfer_test_helpers.hpp"
+#include "can/can.hpp"
+#include <uavcan/transport/dispatcher.hpp>
+
+
+class DispatcherTransferEmulator : public IncomingTransferEmulatorBase
+{
+ CanDriverMock& target_;
+
+public:
+ DispatcherTransferEmulator(CanDriverMock& target, uavcan::NodeID dst_node_id = 127)
+ : IncomingTransferEmulatorBase(dst_node_id)
+ , target_(target)
+ { }
+
+ void sendOneFrame(const uavcan::RxFrame& frame)
+ {
+ CanIfaceMock* const iface = static_cast<CanIfaceMock*>(target_.getIface(frame.getIfaceIndex()));
+ EXPECT_TRUE(iface);
+ if (iface)
+ {
+ iface->pushRx(frame);
+ }
+ }
+};
+
+
+struct RxFrameListener : public uavcan::IRxFrameListener
+{
+ std::vector<uavcan::CanRxFrame> rx_frames;
+
+ virtual void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags)
+ {
+ std::cout << "RX frame [flags=" << flags << "]: " << frame.toString() << std::endl;
+ if ((flags & uavcan::CanIOFlagLoopback) == 0)
+ {
+ rx_frames.push_back(frame);
+ }
+ }
+};
+
+
+static const uavcan::NodeID SELF_NODE_ID(64);
+
+
+TEST(Dispatcher, Reception)
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> pool;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ uavcan::Dispatcher dispatcher(driver, pool, clockmock);
+ ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once
+ ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID));
+ ASSERT_EQ(SELF_NODE_ID, dispatcher.getNodeID());
+
+ DispatcherTransferEmulator emulator(driver, SELF_NODE_ID);
+
+ /*
+ * RX listener
+ */
+ RxFrameListener rx_listener;
+ ASSERT_FALSE(dispatcher.getRxFrameListener());
+ dispatcher.installRxFrameListener(&rx_listener);
+ ASSERT_TRUE(dispatcher.getRxFrameListener());
+ ASSERT_TRUE(rx_listener.rx_frames.empty());
+
+ /*
+ * Test environment
+ */
+ static const uavcan::DataTypeDescriptor TYPES[4] =
+ {
+ makeDataType(uavcan::DataTypeKindMessage, 1),
+ makeDataType(uavcan::DataTypeKindMessage, 2),
+ makeDataType(uavcan::DataTypeKindService, 1),
+ makeDataType(uavcan::DataTypeKindService, 1)
+ };
+
+ typedef std::auto_ptr<TestListener> TestListenerPtr;
+ static const int MaxBufSize = 512;
+ static const int NumSubscribers = 6;
+ TestListenerPtr subscribers[NumSubscribers] =
+ {
+ TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg
+ TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg // Two similar
+ TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[1], MaxBufSize, pool)), // msg
+ TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[2], MaxBufSize, pool)), // srv
+ TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)), // srv
+ TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)) // srv // Repeat again
+ };
+
+ static const std::string DATA[6] =
+ {
+ "Yes, man is mortal, but that would be only half the trouble. "
+ "The worst of it is that he's sometimes unexpectedly mortal - there's the trick!",
+
+ "In fact, I'm beginning to fear that this confusion will go on for a long time. "
+ "And all because he writes down what I said incorrectly.",
+
+ "I had the pleasure of meeting that young man at the Patriarch's Ponds. "
+ "He almost drove me mad myself, proving to me that I don't exist. But you do believe that it is really I?",
+
+ "He was a dreamer, a thinker, a speculative philosopher... or, as his wife would have it, an idiot.",
+
+ "The only way to get ideas for stories is to drink way too much coffee and buy a desk that doesn't "
+ "collapse when you beat your head against it",
+
+ ""
+ };
+
+ for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++)
+ {
+ std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl;
+ }
+
+ const Transfer transfers[8] =
+ {
+ emulator.makeTransfer(0, uavcan::TransferTypeMessageBroadcast, 10, DATA[0], TYPES[0]),
+ emulator.makeTransfer(5, uavcan::TransferTypeMessageBroadcast, 11, DATA[1], TYPES[1]),
+ emulator.makeTransfer(10, uavcan::TransferTypeServiceRequest, 12, DATA[2], TYPES[2]),
+ emulator.makeTransfer(15, uavcan::TransferTypeServiceResponse, 13, DATA[4], TYPES[3]),
+ emulator.makeTransfer(20, uavcan::TransferTypeMessageBroadcast, 14, DATA[3], TYPES[0]),
+ emulator.makeTransfer(25, uavcan::TransferTypeMessageBroadcast, 15, DATA[5], TYPES[1]),
+ // Wrongly addressed:
+ emulator.makeTransfer(29, uavcan::TransferTypeServiceResponse, 10, DATA[0], TYPES[3], 100),
+ emulator.makeTransfer(31, uavcan::TransferTypeServiceRequest, 11, DATA[4], TYPES[2], 101)
+ };
+
+ /*
+ * Registration
+ */
+ for (int i = 0; i < NumSubscribers; i++)
+ {
+ ASSERT_FALSE(dispatcher.hasSubscriber(subscribers[i]->getDataTypeDescriptor().getID()));
+ ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID()));
+ ASSERT_FALSE(dispatcher.hasServer(subscribers[i]->getDataTypeDescriptor().getID()));
+ }
+
+ ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[0].get()));
+ ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[1].get()));
+ ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[2].get()));
+ ASSERT_TRUE(dispatcher.registerServiceRequestListener(subscribers[3].get()));
+ ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[4].get()));
+ ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[5].get()));
+
+ for (int i = 0; i < NumSubscribers; i++)
+ {
+ ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID()));
+ }
+
+ // Subscribers
+ ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[0]->getDataTypeDescriptor().getID()));
+ ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[1]->getDataTypeDescriptor().getID()));
+ ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[2]->getDataTypeDescriptor().getID()));
+
+ // Servers
+ ASSERT_TRUE(dispatcher.hasServer(subscribers[3]->getDataTypeDescriptor().getID()));
+
+ /*
+ * Sending the transfers
+ */
+ // Multiple service request listeners are not allowed
+ ASSERT_FALSE(dispatcher.registerServiceRequestListener(subscribers[3].get()));
+
+ // Item count validation
+ ASSERT_EQ(3, dispatcher.getNumMessageListeners());
+ ASSERT_EQ(1, dispatcher.getNumServiceRequestListeners());
+ ASSERT_EQ(2, dispatcher.getNumServiceResponseListeners());
+
+ for (int i = 0; i < NumSubscribers; i++)
+ {
+ ASSERT_TRUE(subscribers[i]->isEmpty());
+ }
+
+ emulator.send(transfers);
+ emulator.send(transfers); // Just for fun, they will be ignored anyway.
+
+ while (true)
+ {
+ const int res = dispatcher.spinOnce();
+ ASSERT_LE(0, res);
+ clockmock.advance(100);
+ if (res == 0)
+ {
+ break;
+ }
+ }
+
+ /*
+ * Matching.
+ */
+ ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[4]));
+ ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[0]));
+
+ ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[4]));
+ ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[0]));
+
+ ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[5]));
+ ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[1]));
+
+ ASSERT_TRUE(subscribers[3]->matchAndPop(transfers[2]));
+
+ ASSERT_TRUE(subscribers[4]->matchAndPop(transfers[3]));
+
+ ASSERT_TRUE(subscribers[5]->matchAndPop(transfers[3]));
+
+ for (int i = 0; i < NumSubscribers; i++)
+ {
+ ASSERT_TRUE(subscribers[i]->isEmpty());
+ }
+
+ /*
+ * Unregistering all transfers
+ */
+ dispatcher.unregisterMessageListener(subscribers[0].get());
+ dispatcher.unregisterMessageListener(subscribers[1].get());
+ dispatcher.unregisterMessageListener(subscribers[2].get());
+ dispatcher.unregisterServiceRequestListener(subscribers[3].get());
+ dispatcher.unregisterServiceResponseListener(subscribers[4].get());
+ dispatcher.unregisterServiceResponseListener(subscribers[5].get());
+
+ ASSERT_EQ(0, dispatcher.getNumMessageListeners());
+ ASSERT_EQ(0, dispatcher.getNumServiceRequestListeners());
+ ASSERT_EQ(0, dispatcher.getNumServiceResponseListeners());
+
+ /*
+ * Perf counters
+ */
+ EXPECT_LT(0, dispatcher.getTransferPerfCounter().getErrorCount()); // Repeated transfers
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount());
+ EXPECT_EQ(9, dispatcher.getTransferPerfCounter().getRxTransferCount());
+
+ /*
+ * RX listener
+ */
+ std::cout << "Num received frames: " << rx_listener.rx_frames.size() << std::endl;
+ ASSERT_EQ(292, rx_listener.rx_frames.size());
+}
+
+
+TEST(Dispatcher, Transmission)
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ uavcan::Dispatcher dispatcher(driver, pool, clockmock);
+ ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once
+ ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID));
+
+ /*
+ * RX listener
+ */
+ RxFrameListener rx_listener;
+ dispatcher.installRxFrameListener(&rx_listener);
+
+ /*
+ * Transmission
+ */
+ static const uavcan::MonotonicTime TX_DEADLINE = tsMono(123456);
+
+ // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
+ uavcan::Frame frame(123, uavcan::TransferTypeServiceRequest, SELF_NODE_ID, 2, 0);
+ frame.setPayload(reinterpret_cast<const uint8_t*>("123"), 3);
+
+ ASSERT_FALSE(dispatcher.hasPublisher(123));
+ ASSERT_FALSE(dispatcher.hasPublisher(456));
+ const uavcan::OutgoingTransferRegistryKey otr_key(123, uavcan::TransferTypeMessageBroadcast, 0);
+ ASSERT_TRUE(dispatcher.getOutgoingTransferRegistry().accessOrCreate(otr_key,
+ uavcan::MonotonicTime::fromMSec(1000000)));
+ ASSERT_TRUE(dispatcher.hasPublisher(123));
+ ASSERT_FALSE(dispatcher.hasPublisher(456));
+
+ ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0, 0xFF));
+
+ /*
+ * Validation
+ */
+ uavcan::CanFrame expected_can_frame;
+ ASSERT_TRUE(frame.compile(expected_can_frame));
+
+ ASSERT_TRUE(driver.ifaces.at(0).matchAndPopTx(expected_can_frame, TX_DEADLINE));
+ ASSERT_TRUE(driver.ifaces.at(1).matchAndPopTx(expected_can_frame, TX_DEADLINE));
+
+ ASSERT_TRUE(driver.ifaces.at(0).tx.empty());
+ ASSERT_TRUE(driver.ifaces.at(1).tx.empty());
+
+ /*
+ * Perf counters - all empty because dispatcher itself does not count TX transfers
+ */
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount());
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount());
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
+
+ /*
+ * RX listener
+ */
+ ASSERT_TRUE(rx_listener.rx_frames.empty());
+}
+
+
+TEST(Dispatcher, Spin)
+{
+ NullAllocator poolmgr;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock);
+ ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once
+ ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID));
+
+ clockmock.monotonic_auto_advance = 100;
+
+ ASSERT_EQ(100, clockmock.monotonic);
+ ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
+ ASSERT_LE(1000, clockmock.monotonic);
+ ASSERT_EQ(0, dispatcher.spinOnce());
+ ASSERT_LE(1000, clockmock.monotonic);
+ ASSERT_EQ(0, dispatcher.spin(tsMono(1100)));
+ ASSERT_LE(1100, clockmock.monotonic);
+}
+
+
+struct DispatcherTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase
+{
+ uavcan::RxFrame last_frame;
+ unsigned count;
+
+ DispatcherTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher)
+ : uavcan::LoopbackFrameListenerBase(dispatcher)
+ , count(0)
+ { }
+
+ using uavcan::LoopbackFrameListenerBase::startListening;
+ using uavcan::LoopbackFrameListenerBase::isListening;
+
+ void handleLoopbackFrame(const uavcan::RxFrame& frame)
+ {
+ std::cout << "DispatcherTestLoopbackFrameListener: " << frame.toString() << std::endl;
+ last_frame = frame;
+ count++;
+ }
+};
+
+TEST(Dispatcher, Loopback)
+{
+ NullAllocator poolmgr;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock);
+ ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID));
+
+ {
+ DispatcherTestLoopbackFrameListener listener(dispatcher);
+ ASSERT_FALSE(listener.isListening());
+ listener.startListening();
+ ASSERT_TRUE(listener.isListening());
+
+ // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+ // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false
+ uavcan::Frame frame(123, uavcan::TransferTypeServiceResponse, SELF_NODE_ID, 2, 0);
+ frame.setPayload(reinterpret_cast<const uint8_t*>("123"), 3);
+
+ ASSERT_TRUE(listener.last_frame == uavcan::RxFrame());
+
+ ASSERT_LE(0, dispatcher.send(frame, tsMono(1000), tsMono(0), uavcan::CanTxQueue::Persistent,
+ uavcan::CanIOFlagLoopback, 0xFF));
+
+ ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
+
+ ASSERT_TRUE(listener.last_frame != uavcan::RxFrame());
+ ASSERT_TRUE(listener.last_frame == frame);
+ ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); // Last iface
+ ASSERT_EQ(2, listener.count);
+
+ ASSERT_EQ(1, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners());
+ }
+ ASSERT_EQ(0, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/frame.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,389 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <string>
+#include <gtest/gtest.h>
+#include <uavcan/transport/transfer.hpp>
+#include <uavcan/transport/crc.hpp>
+#include "../clock.hpp"
+#include "can/can.hpp"
+
+
+TEST(Frame, MessageParseCompile)
+{
+ using uavcan::Frame;
+ using uavcan::CanFrame;
+ using uavcan::TransferID;
+ using uavcan::TransferType;
+
+ Frame frame;
+
+ /*
+ * Priority
+ * Message Type ID
+ * Service Not Message
+ * Source Node ID
+ */
+ const uint32_t can_id =
+ (16 << 24) | // Priority
+ (20000 << 8) | // Message Type ID
+ (0 << 7) | // Service Not Message
+ (42 << 0); // Source Node ID
+
+ const std::string payload_string = "hello\xD4"; // SET = 110, TID = 20
+
+ /*
+ * Parse
+ */
+ // Invalid CAN frames
+ ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, reinterpret_cast<const uint8_t*>(""), 0)));
+ ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD)));
+
+ // Valid
+ ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
+
+ EXPECT_EQ(TransferID(20), frame.getTransferID());
+ EXPECT_TRUE(frame.isStartOfTransfer());
+ EXPECT_TRUE(frame.isEndOfTransfer());
+ EXPECT_FALSE(frame.getToggle());
+ EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID());
+ EXPECT_TRUE(frame.getDstNodeID().isBroadcast());
+ EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
+ EXPECT_EQ(20000, frame.getDataTypeID().get());
+ EXPECT_EQ(16, frame.getPriority().get());
+
+ EXPECT_EQ(payload_string.length() - 1, frame.getPayloadLen());
+ EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(),
+ payload_string.begin()));
+
+ std::cout << frame.toString() << std::endl;
+
+ /*
+ * Compile
+ */
+ CanFrame can_frame;
+ ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
+
+ ASSERT_TRUE(frame.compile(can_frame));
+ ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT));
+
+ EXPECT_EQ(payload_string.length(), can_frame.dlc);
+ std::cout << can_frame.toString() << std::endl;
+ /*
+ * FUN FACT: comparison of uint8_t with char may fail on the character 0xD4 (depending on the locale),
+ * because it will be considered a Unicode character. Hence, we do reinterpret_cast<>.
+ */
+ EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc,
+ reinterpret_cast<const uint8_t*>(&payload_string[0])));
+
+ /*
+ * Comparison
+ */
+ ASSERT_FALSE(Frame() == frame);
+ ASSERT_TRUE(Frame() != frame);
+ frame = Frame();
+ ASSERT_TRUE(Frame() == frame);
+ ASSERT_FALSE(Frame() != frame);
+}
+
+
+TEST(Frame, ServiceParseCompile)
+{
+ using uavcan::Frame;
+ using uavcan::CanFrame;
+ using uavcan::TransferID;
+ using uavcan::TransferType;
+
+ Frame frame;
+
+ /*
+ * Priority
+ * Service Type ID
+ * Request Not Response
+ * Destination Node ID
+ * Service Not Message
+ * Source Node ID
+ */
+ const uint32_t can_id =
+ (31 << 24) | // Priority
+ (200 << 16) | // Service Type ID
+ (1 << 15) | // Request Not Response
+ (0x42 << 8) | // Destination Node ID
+ (1 << 7) | // Service Not Message
+ (42 << 0); // Source Node ID
+
+ const std::string payload_string = "hello\x6a"; // SET = 011, TID = 10
+
+ /*
+ * Parse
+ */
+ // Invalid CAN frames
+ ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, reinterpret_cast<const uint8_t*>(""), 0)));
+ ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD)));
+
+ // Valid
+ ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
+
+ EXPECT_EQ(TransferID(10), frame.getTransferID());
+ EXPECT_FALSE(frame.isStartOfTransfer());
+ EXPECT_TRUE(frame.isEndOfTransfer());
+ EXPECT_TRUE(frame.getToggle());
+ EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID());
+ EXPECT_EQ(uavcan::NodeID(0x42), frame.getDstNodeID());
+ EXPECT_EQ(uavcan::TransferTypeServiceRequest, frame.getTransferType());
+ EXPECT_EQ(200, frame.getDataTypeID().get());
+ EXPECT_EQ(31, frame.getPriority().get());
+
+ EXPECT_EQ(payload_string.length(), frame.getPayloadLen() + 1);
+ EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(),
+ reinterpret_cast<const uint8_t*>(&payload_string[0])));
+
+ std::cout << frame.toString() << std::endl;
+
+ /*
+ * Compile
+ */
+ CanFrame can_frame;
+ ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
+
+ ASSERT_TRUE(frame.compile(can_frame));
+ ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT));
+
+ EXPECT_EQ(payload_string.length(), can_frame.dlc);
+ EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc,
+ reinterpret_cast<const uint8_t*>(&payload_string[0])));
+
+ /*
+ * Comparison
+ */
+ ASSERT_FALSE(Frame() == frame);
+ ASSERT_TRUE(Frame() != frame);
+ frame = Frame();
+ ASSERT_TRUE(Frame() == frame);
+ ASSERT_FALSE(Frame() != frame);
+}
+
+
+TEST(Frame, AnonymousParseCompile)
+{
+ using uavcan::Frame;
+ using uavcan::CanFrame;
+ using uavcan::TransferID;
+ using uavcan::TransferType;
+
+ Frame frame;
+
+ /*
+ * Priority
+ * Discriminator
+ * Message Type ID
+ * Service Not Message
+ * Source Node ID
+ */
+ const uint32_t can_id =
+ (16383 << 10) | // Discriminator
+ (1 << 8); // Message Type ID
+
+ const std::string payload_string = "hello\xd4"; // SET = 110, TID = 20
+
+ uavcan::TransferCRC payload_crc;
+ payload_crc.add(reinterpret_cast<const uint8_t*>(payload_string.c_str()), unsigned(payload_string.length()));
+
+ /*
+ * Parse
+ */
+ ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
+
+ EXPECT_EQ(TransferID(20), frame.getTransferID());
+ EXPECT_TRUE(frame.isStartOfTransfer());
+ EXPECT_TRUE(frame.isEndOfTransfer());
+ EXPECT_FALSE(frame.getToggle());
+ EXPECT_TRUE(frame.getSrcNodeID().isBroadcast());
+ EXPECT_TRUE(frame.getDstNodeID().isBroadcast());
+ EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
+ EXPECT_EQ(1, frame.getDataTypeID().get());
+ EXPECT_EQ(0, frame.getPriority().get());
+
+ EXPECT_EQ(payload_string.length() - 1, frame.getPayloadLen());
+ EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(),
+ reinterpret_cast<const uint8_t*>(&payload_string[0])));
+
+ std::cout << frame.toString() << std::endl;
+
+ /*
+ * Compile
+ */
+ const uint32_t DiscriminatorMask = 0x00FFFC00;
+ const uint32_t NoDiscriminatorMask = 0xFF0003FF;
+
+ CanFrame can_frame;
+ ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
+
+ ASSERT_TRUE(frame.compile(can_frame));
+ ASSERT_EQ(can_id & NoDiscriminatorMask & uavcan::CanFrame::MaskExtID,
+ can_frame.id & NoDiscriminatorMask & uavcan::CanFrame::MaskExtID);
+
+ EXPECT_EQ(payload_string.length(), can_frame.dlc);
+ EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc,
+ reinterpret_cast<const uint8_t*>(&payload_string[0])));
+
+ EXPECT_EQ((can_frame.id & DiscriminatorMask & uavcan::CanFrame::MaskExtID) >> 10, payload_crc.get() & 16383);
+
+ /*
+ * Comparison
+ */
+ ASSERT_FALSE(Frame() == frame);
+ ASSERT_TRUE(Frame() != frame);
+ frame = Frame();
+ ASSERT_TRUE(Frame() == frame);
+ ASSERT_FALSE(Frame() != frame);
+}
+
+
+TEST(Frame, FrameParsing)
+{
+ using uavcan::Frame;
+ using uavcan::CanFrame;
+ using uavcan::NodeID;
+ using uavcan::TransferID;
+
+ CanFrame can;
+ Frame frame;
+ ASSERT_FALSE(frame.parse(can));
+
+ for (unsigned i = 0; i < sizeof(CanFrame::data); i++)
+ {
+ can.data[i] = uint8_t(i | (i << 4));
+ }
+
+ /*
+ * Message CAN ID fields and offsets:
+ * 24 Priority
+ * 8 Message Type ID
+ * 7 Service Not Message (0)
+ * 0 Source Node ID
+ *
+ * Service CAN ID fields and offsets:
+ * 24 Priority
+ * 16 Service Type ID
+ * 15 Request Not Response
+ * 8 Destination Node ID
+ * 7 Service Not Message (1)
+ * 0 Source Node ID
+ */
+
+ /*
+ * SFT message broadcast
+ */
+ can.id = CanFrame::FlagEFF |
+ (2 << 24) |
+ (456 << 8) |
+ (0 << 7) |
+ (42 << 0);
+
+ can.data[7] = 0xcf; // SET=110, TID=0
+
+ ASSERT_FALSE(frame.parse(can));
+ can.dlc = 8;
+
+ ASSERT_TRUE(frame.parse(can));
+ EXPECT_TRUE(frame.isStartOfTransfer());
+ EXPECT_TRUE(frame.isEndOfTransfer());
+ EXPECT_FALSE(frame.getToggle());
+ ASSERT_EQ(2, frame.getPriority().get());
+ ASSERT_EQ(NodeID(42), frame.getSrcNodeID());
+ ASSERT_EQ(NodeID::Broadcast, frame.getDstNodeID());
+ ASSERT_EQ(456, frame.getDataTypeID().get());
+ ASSERT_EQ(TransferID(15), frame.getTransferID());
+ ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
+
+ // TODO: test service frames
+ // TODO: test malformed frames
+}
+
+
+TEST(Frame, RxFrameParse)
+{
+ using uavcan::Frame;
+ using uavcan::RxFrame;
+ using uavcan::CanFrame;
+ using uavcan::CanRxFrame;
+
+ CanRxFrame can_rx_frame;
+ RxFrame rx_frame;
+
+ // Failure
+ ASSERT_FALSE(rx_frame.parse(can_rx_frame));
+
+ // Valid
+ can_rx_frame.ts_mono = uavcan::MonotonicTime::fromUSec(1); // Zero is not allowed
+ can_rx_frame.id = CanFrame::FlagEFF |
+ (2 << 24) |
+ (456 << 8) |
+ (0 << 7) |
+ (42 << 0);
+
+ ASSERT_FALSE(rx_frame.parse(can_rx_frame));
+
+ can_rx_frame.data[0] = 0xc0; // SOT, EOT
+ can_rx_frame.dlc = 1;
+
+ ASSERT_TRUE(rx_frame.parse(can_rx_frame));
+ ASSERT_EQ(1, rx_frame.getMonotonicTimestamp().toUSec());
+ ASSERT_EQ(0, rx_frame.getIfaceIndex());
+
+ can_rx_frame.ts_mono = tsMono(123);
+ can_rx_frame.iface_index = 2;
+
+ Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0);
+ ASSERT_TRUE(frame.compile(can_rx_frame));
+
+ ASSERT_TRUE(rx_frame.parse(can_rx_frame));
+ ASSERT_EQ(123, rx_frame.getMonotonicTimestamp().toUSec());
+ ASSERT_EQ(2, rx_frame.getIfaceIndex());
+ ASSERT_EQ(456, rx_frame.getDataTypeID().get());
+ ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType());
+}
+
+
+TEST(Frame, FrameToString)
+{
+ using uavcan::Frame;
+ using uavcan::RxFrame;
+
+ // RX frame default
+ RxFrame rx_frame;
+ EXPECT_EQ("prio=255 dtid=65535 tt=3 snid=255 dnid=255 sot=0 eot=0 togl=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0",
+ rx_frame.toString());
+
+ // RX frame max len
+ rx_frame = RxFrame(Frame(uavcan::DataTypeID::MaxPossibleDataTypeIDValue, uavcan::TransferTypeMessageBroadcast,
+ uavcan::NodeID::Max, 0, uavcan::TransferID::Max),
+ uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3);
+
+ uint8_t data[8];
+ for (unsigned i = 0; i < sizeof(data); i++)
+ {
+ data[i] = uint8_t(i);
+ }
+ rx_frame.setPayload(data, sizeof(data));
+
+ rx_frame.setStartOfTransfer(true);
+ rx_frame.setEndOfTransfer(true);
+ rx_frame.flipToggle();
+ rx_frame.setPriority(uavcan::TransferPriority::NumericallyMax);
+
+ EXPECT_EQ("prio=31 dtid=65535 tt=2 snid=127 dnid=0 sot=1 eot=1 togl=1 tid=31 payload=[00 01 02 03 04 05 06] "
+ "ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3",
+ rx_frame.toString());
+
+ // Plain frame default
+ Frame frame;
+ EXPECT_EQ("prio=255 dtid=65535 tt=3 snid=255 dnid=255 sot=0 eot=0 togl=0 tid=0 payload=[]", frame.toString());
+
+ // Plain frame max len
+ frame = rx_frame;
+ EXPECT_EQ("prio=31 dtid=65535 tt=2 snid=127 dnid=0 sot=1 eot=1 togl=1 tid=31 payload=[00 01 02 03 04 05 06]",
+ frame.toString());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/incoming_transfer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include <uavcan/transport/transfer_listener.hpp>
+#include "../clock.hpp"
+#include "transfer_test_helpers.hpp"
+
+
+static uavcan::RxFrame makeFrame()
+{
+ uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0),
+ tsMono(123), tsUtc(456), 0);
+ uint8_t data[8];
+ for (uint8_t i = 0; i < sizeof(data); i++)
+ {
+ data[i] = i;
+ }
+ frame.setPayload(data, sizeof(data));
+ frame.setEndOfTransfer(true);
+ return frame;
+}
+
+
+static bool match(const uavcan::IncomingTransfer& it, const uavcan::RxFrame& frame,
+ const uint8_t* payload, unsigned payload_len)
+{
+ // Fields extracted from the frame struct
+ EXPECT_EQ(it.getMonotonicTimestamp(), frame.getMonotonicTimestamp());
+ EXPECT_EQ(it.getUtcTimestamp(), frame.getUtcTimestamp());
+ EXPECT_EQ(it.getSrcNodeID(), frame.getSrcNodeID());
+ EXPECT_EQ(it.getTransferID(), frame.getTransferID());
+ EXPECT_EQ(it.getTransferType(), frame.getTransferType());
+
+ // Payload comparison
+ static const unsigned BUFLEN = 1024;
+ uint8_t buf_reference[BUFLEN], buf_actual[BUFLEN];
+
+ if (payload_len > BUFLEN)
+ {
+ std::cout << "match(): Payload is too long" << std::endl;
+ exit(1);
+ }
+
+ std::fill(buf_reference, buf_reference + BUFLEN, 0);
+ std::fill(buf_actual, buf_actual + BUFLEN, 0);
+ std::copy(payload, payload + payload_len, buf_reference);
+
+ EXPECT_EQ(payload_len, it.read(0, buf_actual, payload_len * 3));
+ EXPECT_EQ(0, it.read(payload_len, buf_actual, payload_len * 3));
+
+ return std::equal(buf_reference, buf_reference + BUFLEN, buf_actual);
+}
+
+
+TEST(SingleFrameIncomingTransfer, Basic)
+{
+ using uavcan::RxFrame;
+ using uavcan::SingleFrameIncomingTransfer;
+
+ const RxFrame frame = makeFrame();
+ SingleFrameIncomingTransfer it(frame);
+
+ ASSERT_TRUE(match(it, frame, frame.getPayloadPtr(), frame.getPayloadLen()));
+}
+
+
+TEST(MultiFrameIncomingTransfer, Basic)
+{
+ using uavcan::RxFrame;
+ using uavcan::MultiFrameIncomingTransfer;
+
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> poolmgr;
+ uavcan::TransferBufferManager bufmgr(256, poolmgr);
+
+ const RxFrame frame = makeFrame();
+ uavcan::TransferBufferManagerKey bufmgr_key(frame.getSrcNodeID(), frame.getTransferType());
+ uavcan::TransferBufferAccessor tba(bufmgr, bufmgr_key);
+
+ MultiFrameIncomingTransfer it(frame.getMonotonicTimestamp(), frame.getUtcTimestamp(), frame, tba);
+
+ /*
+ * Empty read must fail
+ */
+ uint8_t data_byte = 0;
+ ASSERT_GT(0, it.read(0, &data_byte, 1)); // Error - no such buffer
+
+ /*
+ * Filling the test data
+ */
+ const std::string data = "123Hello world";
+ const uint8_t* const data_ptr = reinterpret_cast<const uint8_t*>(data.c_str());
+ ASSERT_FALSE(bufmgr.access(bufmgr_key));
+ ASSERT_TRUE(bufmgr.create(bufmgr_key));
+ ASSERT_EQ(data.length(), bufmgr.access(bufmgr_key)->write(0, data_ptr, unsigned(data.length())));
+
+ /*
+ * Check
+ */
+ ASSERT_TRUE(match(it, frame, data_ptr, unsigned(data.length())));
+
+ /*
+ * Buffer release
+ */
+ ASSERT_TRUE(bufmgr.access(bufmgr_key));
+ it.release();
+ ASSERT_FALSE(bufmgr.access(bufmgr_key));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/outgoing_transfer_registry.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include <uavcan/transport/outgoing_transfer_registry.hpp>
+#include "../clock.hpp"
+#include "transfer_test_helpers.hpp"
+
+
+TEST(OutgoingTransferRegistry, Basic)
+{
+ using uavcan::OutgoingTransferRegistryKey;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 2, uavcan::MemPoolBlockSize> poolmgr;
+ uavcan::OutgoingTransferRegistry otr(poolmgr);
+
+ otr.cleanup(tsMono(1000));
+
+ static const int NUM_KEYS = 5;
+ const OutgoingTransferRegistryKey keys[NUM_KEYS] =
+ {
+ OutgoingTransferRegistryKey(123, uavcan::TransferTypeServiceRequest, 42),
+ OutgoingTransferRegistryKey(321, uavcan::TransferTypeMessageBroadcast, 0),
+ OutgoingTransferRegistryKey(213, uavcan::TransferTypeServiceRequest, 2),
+ OutgoingTransferRegistryKey(312, uavcan::TransferTypeServiceRequest, 4),
+ OutgoingTransferRegistryKey(456, uavcan::TransferTypeServiceRequest, 2)
+ };
+
+ ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get());
+ ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get());
+ ASSERT_EQ(0, otr.accessOrCreate(keys[2], tsMono(1000000))->get());
+ ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get());
+ ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // OOM
+
+ /*
+ * Incrementing a little
+ */
+ otr.accessOrCreate(keys[0], tsMono(2000000))->increment();
+ otr.accessOrCreate(keys[0], tsMono(4000000))->increment();
+ otr.accessOrCreate(keys[0], tsMono(3000000))->increment();
+ ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get());
+
+ otr.accessOrCreate(keys[2], tsMono(2000000))->increment();
+ otr.accessOrCreate(keys[2], tsMono(3000000))->increment();
+ ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get());
+
+ otr.accessOrCreate(keys[3], tsMono(9000000))->increment();
+ ASSERT_EQ(1, otr.accessOrCreate(keys[3], tsMono(4000000))->get());
+
+ ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(4000000))->get());
+
+ ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // Still OOM
+
+ /*
+ * Checking existence
+ * Exist: 0, 1, 2, 3
+ * Does not exist: 4
+ */
+ ASSERT_TRUE(otr.exists(keys[1].getDataTypeID(), keys[1].getTransferType()));
+ ASSERT_TRUE(otr.exists(keys[0].getDataTypeID(), keys[0].getTransferType()));
+ ASSERT_TRUE(otr.exists(keys[3].getDataTypeID(), keys[3].getTransferType()));
+ ASSERT_TRUE(otr.exists(keys[2].getDataTypeID(), keys[2].getTransferType()));
+
+ ASSERT_FALSE(otr.exists(keys[1].getDataTypeID(), keys[2].getTransferType())); // Invalid combination
+ ASSERT_FALSE(otr.exists(keys[0].getDataTypeID(), keys[1].getTransferType())); // Invalid combination
+ ASSERT_FALSE(otr.exists(keys[4].getDataTypeID(), keys[4].getTransferType())); // Plain missing
+
+ /*
+ * Cleaning up
+ */
+ otr.cleanup(tsMono(4000001)); // Kills 1, 3
+ ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get());
+ ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get());
+ otr.accessOrCreate(keys[1], tsMono(5000000))->increment();
+ otr.accessOrCreate(keys[3], tsMono(5000000))->increment();
+
+ ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get());
+ ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get());
+
+ otr.cleanup(tsMono(5000001)); // Kills 1, 3 (He needs a bath, Jud. He stinks of the ground you buried him in.), 0
+ ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get());
+ ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get());
+ ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get());
+
+ ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(1000000))->get());
+
+ otr.cleanup(tsMono(5000001)); // Frees some memory for 4
+ ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <string>
+#include <gtest/gtest.h>
+#include <uavcan/transport/transfer.hpp>
+
+
+TEST(Transfer, TransferID)
+{
+ using uavcan::TransferID;
+
+ // Tests below are based on this assumption
+ ASSERT_EQ(32, 1 << TransferID::BitLen);
+
+ /*
+ * forwardDistance()
+ */
+ EXPECT_EQ(0, TransferID(0).computeForwardDistance(0));
+ EXPECT_EQ(1, TransferID(0).computeForwardDistance(1));
+ EXPECT_EQ(7, TransferID(0).computeForwardDistance(7));
+
+ EXPECT_EQ(0, TransferID(7).computeForwardDistance(7));
+ EXPECT_EQ(31,TransferID(31).computeForwardDistance(30));
+ EXPECT_EQ(1, TransferID(31).computeForwardDistance(0));
+
+ EXPECT_EQ(30,TransferID(7).computeForwardDistance(5));
+ EXPECT_EQ(5, TransferID(0).computeForwardDistance(5));
+
+ /*
+ * Misc
+ */
+ EXPECT_TRUE(TransferID(2) == TransferID(2));
+ EXPECT_FALSE(TransferID(2) != TransferID(2));
+ EXPECT_FALSE(TransferID(2) == TransferID(0));
+ EXPECT_TRUE(TransferID(2) != TransferID(0));
+
+ TransferID tid;
+ for (int i = 0; i < 999; i++)
+ {
+ ASSERT_EQ(i & ((1 << TransferID::BitLen) - 1), tid.get());
+ const TransferID copy = tid;
+ tid.increment();
+ ASSERT_EQ(1, copy.computeForwardDistance(tid));
+ ASSERT_EQ(31, tid.computeForwardDistance(copy));
+ ASSERT_EQ(0, tid.computeForwardDistance(tid));
+ }
+}
+
+
+TEST(Transfer, NodeID)
+{
+ uavcan::NodeID nid1(1);
+ uavcan::NodeID nid127(127);
+ uavcan::NodeID nid0(0);
+ uavcan::NodeID nidx;
+
+ ASSERT_TRUE(nid1.isUnicast());
+ ASSERT_FALSE(nid1.isBroadcast());
+ ASSERT_TRUE(nid1.isValid());
+
+ ASSERT_TRUE(nid127.isUnicast());
+ ASSERT_FALSE(nid127.isBroadcast());
+ ASSERT_TRUE(nid127.isValid());
+
+ ASSERT_FALSE(nid0.isUnicast());
+ ASSERT_TRUE(nid0.isBroadcast());
+ ASSERT_TRUE(nid0.isValid());
+
+ ASSERT_FALSE(nidx.isUnicast());
+ ASSERT_FALSE(nidx.isBroadcast());
+ ASSERT_FALSE(nidx.isValid());
+
+ /*
+ * Comparison operators
+ */
+ ASSERT_TRUE(nid1 < nid127);
+ ASSERT_TRUE(nid1 <= nid127);
+ ASSERT_TRUE(nid0 < nid1);
+ ASSERT_TRUE(nid0 <= nid1);
+
+ ASSERT_FALSE(nid1 > nid127);
+ ASSERT_FALSE(nid1 >= nid127);
+ ASSERT_FALSE(nid0 > nid1);
+ ASSERT_FALSE(nid0 >= nid1);
+
+ ASSERT_FALSE(nid1 > uavcan::NodeID(1));
+ ASSERT_TRUE(nid1 >= uavcan::NodeID(1));
+
+ ASSERT_FALSE(nid1 == nid127);
+ ASSERT_TRUE(nid127 == uavcan::NodeID(127));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer_buffer.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include <memory>
+#include <uavcan/transport/transfer_buffer.hpp>
+
+static const std::string TEST_DATA =
+ "It was like this: I asked myself one day this question - what if Napoleon, for instance, had happened to be in my "
+ "place, and if he had not had Toulon nor Egypt nor the passage of Mont Blanc to begin his career with, but "
+ "instead of all those picturesque and monumental things, there had simply been some ridiculous old hag, a "
+ "pawnbroker, who had to be murdered too to get money from her trunk (for his career, you understand). "
+ "Well, would he have brought himself to that if there had been no other means?";
+
+template <typename T, unsigned Size>
+static bool allEqual(const T (&a)[Size])
+{
+ unsigned n = Size;
+ while ((--n > 0) && (a[n] == a[0])) { }
+ return n == 0;
+}
+
+template <typename T, unsigned Size, typename R>
+static void fill(T (&a)[Size], R value)
+{
+ for (unsigned i = 0; i < Size; i++)
+ {
+ a[i] = T(value);
+ }
+}
+
+static bool matchAgainst(const std::string& data, const uavcan::ITransferBuffer& tbb,
+ unsigned offset = 0, int len = -1)
+{
+ uint8_t local_buffer[1024];
+ fill(local_buffer, 0);
+ assert((len < 0) || (sizeof(local_buffer) >= static_cast<unsigned>(len)));
+
+ if (len < 0)
+ {
+ const int res = tbb.read(offset, local_buffer, sizeof(local_buffer));
+ if (res < 0)
+ {
+ std::cout << "matchAgainst(): res " << res << std::endl;
+ return false;
+ }
+ len = res;
+ }
+ else
+ {
+ const int res = tbb.read(offset, local_buffer, unsigned(len));
+ if (res != len)
+ {
+ std::cout << "matchAgainst(): res " << res << " expected " << len << std::endl;
+ return false;
+ }
+ }
+ const bool equals = std::equal(local_buffer, local_buffer + len, data.begin() + offset);
+ if (!equals)
+ {
+ std::cout << "local_buffer:\n\t" << local_buffer << std::endl;
+ std::cout << "test_data:\n\t" << std::string(data.begin() + offset, data.begin() + offset + len) << std::endl;
+ }
+ return equals;
+}
+
+static bool matchAgainstTestData(const uavcan::ITransferBuffer& tbb, unsigned offset, int len = -1)
+{
+ return matchAgainst(TEST_DATA, tbb, offset, len);
+}
+
+TEST(TransferBuffer, TestDataValidation)
+{
+ ASSERT_LE(4, TEST_DATA.length() / uavcan::MemPoolBlockSize);
+ uint8_t local_buffer[50];
+ std::copy(TEST_DATA.begin(), TEST_DATA.begin() + sizeof(local_buffer), local_buffer);
+ ASSERT_FALSE(allEqual(local_buffer));
+}
+
+static const int TEST_BUFFER_SIZE = 200;
+
+TEST(StaticTransferBuffer, Basic)
+{
+ using uavcan::StaticTransferBuffer;
+ StaticTransferBuffer<TEST_BUFFER_SIZE> buf;
+
+ uint8_t local_buffer[TEST_BUFFER_SIZE * 2];
+ const uint8_t* const test_data_ptr = reinterpret_cast<const uint8_t*>(TEST_DATA.c_str());
+
+ // Empty reads
+ fill(local_buffer, 0xA5);
+ ASSERT_EQ(0, buf.read(0, local_buffer, 999));
+ ASSERT_EQ(0, buf.read(0, local_buffer, 0));
+ ASSERT_EQ(0, buf.read(999, local_buffer, 0));
+ ASSERT_TRUE(allEqual(local_buffer));
+
+ // Bulk write
+ ASSERT_EQ(TEST_BUFFER_SIZE, buf.write(0, test_data_ptr, unsigned(TEST_DATA.length())));
+ ASSERT_TRUE(matchAgainstTestData(buf, 0));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2));
+ ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4));
+
+ // Reset
+ fill(local_buffer, 0xA5);
+ buf.reset();
+ ASSERT_EQ(0, buf.read(0, local_buffer, 0));
+ ASSERT_EQ(0, buf.read(0, local_buffer, 999));
+ ASSERT_TRUE(allEqual(local_buffer));
+
+ // Random write
+ ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21));
+ ASSERT_TRUE(matchAgainstTestData(buf, 12, 21));
+
+ ASSERT_EQ(12, buf.write(0, test_data_ptr, 12));
+ ASSERT_TRUE(matchAgainstTestData(buf, 0));
+
+ ASSERT_EQ(0, buf.write(21, test_data_ptr + 21, 0));
+ ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, 999));
+ ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21));
+ ASSERT_TRUE(matchAgainstTestData(buf, 0));
+}
+
+
+TEST(TransferBufferManagerEntry, Basic)
+{
+ using uavcan::TransferBufferManagerEntry;
+
+ static const int MAX_SIZE = TEST_BUFFER_SIZE;
+ static const int POOL_BLOCKS = 8;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ TransferBufferManagerEntry buf(pool, MAX_SIZE);
+
+ uint8_t local_buffer[TEST_BUFFER_SIZE * 2];
+ const uint8_t* const test_data_ptr = reinterpret_cast<const uint8_t*>(TEST_DATA.c_str());
+
+ // Empty reads
+ fill(local_buffer, 0xA5);
+ ASSERT_EQ(0, buf.read(0, local_buffer, 999));
+ ASSERT_EQ(0, buf.read(0, local_buffer, 0));
+ ASSERT_EQ(0, buf.read(999, local_buffer, 0));
+ ASSERT_TRUE(allEqual(local_buffer));
+
+ // Bulk write
+ ASSERT_EQ(MAX_SIZE, buf.write(0, test_data_ptr, unsigned(TEST_DATA.length())));
+
+ ASSERT_LT(0, pool.getNumUsedBlocks()); // Making sure some memory was used
+
+ ASSERT_TRUE(matchAgainstTestData(buf, 0));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2));
+ ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4));
+
+ // Reset
+ fill(local_buffer, 0xA5);
+ buf.reset();
+ ASSERT_EQ(0, buf.read(0, local_buffer, 0));
+ ASSERT_EQ(0, buf.read(0, local_buffer, 999));
+ ASSERT_TRUE(allEqual(local_buffer));
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+
+ // Random write
+ ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21));
+ ASSERT_TRUE(matchAgainstTestData(buf, 12, 21));
+
+ ASSERT_EQ(60, buf.write(TEST_BUFFER_SIZE - 60, test_data_ptr + TEST_BUFFER_SIZE - 60, 60));
+ ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE - 60));
+
+ // Now we have two empty regions: empty-data-empty-data
+
+ ASSERT_EQ(0, buf.write(0, test_data_ptr, 0));
+ ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, TEST_BUFFER_SIZE - 21));
+ ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21));
+
+ // Now: empty-data-data-data
+
+ ASSERT_EQ(21, buf.write(0, test_data_ptr, 21));
+ ASSERT_TRUE(matchAgainstTestData(buf, 0));
+
+ // Destroying the object; memory should be released
+ ASSERT_LT(0, pool.getNumUsedBlocks());
+ buf.~TransferBufferManagerEntry();
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+}
+
+
+static const std::string MGR_TEST_DATA[4] =
+{
+ "I thought you would cry out again \'don\'t speak of it, leave off.\'\" Raskolnikov gave a laugh, but rather a "
+ "forced one. \"What, silence again?\" he asked a minute later. \"We must talk about something, you know. ",
+
+ "It would be interesting for me to know how you would decide a certain \'problem\' as Lebeziatnikov would say.\" "
+ "(He was beginning to lose the thread.) \"No, really, I am serious. Imagine, Sonia, that you had known all ",
+
+ "Luzhin\'s intentions beforehand. Known, that is, for a fact, that they would be the ruin of Katerina Ivanovna "
+ "and the children and yourself thrown in--since you don\'t count yourself for anything--Polenka too... for ",
+
+ "she\'ll go the same way. Well, if suddenly it all depended on your decision whether he or they should go on "
+ "living, that is whether Luzhin should go on living and doing wicked things, or Katerina Ivanovna should die? "
+ "How would you decide which of them was to die? I ask you?"
+};
+
+static const int MGR_MAX_BUFFER_SIZE = 100;
+
+TEST(TransferBufferManager, TestDataValidation)
+{
+ for (unsigned i = 0; i < sizeof(MGR_TEST_DATA) / sizeof(MGR_TEST_DATA[0]); i++)
+ {
+ ASSERT_LT(MGR_MAX_BUFFER_SIZE, MGR_TEST_DATA[i].length());
+ }
+}
+
+
+static int fillTestData(const std::string& data, uavcan::ITransferBuffer* tbb)
+{
+ return tbb->write(0, reinterpret_cast<const uint8_t*>(data.c_str()), unsigned(data.length()));
+}
+
+TEST(TransferBufferManager, Basic)
+{
+ using uavcan::TransferBufferManager;
+ using uavcan::TransferBufferManagerKey;
+ using uavcan::ITransferBuffer;
+
+ static const int POOL_BLOCKS = 100;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ std::auto_ptr<TransferBufferManager> mgr(new TransferBufferManager(MGR_MAX_BUFFER_SIZE, pool));
+
+ // Empty
+ ASSERT_FALSE(mgr->access(TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast)));
+ ASSERT_FALSE(mgr->access(TransferBufferManagerKey(127, uavcan::TransferTypeServiceRequest)));
+
+ ITransferBuffer* tbb = UAVCAN_NULLPTR;
+
+ const TransferBufferManagerKey keys[5] =
+ {
+ TransferBufferManagerKey(0, uavcan::TransferTypeServiceRequest),
+ TransferBufferManagerKey(1, uavcan::TransferTypeMessageBroadcast),
+ TransferBufferManagerKey(2, uavcan::TransferTypeServiceRequest),
+ TransferBufferManagerKey(127, uavcan::TransferTypeServiceResponse),
+ TransferBufferManagerKey(64, uavcan::TransferTypeMessageBroadcast)
+ };
+
+ ASSERT_TRUE((tbb = mgr->create(keys[0])));
+ ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[0], tbb));
+ ASSERT_EQ(1, mgr->getNumBuffers());
+
+ ASSERT_TRUE((tbb = mgr->create(keys[1])));
+ ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[1], tbb));
+ ASSERT_EQ(2, mgr->getNumBuffers());
+ ASSERT_LT(2, pool.getNumUsedBlocks());
+
+ ASSERT_TRUE((tbb = mgr->create(keys[2])));
+ ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[2], tbb));
+ ASSERT_EQ(3, mgr->getNumBuffers());
+
+ std::cout << "TransferBufferManager - Basic: Pool usage: " << pool.getNumUsedBlocks() << std::endl;
+
+ ASSERT_TRUE((tbb = mgr->create(keys[3])));
+
+ ASSERT_LT(0, fillTestData(MGR_TEST_DATA[3], tbb));
+ ASSERT_EQ(4, mgr->getNumBuffers());
+
+ // Making sure all buffers contain proper data
+ ASSERT_TRUE((tbb = mgr->access(keys[0])));
+ ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[0], *tbb));
+
+ ASSERT_TRUE((tbb = mgr->access(keys[1])));
+ ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[1], *tbb));
+
+ ASSERT_TRUE((tbb = mgr->access(keys[2])));
+ ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[2], *tbb));
+
+ ASSERT_TRUE((tbb = mgr->access(keys[3])));
+ ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[3], *tbb));
+
+ mgr->remove(keys[1]);
+ ASSERT_FALSE(mgr->access(keys[1]));
+ ASSERT_EQ(3, mgr->getNumBuffers());
+ ASSERT_LT(0, pool.getNumFreeBlocks());
+
+ mgr->remove(keys[0]);
+ ASSERT_FALSE(mgr->access(keys[0]));
+ ASSERT_EQ(2, mgr->getNumBuffers());
+
+ // At this time we have the following NodeID: 2, 127
+ ASSERT_TRUE((tbb = mgr->access(keys[2])));
+ ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[2], *tbb));
+
+ ASSERT_TRUE((tbb = mgr->access(keys[3])));
+ ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[3], *tbb));
+
+ // These were deleted: 0, 1; 3 is still there
+ ASSERT_FALSE(mgr->access(keys[1]));
+ ASSERT_FALSE(mgr->access(keys[0]));
+ ASSERT_TRUE(mgr->access(keys[3]));
+
+ // Filling the memory again in order to check the destruction below
+ ASSERT_TRUE((tbb = mgr->create(keys[1])));
+ ASSERT_LT(0, fillTestData(MGR_TEST_DATA[1], tbb));
+
+ // Deleting the object; all memory must be freed
+ ASSERT_NE(0, pool.getNumUsedBlocks());
+ mgr.reset();
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer_listener.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,268 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "transfer_test_helpers.hpp"
+#include "../clock.hpp"
+
+
+class TransferListenerEmulator : public IncomingTransferEmulatorBase
+{
+ uavcan::TransferListener& target_;
+ const uavcan::DataTypeDescriptor data_type_;
+
+public:
+ TransferListenerEmulator(uavcan::TransferListener& target, const uavcan::DataTypeDescriptor& type,
+ uavcan::NodeID dst_node_id = 127)
+ : IncomingTransferEmulatorBase(dst_node_id)
+ , target_(target)
+ , data_type_(type)
+ { }
+
+ void sendOneFrame(const uavcan::RxFrame& frame) { target_.handleFrame(frame); }
+
+ Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type,
+ uint8_t source_node_id, const std::string& payload)
+ {
+ return IncomingTransferEmulatorBase::makeTransfer(priority, transfer_type, source_node_id, payload, data_type_);
+ }
+};
+
+
+TEST(TransferListener, BasicMFT)
+{
+ const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A");
+
+ static const int NUM_POOL_BLOCKS = 100;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * NUM_POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ uavcan::TransferPerfCounter perf;
+ TestListener subscriber(perf, type, 256, pool);
+
+ /*
+ * Test data
+ */
+ static const std::string DATA[] =
+ {
+ "Build a man a fire, and he'll be warm for a day. "
+ "Set a man on fire, and he'll be warm for the rest of his life.",
+
+ "123456789",
+
+ "In the beginning there was nothing, which exploded.",
+
+ "The USSR, which they'd begun to renovate and improve at about the time when Tatarsky decided to "
+ "change his profession, improved so much that it ceased to exist",
+
+ "BEWARE JET BLAST"
+ };
+
+ for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++)
+ {
+ std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl;
+ }
+
+ TransferListenerEmulator emulator(subscriber, type);
+ const Transfer transfers[] =
+ {
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 1, DATA[0]),
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 2, DATA[1]), // Same NID
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 3, DATA[2]),
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 4, DATA[3]),
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 5, DATA[4]),
+ };
+
+ /*
+ * Sending concurrently
+ * Expected reception order: 1, 4, 2, 0, 3
+ */
+ emulator.send(transfers);
+
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[1]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[4]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[2]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[0]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[3]));
+
+ ASSERT_TRUE(subscriber.isEmpty());
+}
+
+
+TEST(TransferListener, CrcFailure)
+{
+ const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A");
+
+ static const int NUM_POOL_BLOCKS = 100;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * NUM_POOL_BLOCKS, uavcan::MemPoolBlockSize> poolmgr;
+ uavcan::TransferPerfCounter perf;
+ TestListener subscriber(perf, type, 256, poolmgr); // Static buffer only, 2 entries
+
+ /*
+ * Generating transfers with damaged payload (CRC is not valid)
+ */
+ TransferListenerEmulator emulator(subscriber, type);
+ const Transfer tr_mft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 42, "123456789abcdefghik");
+ const Transfer tr_sft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 11, "abcd");
+
+ std::vector<uavcan::RxFrame> ser_mft = serializeTransfer(tr_mft);
+ std::vector<uavcan::RxFrame> ser_sft = serializeTransfer(tr_sft);
+
+ ASSERT_TRUE(ser_mft.size() > 1);
+ ASSERT_TRUE(ser_sft.size() == 1);
+
+ const_cast<uint8_t*>(ser_mft[1].getPayloadPtr())[1] = uint8_t(~ser_mft[1].getPayloadPtr()[1]); // CRC invalid now
+ const_cast<uint8_t*>(ser_sft[0].getPayloadPtr())[2] = uint8_t(~ser_sft[0].getPayloadPtr()[2]); // no CRC here
+
+ /*
+ * Sending and making sure that MFT was not received, but SFT was.
+ */
+ std::vector<std::vector<uavcan::RxFrame> > sers;
+ sers.push_back(ser_mft);
+ sers.push_back(ser_sft);
+ sers.push_back(ser_mft); // Ignored
+ sers.push_back(ser_sft); // Ignored
+
+ emulator.send(sers);
+
+ Transfer tr_sft_damaged = tr_sft;
+ tr_sft_damaged.payload[2] = char(~tr_sft.payload[2]); // Damaging the data similarly, so that it can be matched
+
+ ASSERT_TRUE(subscriber.matchAndPop(tr_sft_damaged));
+ ASSERT_TRUE(subscriber.isEmpty());
+}
+
+
+TEST(TransferListener, BasicSFT)
+{
+ const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A");
+
+ static const int NUM_POOL_BLOCKS = 100;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * NUM_POOL_BLOCKS, uavcan::MemPoolBlockSize> poolmgr;
+ uavcan::TransferPerfCounter perf;
+ TestListener subscriber(perf, type, 0, poolmgr); // Max buf size is 0, i.e. SFT-only
+
+ TransferListenerEmulator emulator(subscriber, type);
+ const Transfer transfers[] =
+ {
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 1, "123"),
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 1, "456"), // Same NID
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, ""),
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 3, "abc"),
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 4, ""),
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "foo"), // Same as 2, not ignored
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "123456789abc"), // Same as 2, not SFT - ignore
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "bar"), // Same as 2, not ignored
+ };
+
+ emulator.send(transfers);
+
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[0]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[1]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[2]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[3]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[4]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[5]));
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[7]));
+
+ ASSERT_TRUE(subscriber.isEmpty());
+}
+
+
+TEST(TransferListener, Cleanup)
+{
+ const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A");
+
+ static const int NUM_POOL_BLOCKS = 100;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * NUM_POOL_BLOCKS, uavcan::MemPoolBlockSize> poolmgr;
+ uavcan::TransferPerfCounter perf;
+ TestListener subscriber(perf, type, 256, poolmgr);
+
+ /*
+ * Generating transfers
+ */
+ TransferListenerEmulator emulator(subscriber, type);
+ const Transfer tr_mft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 42, "123456789abcdefghik");
+ const Transfer tr_sft = emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 11, "abcd");
+
+ const std::vector<uavcan::RxFrame> ser_mft = serializeTransfer(tr_mft);
+ const std::vector<uavcan::RxFrame> ser_sft = serializeTransfer(tr_sft);
+
+ ASSERT_TRUE(ser_mft.size() > 1);
+ ASSERT_TRUE(ser_sft.size() == 1);
+
+ const std::vector<uavcan::RxFrame> ser_mft_begin(ser_mft.begin(), ser_mft.begin() + 1);
+
+ /*
+ * Sending the first part and SFT
+ */
+ std::vector<std::vector<uavcan::RxFrame> > sers;
+ sers.push_back(ser_mft_begin); // Only the first part
+ sers.push_back(ser_sft);
+
+ emulator.send(sers);
+
+ ASSERT_TRUE(subscriber.matchAndPop(tr_sft));
+ ASSERT_TRUE(subscriber.isEmpty());
+
+ /*
+ * Cleanup with huge timestamp value will remove all entries
+ */
+ static_cast<uavcan::TransferListener&>(subscriber).cleanup(tsMono(100000000));
+
+ /*
+ * Sending the same transfers again - they will be accepted since registres were cleared
+ */
+ sers.clear();
+ sers.push_back(ser_mft); // Complete transfer
+ sers.push_back(ser_sft);
+
+ emulator.send(sers);
+
+ ASSERT_TRUE(subscriber.matchAndPop(tr_sft));
+ ASSERT_TRUE(subscriber.matchAndPop(tr_mft));
+ ASSERT_TRUE(subscriber.isEmpty());
+}
+
+
+TEST(TransferListener, AnonymousTransfers)
+{
+ const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A");
+
+ static const int NUM_POOL_BLOCKS = 100;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * NUM_POOL_BLOCKS, uavcan::MemPoolBlockSize> poolmgr;
+ uavcan::TransferPerfCounter perf;
+ TestListener subscriber(perf, type, 0, poolmgr);
+
+ TransferListenerEmulator emulator(subscriber, type);
+ const Transfer transfers[] =
+ {
+ emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 0, "1234567"), // Invalid - not broadcast
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "1234567"), // Valid
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "12345678"), // Invalid - not SFT
+ emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "") // Valid
+ };
+
+ emulator.send(transfers);
+
+ // Nothing will be received, because anonymous transfers are disabled by default
+ ASSERT_TRUE(subscriber.isEmpty());
+
+ subscriber.allowAnonymousTransfers();
+
+ // Re-send everything again
+ emulator.send(transfers);
+
+ // Now the anonymous transfers are enabled
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); // Only SFT broadcast will be accepted
+ ASSERT_TRUE(subscriber.matchAndPop(transfers[3]));
+
+ ASSERT_TRUE(subscriber.isEmpty());
+}
+
+TEST(TransferListener, Sizes)
+{
+ using namespace uavcan;
+
+ std::cout << "sizeof(TransferListener): " << sizeof(TransferListener) << std::endl;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer_receiver.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,561 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include <uavcan/transport/transfer_receiver.hpp>
+#include "../clock.hpp"
+#include "transfer_test_helpers.hpp"
+
+/*
+ * Beware!
+ * The code you're about to look at desperately needs some cleaning.
+ */
+
+enum SotEotToggle
+{
+ SET000 = 0,
+ SET001 = 1,
+ SET010 = 2,
+ SET011 = 3,
+ SET100 = 4,
+ SET101 = 5, // Illegal
+ SET110 = 6,
+ SET111 = 7 // Illegal
+};
+
+struct RxFrameGenerator
+{
+ static const uavcan::TransferBufferManagerKey DEFAULT_KEY;
+ enum { TARGET_NODE_ID = 126 };
+
+ uint16_t data_type_id;
+ uavcan::TransferBufferManagerKey bufmgr_key;
+
+ RxFrameGenerator(uint16_t data_type_id, const uavcan::TransferBufferManagerKey& bufmgr_key = DEFAULT_KEY)
+ : data_type_id(data_type_id)
+ , bufmgr_key(bufmgr_key)
+ { }
+
+ /// iface_index, data, set, transfer_id, ts_monotonic [, ts_utc]
+ uavcan::RxFrame operator()(uint8_t iface_index, const std::string& data, SotEotToggle set,
+ uint8_t transfer_id, uint64_t ts_monotonic, uint64_t ts_utc = 0)
+ {
+ const uavcan::NodeID dst_nid =
+ (bufmgr_key.getTransferType() == uavcan::TransferTypeMessageBroadcast) ?
+ uavcan::NodeID::Broadcast : TARGET_NODE_ID;
+
+ uavcan::Frame frame(data_type_id, bufmgr_key.getTransferType(), bufmgr_key.getNodeID(),
+ dst_nid, transfer_id);
+
+ frame.setStartOfTransfer((set & (1 << 2)) != 0);
+ frame.setEndOfTransfer((set & (1 << 1)) != 0);
+
+ if ((set & (1 << 0)) != 0)
+ {
+ frame.flipToggle();
+ }
+
+ EXPECT_EQ(data.length(),
+ frame.setPayload(reinterpret_cast<const uint8_t*>(data.c_str()), unsigned(data.length())));
+
+ uavcan::RxFrame output(frame, uavcan::MonotonicTime::fromUSec(ts_monotonic),
+ uavcan::UtcTime::fromUSec(ts_utc), iface_index);
+ //std::cout << "Generated frame: " << output.toString() << std::endl;
+
+ return output;
+ }
+};
+
+const uavcan::TransferBufferManagerKey RxFrameGenerator::DEFAULT_KEY(42, uavcan::TransferTypeMessageBroadcast);
+
+
+template <unsigned BufSize>
+struct Context
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> pool;
+ uavcan::TransferReceiver receiver; // Must be default constructible and copyable
+ uavcan::TransferBufferManager bufmgr;
+
+ Context() :
+ bufmgr(BufSize, pool)
+ { }
+
+ ~Context()
+ {
+ // We need to destroy the receiver before its buffer manager
+ receiver = uavcan::TransferReceiver();
+ }
+};
+
+
+static bool matchBufferContent(const uavcan::ITransferBuffer* tbb, const std::string& content)
+{
+ uint8_t data[1024];
+ std::fill(data, data + sizeof(data), 0);
+ if (content.length() > sizeof(data))
+ {
+ std::cerr << "matchBufferContent(): Content is too long" << std::endl;
+ std::exit(1);
+ }
+ tbb->read(0, data, unsigned(content.length()));
+ if (std::equal(content.begin(), content.end(), data))
+ {
+ return true;
+ }
+ std::cout << "Buffer content mismatch:"
+ << "\n\tExpected: " << content
+ << "\n\tActually: " << reinterpret_cast<const char*>(data)
+ << std::endl;
+ return false;
+}
+
+
+#define CHECK_NOT_COMPLETE(x) ASSERT_EQ(uavcan::TransferReceiver::ResultNotComplete, (x))
+#define CHECK_COMPLETE(x) ASSERT_EQ(uavcan::TransferReceiver::ResultComplete, (x))
+#define CHECK_SINGLE_FRAME(x) ASSERT_EQ(uavcan::TransferReceiver::ResultSingleFrame, (x))
+
+TEST(TransferReceiver, Basic)
+{
+ using uavcan::TransferReceiver;
+ Context<32> context;
+ RxFrameGenerator gen(789);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+ uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
+
+ std::cout << "sizeof(TransferReceiver): " << sizeof(TransferReceiver) << std::endl;
+
+ /*
+ * Empty
+ */
+ ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
+ ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ /*
+ * Single frame transfer with zero ts, must be ignored
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "Foo", SET110, 0, 0), bk));
+ ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
+ ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ /*
+ * Valid compound transfer
+ * Args: iface_index, data, set, transfer_id, ts_monotonic
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "\x34\x12" "34567", SET100, 0, 100), bk));
+ CHECK_COMPLETE( rcv.addFrame(gen(0, "foo", SET011, 0, 200), bk));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567foo"));
+ ASSERT_EQ(0x1234, rcv.getLastTransferCrc());
+ ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); // Not initialized yet
+ ASSERT_EQ(100, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ /*
+ * Compound transfer mixed with invalid frames; buffer was not released explicitly
+ * Args: iface_index, data, set, transfer_id, ts_monotonic
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET100, 0, 300), bk)); // Previous TID, rejected
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "rty", SET100, 0, 300), bk)); // Previous TID, wrong iface
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "\x9a\x78" "34567", SET100, 1, 1000), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET100, 1, 1100), bk)); // Old toggle
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET000, 1, 1100), bk)); // Old toggle
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "abcdefg", SET001, 1, 1200), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "4567891", SET001, 2, 1300), bk)); // Next TID, but not SOT
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET010, 1, 1300), bk)); // Wrong iface
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", SET001, 1, 1300), bk)); // Unexpected toggle
+ CHECK_COMPLETE( rcv.addFrame(gen(0, "", SET010, 1, 1300), bk));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcdefg"));
+ ASSERT_EQ(0x789A, rcv.getLastTransferCrc());
+ ASSERT_GE(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
+ ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval());
+ ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_FALSE(rcv.isTimedOut(tsMono(1000)));
+ ASSERT_FALSE(rcv.isTimedOut(tsMono(5000)));
+ ASSERT_TRUE(rcv.isTimedOut(tsMono(60000000)));
+
+ /*
+ * Single-frame transfers
+ * Args: iface_index, data, set, transfer_id, ts_monotonic
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET110, 1, 2000), bk)); // Previous TID
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 2, 2100), bk)); // Wrong iface
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 2, 2200), bk));
+
+ ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer must be removed
+ ASSERT_GT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
+ ASSERT_EQ(2200, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 3, 2500), bk));
+ ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 0, 3000), bk));
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 1, 3100), bk));
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 3, 3200), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 0, 3300), bk)); // Old TID, wrong iface
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 2, 3400), bk)); // Old TID, wrong iface
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 3, 3500), bk)); // Old TID, wrong iface
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 4, 3600), bk));
+ ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ /*
+ * Timeouts
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 1, 5000), bk)); // Wrong iface - ignored
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", SET110, 6, 1500000), bk)); // Accepted due to iface timeout
+ ASSERT_EQ(1500000, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET110, 7, 1500100), bk)); // Ignored - old iface 0
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", SET110, 7, 1500100), bk));
+ ASSERT_EQ(1500100, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 7, 1500100), bk)); // Old TID
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 7, 100000000), bk)); // Accepted - global timeout
+ ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 8, 100000100), bk));
+ ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ ASSERT_TRUE(rcv.isTimedOut(tsMono(900000000)));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "34567", SET100, 0, 900000000), bk)); // Global timeout
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 0, 900000100), bk)); // Wrong iface
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET011, 0, 900000200), bk)); // Wrong iface
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", SET011, 0, 900000200), bk));
+
+ ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic().toUSec());
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ ASSERT_FALSE(rcv.isTimedOut(tsMono(1000)));
+ ASSERT_FALSE(rcv.isTimedOut(tsMono(900000300)));
+ ASSERT_TRUE(rcv.isTimedOut(tsMono(9990000000)));
+
+ ASSERT_LT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval());
+ ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval());
+ ASSERT_GE(TransferReceiver::getMaxTransferInterval(), rcv.getInterval());
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwe"));
+ ASSERT_EQ(0x5678, rcv.getLastTransferCrc());
+
+ /*
+ * Destruction
+ */
+ ASSERT_TRUE(bufmgr.access(gen.bufmgr_key));
+ context.receiver.~TransferReceiver(); // TransferReceiver does not own the buffer, it must not be released!
+ ASSERT_TRUE(bufmgr.access(gen.bufmgr_key)); // Making sure that the buffer is still there
+}
+
+
+TEST(TransferReceiver, OutOfBufferSpace_32bytes)
+{
+ Context<32> context;
+ RxFrameGenerator gen(789);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+ uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
+
+ /*
+ * Simple transfer, maximum buffer length
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 1, 100000000), bk)); // 5
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 1, 100000100), bk)); // 12
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET000, 1, 100000200), bk)); // 19
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 1, 100000300), bk)); // 26
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "123456", SET010, 1, 100000400), bk)); // 32
+
+ ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567123456712345671234567123456"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); // CRC from "12", which is 0x3231 in little endian
+
+ /*
+ * Transfer longer than available buffer space
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 2, 100001000), bk)); // 5
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 2, 100001100), bk)); // 12
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET000, 2, 100001200), bk)); // 19
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 2, 100001200), bk)); // 26
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET010, 2, 100001300), bk)); // 33 // EOT, ignored - lost sync
+
+ ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); // Timestamp will not be overriden
+ ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed
+
+ ASSERT_EQ(1, rcv.yieldErrorCount());
+ ASSERT_EQ(0, rcv.yieldErrorCount());
+}
+
+
+TEST(TransferReceiver, OutOfOrderFrames)
+{
+ Context<32> context;
+ RxFrameGenerator gen(789);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+ uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
+
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 7, 100000000), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET000, 7, 100000100), bk)); // Out of order
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 7, 100000200), bk)); // Out of order
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 7, 100000300), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET011, 7, 100000200), bk)); // Out of order
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 7, 100000400), bk));
+
+ ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+ ASSERT_EQ(3, rcv.yieldErrorCount());
+ ASSERT_EQ(0, rcv.yieldErrorCount());
+}
+
+
+TEST(TransferReceiver, IntervalMeasurement)
+{
+ Context<32> context;
+ RxFrameGenerator gen(789);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+ uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
+
+ static const int INTERVAL = 1000;
+ uavcan::TransferID tid;
+ uint64_t timestamp = 100000000;
+
+ for (int i = 0; i < 1000; i++)
+ {
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, tid.get(), timestamp), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, tid.get(), timestamp), bk));
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, tid.get(), timestamp), bk));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+ ASSERT_EQ(timestamp, rcv.getLastTransferTimestampMonotonic().toUSec());
+
+ timestamp += uint64_t(INTERVAL);
+ tid.increment();
+ }
+
+ ASSERT_EQ(INTERVAL, rcv.getInterval().toUSec());
+}
+
+
+TEST(TransferReceiver, Restart)
+{
+ Context<32> context;
+ RxFrameGenerator gen(789);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+ uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
+
+ /*
+ * This transfer looks complete, but must be ignored because of large delay after the first frame
+ * Args: iface_index, data, set, transfer_id, ts_monotonic [, ts_utc]
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET100, 0, 100), bk)); // Begin
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 0, 100000100), bk)); // Continue 100 sec later, expired
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET010, 0, 100000200), bk)); // Ignored
+
+ /*
+ * Begins immediately after, encounters a delay 0.9 sec but completes OK
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 0, 100000300), bk)); // Begin
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 0, 100900300), bk)); // 0.9 sec later
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "1234567", SET010, 0, 100900400), bk)); // OK nevertheless
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+ std::cout << "Interval: " << rcv.getInterval().toString() << std::endl;
+
+ /*
+ * Begins OK, gets a timeout, switches to another iface
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET100, 1, 103000500), bk)); // Begin
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET001, 1, 105000500), bk)); // 2 sec later, timeout
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 1, 105000600), bk)); // Same TID, another iface - ignore
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 2, 105000700), bk)); // Not first frame - ignore
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 2, 105000800), bk)); // First, another iface - restart
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 1, 105000600), bk)); // Old iface - ignore
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET001, 2, 105000900), bk)); // Continuing
+ CHECK_COMPLETE( rcv.addFrame(gen(0, "1234567", SET010, 2, 105000910), bk)); // Done
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+ ASSERT_EQ(4, rcv.yieldErrorCount());
+ ASSERT_EQ(0, rcv.yieldErrorCount());
+}
+
+
+TEST(TransferReceiver, UtcTransferTimestamping)
+{
+ Context<32> context;
+ RxFrameGenerator gen(789);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+ uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
+
+ /*
+ * Zero UTC timestamp must be preserved
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 0, 1, 0), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 0, 2, 0), bk));
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 0, 3, 0), bk));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd"));
+ ASSERT_EQ(1, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_EQ(0, rcv.getLastTransferTimestampUtc().toUSec());
+
+ /*
+ * Non-zero UTC timestamp
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 1, 4, 123), bk)); // This UTC is going to be preserved
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 1, 5, 0), bk)); // Following are ignored
+ CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 1, 6, 42), bk));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd"));
+ ASSERT_EQ(4, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_EQ(123, rcv.getLastTransferTimestampUtc().toUSec());
+
+ /*
+ * Single-frame transfers
+ * iface_index, data, set, transfer_id, ts_monotonic
+ */
+ CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "abc", SET110, 2, 10, 100000000), bk)); // Exact value is irrelevant
+ ASSERT_EQ(10, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_EQ(100000000, rcv.getLastTransferTimestampUtc().toUSec());
+
+ /*
+ * Restart recovery
+ */
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 1, 100000000, 800000000), bk));
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET001, 1, 100000001, 300000000), bk));
+ CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET010, 1, 100000002, 900000000), bk));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd"));
+ ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec());
+ ASSERT_EQ(800000000, rcv.getLastTransferTimestampUtc().toUSec());
+}
+
+
+TEST(TransferReceiver, HeaderParsing)
+{
+ static const std::string SFT_PAYLOAD = "1234567";
+
+ uavcan::TransferID tid;
+
+ /*
+ * Broadcast
+ */
+ {
+ Context<32> context;
+ RxFrameGenerator gen(123);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+
+ /*
+ * MFT, message broadcasting
+ */
+ {
+ gen.bufmgr_key =
+ uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast);
+ uavcan::TransferBufferAccessor bk1(context.bufmgr, gen.bufmgr_key);
+
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), 1), bk1));
+ CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET011, tid.get(), 2), bk1));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+ tid.increment();
+ bk1.remove();
+ }
+
+ /*
+ * SFT, message broadcasting
+ */
+ {
+ gen.bufmgr_key =
+ uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast);
+ uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key);
+
+ const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), 1000);
+
+ CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk));
+ ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero
+
+ // All bytes are payload, zero overhead
+ ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr()));
+
+ tid.increment();
+ }
+ }
+
+ /*
+ * Unicast
+ */
+ {
+ Context<32> context;
+ RxFrameGenerator gen(123);
+ uavcan::TransferReceiver& rcv = context.receiver;
+ uavcan::TransferBufferManager& bufmgr = context.bufmgr;
+
+ static const uavcan::TransferType ADDRESSED_TRANSFER_TYPES[2] =
+ {
+ uavcan::TransferTypeServiceRequest,
+ uavcan::TransferTypeServiceResponse
+ };
+
+ /*
+ * MFT, service request/response
+ */
+ for (unsigned i = 0; i < (sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++)
+ {
+ gen.bufmgr_key =
+ uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]);
+ uavcan::TransferBufferAccessor bk2(context.bufmgr, gen.bufmgr_key);
+
+ const uint64_t ts_monotonic = i + 10;
+
+ CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), ts_monotonic), bk2));
+ CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET011, tid.get(), ts_monotonic), bk2));
+
+ ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd"));
+ ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+ tid.increment();
+ bk2.remove();
+ }
+
+ /*
+ * SFT, message unicast, service request/response
+ */
+ for (unsigned i = 0; i < int(sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++)
+ {
+ gen.bufmgr_key =
+ uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]);
+ uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key);
+
+ const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), i + 10000U);
+
+ CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk));
+ ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero
+
+ // First byte must be ignored
+ ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr()));
+
+ tid.increment();
+ }
+ }
+
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer_sender.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include "transfer_test_helpers.hpp"
+#include "can/can.hpp"
+#include <uavcan/transport/transfer_sender.hpp>
+
+static int sendOne(uavcan::TransferSender& sender, const std::string& data,
+ uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
+ uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id)
+{
+ return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), unsigned(data.length()),
+ uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline),
+ uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id);
+}
+
+static int sendOne(uavcan::TransferSender& sender, const std::string& data,
+ uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
+ uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid)
+{
+ return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), unsigned(data.length()),
+ uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline),
+ uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id, tid);
+}
+
+
+TEST(TransferSender, Basic)
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> poolmgr;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ static const uavcan::NodeID TX_NODE_ID(64);
+ static const uavcan::NodeID RX_NODE_ID(65);
+ uavcan::Dispatcher dispatcher_tx(driver, poolmgr, clockmock);
+ uavcan::Dispatcher dispatcher_rx(driver, poolmgr, clockmock);
+ ASSERT_TRUE(dispatcher_tx.setNodeID(TX_NODE_ID));
+ ASSERT_TRUE(dispatcher_rx.setNodeID(RX_NODE_ID));
+
+ /*
+ * Test environment
+ */
+ static const uavcan::DataTypeDescriptor TYPES[2] =
+ {
+ makeDataType(uavcan::DataTypeKindMessage, 1),
+ makeDataType(uavcan::DataTypeKindService, 1)
+ };
+
+ uavcan::TransferSender senders[2] =
+ {
+ uavcan::TransferSender(dispatcher_tx, TYPES[0], uavcan::CanTxQueue::Volatile),
+ uavcan::TransferSender(dispatcher_tx, TYPES[1], uavcan::CanTxQueue::Persistent)
+ };
+
+ static const std::string DATA[4] =
+ {
+ "Don't panic.",
+
+ "The ships hung in the sky in much the same way that bricks don't.",
+
+ "Would it save you a lot of time if I just gave up and went mad now?",
+
+ "If there's anything more important than my ego around, I want it caught and shot now."
+ };
+
+ /*
+ * Transmission
+ */
+ static const uint64_t TX_DEADLINE = 1000000;
+
+ // Low priority
+ senders[0].setPriority(20);
+ sendOne(senders[0], DATA[0], TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0);
+ sendOne(senders[0], DATA[1], TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0);
+ // High priority
+ senders[0].setPriority(10);
+ sendOne(senders[0], "123", TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0);
+ sendOne(senders[0], "456", TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0);
+
+ senders[1].setPriority(15);
+ sendOne(senders[1], DATA[2], TX_DEADLINE, 0, uavcan::TransferTypeServiceRequest, RX_NODE_ID);
+ sendOne(senders[1], DATA[3], TX_DEADLINE, 0, uavcan::TransferTypeServiceResponse, RX_NODE_ID, 1);
+ sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TransferTypeServiceRequest, RX_NODE_ID);
+ sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TransferTypeServiceResponse, RX_NODE_ID, 2);
+
+ using namespace uavcan;
+ static const Transfer TRANSFERS[8] =
+ {
+ Transfer(TX_DEADLINE, 0, 20, TransferTypeMessageBroadcast, 0, TX_NODE_ID, 0, DATA[0], TYPES[0]),
+ Transfer(TX_DEADLINE, 0, 20, TransferTypeMessageBroadcast, 1, TX_NODE_ID, 0, DATA[1], TYPES[0]),
+ Transfer(TX_DEADLINE, 0, 10, TransferTypeMessageBroadcast, 2, TX_NODE_ID, 0, "123", TYPES[0]),
+ Transfer(TX_DEADLINE, 0, 10, TransferTypeMessageBroadcast, 3, TX_NODE_ID, 0, "456", TYPES[0]),
+
+ Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceRequest, 0, TX_NODE_ID, RX_NODE_ID, DATA[2], TYPES[1]),
+ Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceResponse, 1, TX_NODE_ID, RX_NODE_ID, DATA[3], TYPES[1]),
+ Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceRequest, 1, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]),
+ Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceResponse, 2, TX_NODE_ID, RX_NODE_ID, "", TYPES[1])
+ };
+
+ /*
+ * Making sure that the abort flag is not used.
+ */
+ ASSERT_EQ(0, driver.ifaces.at(0).tx.front().flags);
+
+ /*
+ * Receiving on the other side.
+ */
+ for (uint8_t i = 0; i < driver.getNumIfaces(); i++) // Moving the frames from TX to RX side
+ {
+ CanIfaceMock& iface = driver.ifaces.at(i);
+ std::cout << "Num frames: " << iface.tx.size() << std::endl;
+ while (!iface.tx.empty())
+ {
+ CanIfaceMock::FrameWithTime ft = iface.tx.front();
+ iface.tx.pop();
+ iface.rx.push(ft);
+ }
+ }
+
+ TestListener sub_msg(dispatcher_rx.getTransferPerfCounter(), TYPES[0], 512, poolmgr);
+ TestListener sub_srv_req(dispatcher_rx.getTransferPerfCounter(), TYPES[1], 512, poolmgr);
+ TestListener sub_srv_resp(dispatcher_rx.getTransferPerfCounter(), TYPES[1], 512, poolmgr);
+
+ dispatcher_rx.registerMessageListener(&sub_msg);
+ dispatcher_rx.registerServiceRequestListener(&sub_srv_req);
+ dispatcher_rx.registerServiceResponseListener(&sub_srv_resp);
+
+ while (true)
+ {
+ const int res = dispatcher_rx.spin(tsMono(0));
+ ASSERT_LE(0, res);
+ clockmock.advance(100);
+ if (res == 0)
+ {
+ break;
+ }
+ }
+
+ /*
+ * Validation
+ */
+ ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[0]));
+ ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[1]));
+ ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[2]));
+ ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[3]));
+
+ ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[4]));
+ ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[6]));
+
+ ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[5]));
+ ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[7]));
+
+ /*
+ * Perf counters
+ */
+ EXPECT_EQ(0, dispatcher_tx.getTransferPerfCounter().getErrorCount());
+ EXPECT_EQ(8, dispatcher_tx.getTransferPerfCounter().getTxTransferCount());
+ EXPECT_EQ(0, dispatcher_tx.getTransferPerfCounter().getRxTransferCount());
+
+ EXPECT_EQ(0, dispatcher_rx.getTransferPerfCounter().getErrorCount());
+ EXPECT_EQ(0, dispatcher_rx.getTransferPerfCounter().getTxTransferCount());
+ EXPECT_EQ(8, dispatcher_rx.getTransferPerfCounter().getRxTransferCount());
+}
+
+
+struct TransferSenderTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase
+{
+ uavcan::RxFrame last_frame;
+ unsigned count;
+
+ TransferSenderTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher)
+ : uavcan::LoopbackFrameListenerBase(dispatcher)
+ , count(0)
+ {
+ startListening();
+ }
+
+ void handleLoopbackFrame(const uavcan::RxFrame& frame)
+ {
+ last_frame = frame;
+ count++;
+ }
+};
+
+TEST(TransferSender, Loopback)
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> poolmgr;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ static const uavcan::NodeID TX_NODE_ID(64);
+ uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock);
+ ASSERT_TRUE(dispatcher.setNodeID(TX_NODE_ID));
+
+ uavcan::DataTypeDescriptor desc = makeDataType(uavcan::DataTypeKindMessage, 1, "Foobar");
+
+ uavcan::TransferSender sender(dispatcher, desc, uavcan::CanTxQueue::Volatile);
+
+ sender.setCanIOFlags(uavcan::CanIOFlagLoopback);
+ ASSERT_EQ(uavcan::CanIOFlagLoopback, sender.getCanIOFlags());
+
+ sender.setIfaceMask(2);
+ ASSERT_EQ(2, sender.getIfaceMask());
+
+ TransferSenderTestLoopbackFrameListener listener(dispatcher);
+
+ ASSERT_LE(0, sender.send(reinterpret_cast<const uint8_t*>("123"), 3, tsMono(1000), tsMono(0),
+ uavcan::TransferTypeMessageBroadcast, 0));
+
+ ASSERT_EQ(0, listener.count);
+ ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
+ ASSERT_EQ(1, listener.count);
+ ASSERT_EQ(1, listener.last_frame.getIfaceIndex());
+ ASSERT_EQ(3, listener.last_frame.getPayloadLen());
+ ASSERT_TRUE(TX_NODE_ID == listener.last_frame.getSrcNodeID());
+ ASSERT_TRUE(listener.last_frame.isEndOfTransfer());
+
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount());
+ EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount());
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
+}
+
+TEST(TransferSender, PassiveMode)
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> poolmgr;
+
+ SystemClockMock clockmock(100);
+ CanDriverMock driver(2, clockmock);
+
+ uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock);
+
+ uavcan::TransferSender sender(dispatcher, makeDataType(uavcan::DataTypeKindMessage, 123),
+ uavcan::CanTxQueue::Volatile);
+
+ static const uint8_t Payload[] = {1, 2, 3, 4, 5};
+
+ // By default, sending in passive mode is not enabled
+ ASSERT_EQ(-uavcan::ErrPassiveMode,
+ sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
+ uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast));
+
+ // Overriding the default
+ sender.allowAnonymousTransfers();
+
+ // OK, now we can broadcast in any mode
+ ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
+ uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast));
+
+ // ...but not unicast or anything else
+ ASSERT_EQ(-uavcan::ErrPassiveMode,
+ sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
+ uavcan::TransferTypeServiceRequest, uavcan::NodeID(42)));
+
+ // Making sure the abort flag is set
+ ASSERT_FALSE(driver.ifaces.at(0).tx.empty());
+ ASSERT_EQ(uavcan::CanIOFlagAbortOnError, driver.ifaces.at(0).tx.front().flags);
+
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount());
+ EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount());
+ EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer_test_helpers.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "transfer_test_helpers.hpp"
+#include "../clock.hpp"
+
+
+TEST(TransferTestHelpers, Transfer)
+{
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
+
+ uavcan::TransferBufferManager mgr(128, pool);
+ uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast));
+
+ uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0),
+ uavcan::MonotonicTime(), uavcan::UtcTime(), 0);
+ frame.setEndOfTransfer(true);
+ uavcan::MultiFrameIncomingTransfer mfit(tsMono(10), tsUtc(1000), frame, tba);
+
+ // Filling the buffer with data
+ static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!";
+ ASSERT_TRUE(tba.create());
+ ASSERT_EQ(TEST_DATA.length(), tba.access()->write(0, reinterpret_cast<const uint8_t*>(TEST_DATA.c_str()),
+ unsigned(TEST_DATA.length())));
+
+ // Reading back
+ const Transfer transfer(mfit, uavcan::DataTypeDescriptor());
+ ASSERT_EQ(TEST_DATA, transfer.payload);
+}
+
+
+TEST(TransferTestHelpers, MFTSerialization)
+{
+ uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "Foo");
+
+ static const std::string DATA = "To go wrong in one's own way is better than to go right in someone else's.";
+ const Transfer transfer(1, 100000, 10,
+ uavcan::TransferTypeServiceRequest, 2, 42, 127, DATA, type);
+
+ const std::vector<uavcan::RxFrame> ser = serializeTransfer(transfer);
+
+ std::cout << "Serialized transfer:\n";
+ for (std::vector<uavcan::RxFrame>::const_iterator it = ser.begin(); it != ser.end(); ++it)
+ {
+ std::cout << "\t" << it->toString() << "\n";
+ }
+
+ for (std::vector<uavcan::RxFrame>::const_iterator it = ser.begin(); it != ser.end(); ++it)
+ {
+ std::cout << "\t'";
+ for (unsigned i = 0; i < it->getPayloadLen(); i++)
+ {
+ uint8_t ch = it->getPayloadPtr()[i];
+ if (ch < 0x20 || ch > 0x7E)
+ {
+ ch = '.';
+ }
+ std::cout << static_cast<char>(ch);
+ }
+ std::cout << "'\n";
+ }
+ std::cout << std::flush;
+}
+
+
+TEST(TransferTestHelpers, SFTSerialization)
+{
+ uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "Foo");
+
+ {
+ const Transfer transfer(1, 100000, 10,
+ uavcan::TransferTypeMessageBroadcast, 7, 42, 0, "Nvrfrgt", type);
+ const std::vector<uavcan::RxFrame> ser = serializeTransfer(transfer);
+ ASSERT_EQ(1, ser.size());
+ std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n";
+ }
+ {
+ const Transfer transfer(1, 100000, 11,
+ uavcan::TransferTypeServiceRequest, 7, 42, 127, "7-chars", type);
+ const std::vector<uavcan::RxFrame> ser = serializeTransfer(transfer);
+ ASSERT_EQ(1, ser.size());
+ std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n";
+ }
+ {
+ const Transfer transfer(1, 100000, 12,
+ uavcan::TransferTypeMessageBroadcast, 7, 42, 0, "", type);
+ const std::vector<uavcan::RxFrame> ser = serializeTransfer(transfer);
+ ASSERT_EQ(1, ser.size());
+ std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n";
+ }
+ {
+ const Transfer transfer(1, 100000, 13,
+ uavcan::TransferTypeServiceResponse, 7, 42, 127, "", type);
+ const std::vector<uavcan::RxFrame> ser = serializeTransfer(transfer);
+ ASSERT_EQ(1, ser.size());
+ std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n";
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/transport/transfer_test_helpers.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <queue>
+#include <vector>
+#include <gtest/gtest.h>
+#include <uavcan/transport/transfer_listener.hpp>
+
+/**
+ * UAVCAN transfer representation used in various tests.
+ */
+struct Transfer
+{
+ uavcan::MonotonicTime ts_monotonic;
+ uavcan::UtcTime ts_utc;
+ uavcan::TransferPriority priority;
+ uavcan::TransferType transfer_type;
+ uavcan::TransferID transfer_id;
+ uavcan::NodeID src_node_id;
+ uavcan::NodeID dst_node_id;
+ uavcan::DataTypeDescriptor data_type;
+ std::string payload;
+
+ Transfer(const uavcan::IncomingTransfer& tr, const uavcan::DataTypeDescriptor& data_type)
+ : ts_monotonic(tr.getMonotonicTimestamp())
+ , ts_utc(tr.getUtcTimestamp())
+ , priority(tr.getPriority())
+ , transfer_type(tr.getTransferType())
+ , transfer_id(tr.getTransferID())
+ , src_node_id(tr.getSrcNodeID())
+ , dst_node_id() // default is invalid
+ , data_type(data_type)
+ {
+ unsigned offset = 0;
+ while (true)
+ {
+ uint8_t buf[256];
+ int res = tr.read(offset, buf, sizeof(buf));
+ if (res < 0)
+ {
+ std::cout << "IncomingTransferContainer: read failure " << res << std::endl;
+ exit(1);
+ }
+ if (res == 0)
+ {
+ break;
+ }
+ payload += std::string(reinterpret_cast<const char*>(buf), unsigned(res));
+ offset += unsigned(res);
+ }
+ }
+
+ Transfer(uavcan::MonotonicTime ts_monotonic, uavcan::UtcTime ts_utc, uavcan::TransferPriority priority,
+ uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id,
+ uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
+ : ts_monotonic(ts_monotonic)
+ , ts_utc(ts_utc)
+ , priority(priority)
+ , transfer_type(transfer_type)
+ , transfer_id(transfer_id)
+ , src_node_id(src_node_id)
+ , dst_node_id(dst_node_id)
+ , data_type(data_type)
+ , payload(payload)
+ { }
+
+ Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferPriority priority,
+ uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id,
+ uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type)
+ : ts_monotonic(uavcan::MonotonicTime::fromUSec(ts_monotonic))
+ , ts_utc(uavcan::UtcTime::fromUSec(ts_utc))
+ , priority(priority)
+ , transfer_type(transfer_type)
+ , transfer_id(transfer_id)
+ , src_node_id(src_node_id)
+ , dst_node_id(dst_node_id)
+ , data_type(data_type)
+ , payload(payload)
+ { }
+
+ bool operator==(const Transfer& rhs) const
+ {
+ return
+ (ts_monotonic == rhs.ts_monotonic) &&
+ ((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) &&
+ (priority == rhs.priority) &&
+ (transfer_type == rhs.transfer_type) &&
+ (transfer_id == rhs.transfer_id) &&
+ (src_node_id == rhs.src_node_id) &&
+ ((dst_node_id.isValid() && rhs.dst_node_id.isValid()) ? (dst_node_id == rhs.dst_node_id) : true) &&
+ (data_type == rhs.data_type) &&
+ (payload == rhs.payload);
+ }
+
+ std::string toString() const
+ {
+ std::ostringstream os;
+ os << "ts_m=" << ts_monotonic
+ << " ts_utc=" << ts_utc
+ << " prio=" << int(priority.get())
+ << " tt=" << int(transfer_type)
+ << " tid=" << int(transfer_id.get())
+ << " snid=" << int(src_node_id.get())
+ << " dnid=" << int(dst_node_id.get())
+ << " dtid=" << int(data_type.getID().get())
+ << "\n\t'" << payload << "'";
+ return os.str();
+ }
+};
+
+/**
+ * This subscriber accepts any types of transfers - this makes testing easier.
+ * In reality, uavcan::TransferListener should accept only specific transfer types
+ * which are dispatched/filtered by uavcan::Dispatcher.
+ */
+class TestListener : public uavcan::TransferListener
+{
+ typedef uavcan::TransferListener Base;
+
+ std::queue<Transfer> transfers_;
+
+public:
+ TestListener(uavcan::TransferPerfCounter& perf, const uavcan::DataTypeDescriptor& data_type,
+ uavcan::uint16_t max_buffer_size, uavcan::IPoolAllocator& allocator)
+ : Base(perf, data_type, max_buffer_size, allocator)
+ { }
+
+ void handleIncomingTransfer(uavcan::IncomingTransfer& transfer)
+ {
+ const Transfer rx(transfer, Base::getDataTypeDescriptor());
+ transfers_.push(rx);
+ std::cout << "Received transfer: " << rx.toString() << std::endl;
+
+ const bool single_frame = dynamic_cast<uavcan::SingleFrameIncomingTransfer*>(&transfer) != UAVCAN_NULLPTR;
+
+ const bool anonymous = single_frame &&
+ transfer.getSrcNodeID().isBroadcast() &&
+ (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast);
+
+ ASSERT_EQ(anonymous, transfer.isAnonymousTransfer());
+ }
+
+ bool matchAndPop(const Transfer& reference)
+ {
+ if (transfers_.empty())
+ {
+ std::cout << "No received transfers" << std::endl;
+ return false;
+ }
+
+ const Transfer tr = transfers_.front();
+ transfers_.pop();
+
+ const bool res = (tr == reference);
+ if (!res)
+ {
+ std::cout << "TestSubscriber: Transfer mismatch:\n"
+ << "Expected: " << reference.toString() << "\n"
+ << "Received: " << tr.toString() << std::endl;
+ }
+ return res;
+ }
+
+ unsigned getNumReceivedTransfers() const { return static_cast<unsigned>(transfers_.size()); }
+ bool isEmpty() const { return transfers_.empty(); }
+};
+
+
+namespace
+{
+
+std::vector<uavcan::RxFrame> serializeTransfer(const Transfer& transfer)
+{
+ const bool need_crc = transfer.payload.length() > (sizeof(uavcan::CanFrame::data) - 1);
+
+ std::vector<uint8_t> raw_payload;
+ if (need_crc)
+ {
+ uavcan::TransferCRC payload_crc = transfer.data_type.getSignature().toTransferCRC();
+ payload_crc.add(reinterpret_cast<const uint8_t*>(transfer.payload.c_str()), uint16_t(transfer.payload.length()));
+ // Little endian
+ raw_payload.push_back(uint8_t(payload_crc.get() & 0xFF));
+ raw_payload.push_back(uint8_t((payload_crc.get() >> 8) & 0xFF));
+ }
+ raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end());
+
+ std::vector<uavcan::RxFrame> output;
+ unsigned offset = 0;
+ uavcan::MonotonicTime ts_monotonic = transfer.ts_monotonic;
+ uavcan::UtcTime ts_utc = transfer.ts_utc;
+
+ uavcan::Frame frm(transfer.data_type.getID(), transfer.transfer_type, transfer.src_node_id,
+ transfer.dst_node_id, transfer.transfer_id);
+ frm.setStartOfTransfer(true);
+ frm.setPriority(transfer.priority);
+
+ while (true)
+ {
+ const int bytes_left = int(raw_payload.size()) - int(offset);
+ EXPECT_TRUE(bytes_left >= 0);
+
+ const int spres = frm.setPayload(&*(raw_payload.begin() + offset), unsigned(bytes_left));
+ if (spres < 0)
+ {
+ std::cerr << ">_<" << std::endl;
+ std::exit(1);
+ }
+ if (spres == bytes_left)
+ {
+ frm.setEndOfTransfer(true);
+ }
+
+ offset += unsigned(spres);
+
+ const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);
+ ts_monotonic += uavcan::MonotonicDuration::fromUSec(1);
+ ts_utc += uavcan::UtcDuration::fromUSec(1);
+
+ output.push_back(rxfrm);
+ if (frm.isEndOfTransfer())
+ {
+ break;
+ }
+
+ frm.setStartOfTransfer(false);
+ frm.flipToggle();
+ }
+ return output;
+}
+
+inline uavcan::DataTypeDescriptor makeDataType(uavcan::DataTypeKind kind, uint16_t id, const char* name = "")
+{
+ const uavcan::DataTypeSignature signature((uint64_t(kind) << 16) | uint16_t(id << 8) | uint16_t(id & 0xFF));
+ return uavcan::DataTypeDescriptor(kind, id, signature, name);
+}
+
+}
+
+
+class IncomingTransferEmulatorBase
+{
+ uavcan::MonotonicTime ts_;
+ uavcan::TransferID tid_;
+ uavcan::NodeID dst_node_id_;
+
+public:
+ IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id)
+ : dst_node_id_(dst_node_id)
+ { }
+
+ virtual ~IncomingTransferEmulatorBase() { }
+
+ Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type,
+ uint8_t source_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& type,
+ uavcan::NodeID dst_node_id_override = uavcan::NodeID())
+ {
+ ts_ += uavcan::MonotonicDuration::fromUSec(100);
+ const uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(ts_.toUSec() + 1000000000ul);
+ const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast) ?
+ uavcan::NodeID::Broadcast :
+ (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_);
+ const Transfer tr(ts_, utc, priority, transfer_type, tid_, source_node_id, dst_node_id, payload, type);
+ tid_.increment();
+ return tr;
+ }
+
+ virtual void sendOneFrame(const uavcan::RxFrame& frame) = 0;
+
+ void send(const std::vector<std::vector<uavcan::RxFrame> >& sers)
+ {
+ unsigned index = 0;
+ while (true)
+ {
+ // Sending all transfers concurrently
+ bool all_empty = true;
+ for (std::vector<std::vector<uavcan::RxFrame> >::const_iterator it = sers.begin(); it != sers.end(); ++it)
+ {
+ if (it->size() <= index)
+ {
+ continue;
+ }
+ all_empty = false;
+ std::cout << "Incoming Transfer Emulator: Sending: " << it->at(index).toString() << std::endl;
+ sendOneFrame(it->at(index));
+ }
+ index++;
+ if (all_empty)
+ {
+ break;
+ }
+ }
+ }
+
+ void send(const Transfer* transfers, unsigned num_transfers)
+ {
+ std::vector<std::vector<uavcan::RxFrame> > sers;
+ while (num_transfers--)
+ {
+ sers.push_back(serializeTransfer(*transfers++));
+ }
+ send(sers);
+ }
+
+ template <int SIZE> void send(const Transfer (&transfers)[SIZE]) { send(transfers, SIZE); }
+};
+
+/**
+ * Zero allocator - always fails
+ */
+class NullAllocator : public uavcan::IPoolAllocator
+{
+public:
+ virtual void* allocate(std::size_t) { return UAVCAN_NULLPTR; }
+ virtual void deallocate(const void*) { }
+ virtual uint16_t getBlockCapacity() const { return 0; }
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/util/comparison.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/util/comparison.hpp>
+
+TEST(Comparison, Basic)
+{
+ // Basic same type floats
+ ASSERT_TRUE(uavcan::areClose(0.1, 0.1));
+ ASSERT_TRUE(uavcan::areClose(0.1F, 0.1F));
+ ASSERT_TRUE(uavcan::areClose(0.1L, 0.1L));
+
+ // Basic mixed type floats
+ ASSERT_TRUE(uavcan::areClose(0.1F, 0.1));
+ ASSERT_TRUE(uavcan::areClose(0.1, 0.1F));
+ ASSERT_TRUE(uavcan::areClose(0.1F, 0.1L));
+ ASSERT_TRUE(uavcan::areClose(0.1L, 0.1F));
+ ASSERT_TRUE(uavcan::areClose(0.1, 0.1L));
+ ASSERT_TRUE(uavcan::areClose(0.1L, 0.1));
+
+ // Basic floats
+ ASSERT_TRUE(uavcan::areClose(0x07, '\x07'));
+ ASSERT_TRUE(uavcan::areClose(123456789LL, 123456789));
+ ASSERT_TRUE(uavcan::areClose("123", std::string("123")));
+
+ // Non-equality
+ ASSERT_FALSE(uavcan::areClose(-0.1, 0.1));
+ ASSERT_FALSE(uavcan::areClose(-0.1F, 0.0L));
+ ASSERT_FALSE(uavcan::areClose("123", std::string("12")));
+ ASSERT_FALSE(uavcan::areClose(0x07L, '\0'));
+}
+
+TEST(Comparison, FloatSpecialCase)
+{
+ ASSERT_FALSE(uavcan::areClose(0.1, std::numeric_limits<double>::infinity()));
+
+ ASSERT_TRUE(uavcan::areClose(std::numeric_limits<float>::infinity(),
+ std::numeric_limits<long double>::infinity()));
+
+ ASSERT_FALSE(uavcan::areClose(std::numeric_limits<float>::infinity(), -std::numeric_limits<float>::infinity()));
+
+ ASSERT_FALSE(uavcan::areClose(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN()));
+}
+
+TEST(Comparison, FloatULP)
+{
+ ASSERT_FALSE(uavcan::areClose(0.100000000000000001L, 0.1L));
+ ASSERT_TRUE( uavcan::areClose(0.100000000000000001, 0.1L));
+ ASSERT_TRUE( uavcan::areClose(0.100000000000000001F, 0.1L));
+
+ // Near zero
+ ASSERT_TRUE( uavcan::areClose(0.0F, std::numeric_limits<float>::epsilon()));
+ ASSERT_TRUE( uavcan::areClose(0.0F, -std::numeric_limits<float>::epsilon()));
+ ASSERT_FALSE(uavcan::areClose(0.0F, std::numeric_limits<float>::epsilon() * 2));
+}
+
+TEST(Comparison, BruteforceValidation)
+{
+ const std::streamsize default_precision = std::cout.precision();
+ std::cout.precision(20);
+
+ float x = -uavcan::NumericTraits<float>::max();
+
+ while (x < uavcan::NumericTraits<float>::max())
+ {
+ const float y1 = x + std::abs(x) * (uavcan::NumericTraits<float>::epsilon() * 2); // Still equal
+ const float y2 = x + uavcan::max(std::abs(x), 1.F) * (uavcan::NumericTraits<float>::epsilon() * 20); // Nope
+
+ if (!uavcan::areClose(y1, x))
+ {
+ std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl;
+ ASSERT_TRUE(false);
+ }
+ if (uavcan::areClose(y2, x))
+ {
+ std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl;
+ ASSERT_TRUE(false);
+ }
+
+ x = y2;
+ }
+
+ std::cout.precision(default_precision);
+}
+
+
+struct B
+{
+ long double b;
+ B(long double val = 0.0L) : b(val) { }
+};
+
+struct A
+{
+ float a;
+ explicit A(float val = 0.0F) : a(val) { }
+
+ bool isClose(A rhs) const
+ {
+ std::cout << "bool A::isClose(A) --> " << rhs.a << std::endl;
+ return uavcan::areClose(a, rhs.a);
+ }
+
+ bool isClose(const B& rhs) const
+ {
+ std::cout << "bool A::isClose(const B&) --> " << rhs.b << std::endl;
+ return uavcan::areClose(a, rhs.b);
+ }
+};
+
+struct C
+{
+ long long c;
+ explicit C(long long val = 0.0L) : c(val) { }
+
+ bool operator==(B rhs) const
+ {
+ std::cout << "bool C::operator==(B) --> " << rhs.b << std::endl;
+ return c == static_cast<long long>(rhs.b);
+ }
+};
+
+TEST(Comparison, IsCloseMethod)
+{
+ B b;
+ A a;
+ C c;
+
+ std::cout << 1 << std::endl;
+ ASSERT_TRUE(uavcan::areClose(a, b)); // Fuzzy
+ ASSERT_TRUE(uavcan::areClose(a, A())); // Fuzzy
+ ASSERT_TRUE(uavcan::areClose(b, a)); // Fuzzy, reverse application
+ ASSERT_TRUE(uavcan::areClose(c, b)); // Exact
+
+ std::cout << 2 << std::endl;
+
+ a.a = uavcan::NumericTraits<float>::epsilon();
+
+ ASSERT_TRUE(uavcan::areClose(a, b));
+ ASSERT_TRUE(uavcan::areClose(b, a));
+ ASSERT_TRUE(a.isClose(b));
+ ASSERT_TRUE(a.isClose(A()));
+ ASSERT_TRUE(uavcan::areClose(A(), a));
+
+ std::cout << 3 << std::endl;
+
+ a.a = 1e-5F;
+
+ ASSERT_FALSE(uavcan::areClose(a, b));
+ ASSERT_FALSE(uavcan::areClose(b, a));
+ ASSERT_FALSE(uavcan::areClose(A(), a));
+
+ std::cout << 4 << std::endl;
+
+ b.b = 1.1L;
+ c.c = 1;
+
+ ASSERT_TRUE(uavcan::areClose(c, b)); // Round to integer
+ ASSERT_TRUE(uavcan::areClose(c, 1.0L)); // Implicit cast to B
+ ASSERT_FALSE(uavcan::areClose(c, 0.0L));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/util/lazy_constructor.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/util/lazy_constructor.hpp>
+
+
+TEST(LazyConstructor, Basic)
+{
+ using ::uavcan::LazyConstructor;
+
+ LazyConstructor<std::string> a;
+ LazyConstructor<std::string> b;
+
+ ASSERT_FALSE(a);
+ ASSERT_FALSE(b.isConstructed());
+
+ /*
+ * Construction
+ */
+ a.destroy(); // no-op
+ a.construct();
+ b.construct<const char*>("Hello world");
+
+ ASSERT_TRUE(a);
+ ASSERT_TRUE(b.isConstructed());
+
+ ASSERT_NE(*a, *b);
+ ASSERT_STRNE(a->c_str(), b->c_str());
+
+ ASSERT_EQ(*a, "");
+ ASSERT_EQ(*b, "Hello world");
+
+ /*
+ * Copying
+ */
+ a = b; // Assignment operator performs destruction and immediate copy construction
+ ASSERT_EQ(*a, *b);
+ ASSERT_EQ(*a, "Hello world");
+
+ LazyConstructor<std::string> c(a); // Copy constructor call is forwarded to std::string
+
+ ASSERT_EQ(*c, *a);
+
+ *a = "123";
+ ASSERT_NE(*c, *a);
+ ASSERT_EQ(*c, *b);
+
+ *c = "456";
+ ASSERT_NE(*a, *c);
+ ASSERT_NE(*b, *a);
+ ASSERT_NE(*c, *b);
+
+ /*
+ * Destruction
+ */
+ ASSERT_TRUE(c);
+ c.destroy();
+ ASSERT_FALSE(c);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/util/linked_list.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,145 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/util/linked_list.hpp>
+
+struct ListItem : uavcan::LinkedListNode<ListItem>
+{
+ int value;
+
+ ListItem(int value = 0)
+ : value(value)
+ { }
+
+ struct GreaterThanComparator
+ {
+ const int compare_with;
+
+ GreaterThanComparator(int compare_with)
+ : compare_with(compare_with)
+ { }
+
+ bool operator()(const ListItem* item) const
+ {
+ return item->value > compare_with;
+ }
+ };
+
+ void insort(uavcan::LinkedListRoot<ListItem>& root)
+ {
+ root.insertBefore(this, GreaterThanComparator(value));
+ }
+};
+
+TEST(LinkedList, Basic)
+{
+ uavcan::LinkedListRoot<ListItem> root;
+
+ /*
+ * Insert/remove
+ */
+ EXPECT_EQ(0, root.getLength());
+
+ ListItem item1;
+ root.insert(&item1);
+ root.insert(&item1); // Insert twice - second will be ignored
+ EXPECT_EQ(1, root.getLength());
+
+ root.remove(&item1);
+ root.remove(&item1);
+ EXPECT_EQ(0, root.getLength());
+
+ ListItem items[3];
+ root.insert(&item1);
+ root.insert(items + 0);
+ root.insert(items + 1);
+ root.insert(items + 2);
+ EXPECT_EQ(4, root.getLength());
+
+ /*
+ * Order persistence
+ */
+ items[0].value = 10;
+ items[1].value = 11;
+ items[2].value = 12;
+ const int expected_values[] = {12, 11, 10, 0};
+ ListItem* node = root.get();
+ for (int i = 0; i < 4; i++)
+ {
+ EXPECT_EQ(expected_values[i], node->value);
+ node = node->getNextListNode();
+ }
+
+ root.remove(items + 0);
+ root.remove(items + 2);
+ root.remove(items + 2);
+ EXPECT_EQ(2, root.getLength());
+
+ const int expected_values2[] = {11, 0};
+ node = root.get();
+ for (int i = 0; i < 2; i++)
+ {
+ EXPECT_EQ(expected_values2[i], node->value);
+ node = node->getNextListNode();
+ }
+
+ root.insert(items + 2);
+ EXPECT_EQ(3, root.getLength());
+ EXPECT_EQ(12, root.get()->value);
+
+ /*
+ * Emptying
+ */
+ root.remove(&item1);
+ root.remove(items + 0);
+ root.remove(items + 1);
+ root.remove(items + 2);
+ EXPECT_EQ(0, root.getLength());
+}
+
+TEST(LinkedList, Sorting)
+{
+ uavcan::LinkedListRoot<ListItem> root;
+ ListItem items[6];
+ for (int i = 0; i < 6; i++)
+ {
+ items[i].value = i;
+ }
+
+ EXPECT_EQ(0, root.getLength());
+
+ items[2].insort(root);
+ EXPECT_EQ(1, root.getLength());
+
+ items[2].insort(root); // Making sure the duplicate will be removed
+ EXPECT_EQ(1, root.getLength());
+
+ items[3].insort(root);
+
+ items[0].insort(root);
+
+ items[4].insort(root);
+ EXPECT_EQ(4, root.getLength());
+
+ items[1].insort(root);
+ EXPECT_EQ(5, root.getLength());
+
+ items[1].insort(root); // Another duplicate
+ EXPECT_EQ(5, root.getLength());
+
+ items[5].insort(root);
+
+ EXPECT_EQ(6, root.getLength());
+
+ int prev_val = -100500;
+ const ListItem* item = root.get();
+ while (item)
+ {
+ //std::cout << item->value << std::endl;
+ EXPECT_LT(prev_val, item->value);
+ prev_val = item->value;
+ item = item->getNextListNode();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/util/map.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <string>
+#include <cstdio>
+#include <memory>
+#include <gtest/gtest.h>
+#include <uavcan/util/map.hpp>
+
+
+/*
+ * TODO: This one test has been temporarily disabled because it is not compatible with newer versions of libstdc++
+ * that ship with newer versions of GCC. The problem is that std::string has become too large to fit into a 64-byte
+ * large memory block. This should be fixed in the future.
+ */
+#if 0
+static std::string toString(long x)
+{
+ char buf[80];
+ std::snprintf(buf, sizeof(buf), "%li", x);
+ return std::string(buf);
+}
+
+static bool oddValuePredicate(const std::string& key, const std::string& value)
+{
+ EXPECT_FALSE(key.empty());
+ EXPECT_FALSE(value.empty());
+ const int num = atoi(value.c_str());
+ return num & 1;
+}
+
+struct KeyFindPredicate
+{
+ const std::string target;
+ KeyFindPredicate(std::string target) : target(target) { }
+ bool operator()(const std::string& key, const std::string&) const { return key == target; }
+};
+
+struct ValueFindPredicate
+{
+ const std::string target;
+ ValueFindPredicate(std::string target) : target(target) { }
+ bool operator()(const std::string&, const std::string& value) const { return value == target; }
+};
+
+
+TEST(Map, Basic)
+{
+ using uavcan::Map;
+
+ static const int POOL_BLOCKS = 3;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ typedef Map<std::string, std::string> MapType;
+ std::auto_ptr<MapType> map(new MapType(pool));
+
+ // Empty
+ ASSERT_FALSE(map->access("hi"));
+ map->remove("foo");
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+ ASSERT_FALSE(map->getByIndex(0));
+ ASSERT_FALSE(map->getByIndex(1));
+ ASSERT_FALSE(map->getByIndex(10000));
+
+ // Insertion
+ ASSERT_EQ("a", *map->insert("1", "a"));
+ ASSERT_EQ("b", *map->insert("2", "b"));
+ ASSERT_EQ(1, pool.getNumUsedBlocks());
+ ASSERT_EQ(2, map->getSize());
+
+ // Ordering
+ ASSERT_TRUE(map->getByIndex(0)->match("1"));
+ ASSERT_TRUE(map->getByIndex(1)->match("2"));
+
+ // Insertion
+ ASSERT_EQ("c", *map->insert("3", "c"));
+ ASSERT_EQ(1, pool.getNumUsedBlocks());
+
+ ASSERT_EQ("d", *map->insert("4", "d"));
+ ASSERT_EQ(2, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block
+ ASSERT_EQ(4, map->getSize());
+
+ // Making sure everything is here
+ ASSERT_EQ("a", *map->access("1"));
+ ASSERT_EQ("b", *map->access("2"));
+ ASSERT_EQ("c", *map->access("3"));
+ ASSERT_EQ("d", *map->access("4"));
+ ASSERT_FALSE(map->access("hi"));
+
+ // Modifying existing entries
+ *map->access("1") = "A";
+ *map->access("2") = "B";
+ *map->access("3") = "C";
+ *map->access("4") = "D";
+ ASSERT_EQ("A", *map->access("1"));
+ ASSERT_EQ("B", *map->access("2"));
+ ASSERT_EQ("C", *map->access("3"));
+ ASSERT_EQ("D", *map->access("4"));
+
+ // Finding some keys
+ ASSERT_EQ("1", *map->find(KeyFindPredicate("1")));
+ ASSERT_EQ("2", *map->find(KeyFindPredicate("2")));
+ ASSERT_EQ("3", *map->find(KeyFindPredicate("3")));
+ ASSERT_EQ("4", *map->find(KeyFindPredicate("4")));
+ ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_key")));
+
+ // Finding some values
+ ASSERT_EQ("1", *map->find(ValueFindPredicate("A")));
+ ASSERT_EQ("2", *map->find(ValueFindPredicate("B")));
+ ASSERT_EQ("3", *map->find(ValueFindPredicate("C")));
+ ASSERT_EQ("4", *map->find(ValueFindPredicate("D")));
+ ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_value")));
+
+ // Removing one
+ map->remove("1"); // One of dynamics now migrates to the static storage
+ map->remove("foo"); // There's no such thing anyway
+ ASSERT_EQ(2, pool.getNumUsedBlocks());
+ ASSERT_EQ(3, map->getSize());
+
+ ASSERT_FALSE(map->access("1"));
+ ASSERT_EQ("B", *map->access("2"));
+ ASSERT_EQ("C", *map->access("3"));
+ ASSERT_EQ("D", *map->access("4"));
+
+ // Removing another
+ map->remove("2");
+ ASSERT_EQ(2, map->getSize());
+ ASSERT_EQ(2, pool.getNumUsedBlocks());
+
+ ASSERT_FALSE(map->access("1"));
+ ASSERT_FALSE(map->access("2"));
+ ASSERT_EQ("C", *map->access("3"));
+ ASSERT_EQ("D", *map->access("4"));
+
+ // Adding some new
+ unsigned max_key_integer = 0;
+ for (int i = 0; i < 100; i++)
+ {
+ const std::string key = toString(i);
+ const std::string value = toString(i);
+ std::string* res = map->insert(key, value); // Will override some from the above
+ if (res == UAVCAN_NULLPTR)
+ {
+ ASSERT_LT(2, i);
+ break;
+ }
+ else
+ {
+ ASSERT_EQ(value, *res);
+ }
+ max_key_integer = unsigned(i);
+ }
+ std::cout << "Max key/value: " << max_key_integer << std::endl;
+ ASSERT_LT(4, max_key_integer);
+
+ // Making sure there is true OOM
+ ASSERT_EQ(0, pool.getNumFreeBlocks());
+ ASSERT_FALSE(map->insert("nonexistent", "value"));
+ ASSERT_FALSE(map->access("nonexistent"));
+ ASSERT_FALSE(map->access("value"));
+
+ // Removing odd values - nearly half of them
+ map->removeAllWhere(oddValuePredicate);
+
+ // Making sure there's no odd values left
+ for (unsigned kv_int = 0; kv_int <= max_key_integer; kv_int++)
+ {
+ const std::string* val = map->access(toString(kv_int));
+ if (val)
+ {
+ ASSERT_FALSE(kv_int & 1);
+ }
+ else
+ {
+ ASSERT_TRUE(kv_int & 1);
+ }
+ }
+
+ // Making sure the memory will be released
+ map.reset();
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+}
+#endif
+
+
+TEST(Map, PrimitiveKey)
+{
+ using uavcan::Map;
+
+ static const int POOL_BLOCKS = 3;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ typedef Map<short, short> MapType;
+ std::auto_ptr<MapType> map(new MapType(pool));
+
+ // Empty
+ ASSERT_FALSE(map->access(1));
+ map->remove(8);
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+ ASSERT_EQ(0, map->getSize());
+ ASSERT_FALSE(map->getByIndex(0));
+
+ // Insertion
+ ASSERT_EQ(1, *map->insert(1, 1));
+ ASSERT_EQ(1, map->getSize());
+ ASSERT_EQ(2, *map->insert(2, 2));
+ ASSERT_EQ(2, map->getSize());
+ ASSERT_EQ(3, *map->insert(3, 3));
+ ASSERT_EQ(4, *map->insert(4, 4));
+ ASSERT_EQ(4, map->getSize());
+
+ // Ordering
+ ASSERT_TRUE(map->getByIndex(0)->match(1));
+ ASSERT_TRUE(map->getByIndex(1)->match(2));
+ ASSERT_TRUE(map->getByIndex(2)->match(3));
+ ASSERT_TRUE(map->getByIndex(3)->match(4));
+ ASSERT_FALSE(map->getByIndex(5));
+ ASSERT_FALSE(map->getByIndex(1000));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/util/multiset.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#include <string>
+#include <cstdio>
+#include <memory>
+#include <gtest/gtest.h>
+#include <uavcan/util/multiset.hpp>
+
+
+static std::string toString(long x)
+{
+ char buf[80];
+ std::snprintf(buf, sizeof(buf), "%li", x);
+ return std::string(buf);
+}
+
+static bool oddValuePredicate(const std::string& value)
+{
+ EXPECT_FALSE(value.empty());
+ const int num = atoi(value.c_str());
+ return num & 1;
+}
+
+struct FindPredicate
+{
+ const std::string target;
+ FindPredicate(const std::string& target) : target(target) { }
+ bool operator()(const std::string& value) const { return value == target; }
+};
+
+struct NoncopyableWithCounter : uavcan::Noncopyable
+{
+ static int num_objects;
+ long long value;
+
+ NoncopyableWithCounter() : value(0) { num_objects++; }
+ NoncopyableWithCounter(long long x) : value(x) { num_objects++; }
+ ~NoncopyableWithCounter() { num_objects--; }
+
+ static bool isNegative(const NoncopyableWithCounter& val)
+ {
+ return val.value < 0;
+ }
+
+ bool operator==(const NoncopyableWithCounter& ref) const { return ref.value == value; }
+};
+
+int NoncopyableWithCounter::num_objects = 0;
+
+template <typename T>
+struct SummationOperator : uavcan::Noncopyable
+{
+ T accumulator;
+ SummationOperator() : accumulator() { }
+ void operator()(const T& x) { accumulator += x; }
+};
+
+struct ClearingOperator
+{
+ template <typename T>
+ void operator()(T& x) const { x = T(); }
+};
+
+
+TEST(Multiset, Basic)
+{
+ using uavcan::Multiset;
+
+ static const int POOL_BLOCKS = 4;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ typedef Multiset<std::string> MultisetType;
+ std::auto_ptr<MultisetType> mset(new MultisetType(pool));
+
+ typedef SummationOperator<std::string> StringConcatenationOperator;
+
+ // Empty
+ mset->removeFirst("foo");
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+ ASSERT_FALSE(mset->getByIndex(0));
+ ASSERT_FALSE(mset->getByIndex(1));
+ ASSERT_FALSE(mset->getByIndex(10000));
+
+ // Static addion
+ ASSERT_EQ("1", *mset->emplace("1"));
+ ASSERT_EQ("2", *mset->emplace("2"));
+ ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more
+ ASSERT_EQ(2, mset->getSize());
+
+ {
+ StringConcatenationOperator op;
+ mset->forEach<StringConcatenationOperator&>(op);
+ ASSERT_EQ(2, op.accumulator.size());
+ }
+
+ // Dynamic addition
+ ASSERT_EQ("3", *mset->emplace("3"));
+ ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more
+
+ ASSERT_EQ("4", *mset->emplace("4"));
+ ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more
+ ASSERT_EQ(4, mset->getSize());
+
+ ASSERT_FALSE(mset->getByIndex(100));
+ ASSERT_FALSE(mset->getByIndex(4));
+
+ // Finding some items
+ ASSERT_EQ("1", *mset->find(FindPredicate("1")));
+ ASSERT_EQ("2", *mset->find(FindPredicate("2")));
+ ASSERT_EQ("3", *mset->find(FindPredicate("3")));
+ ASSERT_EQ("4", *mset->find(FindPredicate("4")));
+ ASSERT_FALSE(mset->find(FindPredicate("nonexistent")));
+
+ {
+ StringConcatenationOperator op;
+ mset->forEach<StringConcatenationOperator&>(op);
+ std::cout << "Accumulator: " << op.accumulator << std::endl;
+ ASSERT_EQ(4, op.accumulator.size());
+ }
+
+ // Removing some
+ mset->removeFirst("1");
+ mset->removeFirst("foo"); // There's no such thing anyway
+ mset->removeFirst("2");
+
+ // Adding some new items
+ unsigned max_value_integer = 0;
+ for (int i = 0; i < 100; i++)
+ {
+ const std::string value = toString(i);
+ std::string* res = mset->emplace(value); // Will NOT override above
+ if (res == UAVCAN_NULLPTR)
+ {
+ ASSERT_LT(1, i);
+ break;
+ }
+ else
+ {
+ ASSERT_EQ(value, *res);
+ }
+ max_value_integer = unsigned(i);
+ }
+ std::cout << "Max value: " << max_value_integer << std::endl;
+
+ // Making sure there is true OOM
+ ASSERT_EQ(0, pool.getNumFreeBlocks());
+ ASSERT_FALSE(mset->emplace("nonexistent"));
+
+ // Removing odd values - nearly half of them
+ mset->removeAllWhere(oddValuePredicate);
+
+ // Making sure there's no odd values left
+ for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++)
+ {
+ const std::string* val = mset->find(FindPredicate(toString(kv_int)));
+ if (val)
+ {
+ ASSERT_FALSE(kv_int & 1);
+ }
+ else
+ {
+ ASSERT_TRUE(kv_int & 1);
+ }
+ }
+
+ // Clearing all strings
+ {
+ StringConcatenationOperator op;
+ mset->forEach<StringConcatenationOperator&>(op);
+ std::cout << "Accumulator before clearing: " << op.accumulator << std::endl;
+ }
+ mset->forEach(ClearingOperator());
+ {
+ StringConcatenationOperator op;
+ mset->forEach<StringConcatenationOperator&>(op);
+ std::cout << "Accumulator after clearing: " << op.accumulator << std::endl;
+ ASSERT_TRUE(op.accumulator.empty());
+ }
+
+ // Making sure the memory will be released
+ mset.reset();
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+}
+
+
+TEST(Multiset, PrimitiveKey)
+{
+ using uavcan::Multiset;
+
+ static const int POOL_BLOCKS = 3;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ typedef Multiset<int> MultisetType;
+ std::auto_ptr<MultisetType> mset(new MultisetType(pool));
+
+ // Empty
+ mset->removeFirst(8);
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+ ASSERT_EQ(0, mset->getSize());
+ ASSERT_FALSE(mset->getByIndex(0));
+
+ // Insertion
+ ASSERT_EQ(1, *mset->emplace(1));
+ ASSERT_EQ(1, mset->getSize());
+ ASSERT_EQ(2, *mset->emplace(2));
+ ASSERT_EQ(2, mset->getSize());
+ ASSERT_EQ(3, *mset->emplace(3));
+ ASSERT_EQ(4, *mset->emplace(4));
+ ASSERT_EQ(4, mset->getSize());
+
+ // Summation and clearing
+ {
+ SummationOperator<int> summation_operator;
+ mset->forEach<SummationOperator<int>&>(summation_operator);
+ ASSERT_EQ(1 + 2 + 3 + 4, summation_operator.accumulator);
+ }
+ mset->forEach(ClearingOperator());
+ {
+ SummationOperator<int> summation_operator;
+ mset->forEach<SummationOperator<int>&>(summation_operator);
+ ASSERT_EQ(0, summation_operator.accumulator);
+ }
+}
+
+
+TEST(Multiset, NoncopyableWithCounter)
+{
+ using uavcan::Multiset;
+
+ static const int POOL_BLOCKS = 3;
+ uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+
+ typedef Multiset<NoncopyableWithCounter> MultisetType;
+ std::auto_ptr<MultisetType> mset(new MultisetType(pool));
+
+ ASSERT_EQ(0, NoncopyableWithCounter::num_objects);
+ ASSERT_EQ(0, mset->emplace()->value);
+ ASSERT_EQ(1, NoncopyableWithCounter::num_objects);
+ ASSERT_EQ(123, mset->emplace(123)->value);
+ ASSERT_EQ(2, NoncopyableWithCounter::num_objects);
+ ASSERT_EQ(-456, mset->emplace(-456)->value);
+ ASSERT_EQ(3, NoncopyableWithCounter::num_objects);
+ ASSERT_EQ(456, mset->emplace(456)->value);
+ ASSERT_EQ(4, NoncopyableWithCounter::num_objects);
+ ASSERT_EQ(-789, mset->emplace(-789)->value);
+ ASSERT_EQ(5, NoncopyableWithCounter::num_objects);
+
+ mset->removeFirst(NoncopyableWithCounter(0));
+ ASSERT_EQ(4, NoncopyableWithCounter::num_objects);
+
+ mset->removeFirstWhere(&NoncopyableWithCounter::isNegative);
+ ASSERT_EQ(3, NoncopyableWithCounter::num_objects);
+
+ mset->removeAllWhere(&NoncopyableWithCounter::isNegative);
+ ASSERT_EQ(2, NoncopyableWithCounter::num_objects); // Only 1 and 2 are left
+
+ mset.reset();
+
+ ASSERT_EQ(0, pool.getNumUsedBlocks());
+ ASSERT_EQ(0, NoncopyableWithCounter::num_objects); // All destroyed
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/test/util/templates.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/util/templates.hpp>
+#include <limits>
+
+struct NonConvertible { };
+
+struct ConvertibleToBool
+{
+ const bool value;
+ ConvertibleToBool(bool value) : value(value) { }
+ operator bool() const { return value; }
+ bool operator!() const { return !value; }
+};
+
+struct NonDefaultConstructible
+{
+ NonDefaultConstructible(int) { }
+};
+
+TEST(Util, CoerceOrFallback)
+{
+ using uavcan::coerceOrFallback;
+
+ ASSERT_FALSE(coerceOrFallback<bool>(NonConvertible()));
+ ASSERT_TRUE(coerceOrFallback<bool>(NonConvertible(), true));
+
+ ASSERT_EQ(0, coerceOrFallback<long>(NonConvertible()));
+ ASSERT_EQ(9000, coerceOrFallback<long>(NonConvertible(), 9000));
+
+ ASSERT_TRUE(coerceOrFallback<bool>(ConvertibleToBool(true)));
+ ASSERT_TRUE(coerceOrFallback<bool>(ConvertibleToBool(true), false));
+ ASSERT_FALSE(coerceOrFallback<bool>(ConvertibleToBool(false)));
+ ASSERT_FALSE(coerceOrFallback<bool>(ConvertibleToBool(false), true));
+ ASSERT_EQ(1, coerceOrFallback<long>(ConvertibleToBool(true)));
+ ASSERT_EQ(0, coerceOrFallback<long>(ConvertibleToBool(false), -100));
+
+ //coerceOrFallback<NonDefaultConstructible>(ConvertibleToBool(true)); // Will fail to compile
+ coerceOrFallback<NonDefaultConstructible>(NonConvertible(), NonDefaultConstructible(64));
+}
+
+TEST(Util, FloatClassification)
+{
+ // NAN
+ ASSERT_TRUE(uavcan::isNaN(std::numeric_limits<float>::quiet_NaN()));
+ ASSERT_FALSE(uavcan::isNaN(std::numeric_limits<double>::infinity()));
+ ASSERT_FALSE(uavcan::isNaN(std::numeric_limits<long double>::infinity()));
+ ASSERT_FALSE(uavcan::isNaN(123.456));
+
+ // INF
+ ASSERT_TRUE(uavcan::isInfinity(std::numeric_limits<float>::infinity()));
+ ASSERT_TRUE(uavcan::isInfinity(-std::numeric_limits<long double>::infinity()));
+ ASSERT_FALSE(uavcan::isInfinity(std::numeric_limits<float>::quiet_NaN()));
+ ASSERT_FALSE(uavcan::isInfinity(-0.1L));
+
+ // Signbit
+ ASSERT_FALSE(uavcan::getSignBit(12));
+ ASSERT_TRUE(uavcan::getSignBit(-std::numeric_limits<long double>::infinity()));
+ ASSERT_FALSE(uavcan::getSignBit(std::numeric_limits<float>::infinity()));
+ ASSERT_TRUE(uavcan::getSignBit(-0.0)); // Negative zero
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan/tools/coverity_scan_model.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,45 @@
+/*
+ * Coverity Scan model.
+ *
+ * - A model file can't import any header files.
+ * - Therefore only some built-in primitives like int, char and void are
+ * available but not wchar_t, NULL etc.
+ * - Modeling doesn't need full structs and typedefs. Rudimentary structs
+ * and similar types are sufficient.
+ * - An uninitialized local pointer is not an error. It signifies that the
+ * variable could be either NULL or have some data.
+ *
+ * Coverity Scan doesn't pick up modifications automatically. The model file
+ * must be uploaded by an admin in the analysis settings of
+ * https://scan.coverity.com/projects/1513
+ */
+
+namespace std
+{
+ typedef unsigned long size_t;
+}
+
+namespace uavcan
+{
+
+void handleFatalError(const char* msg)
+{
+ __coverity_panic__();
+}
+
+template <std::size_t PoolSize, std::size_t BlockSize>
+class PoolAllocator
+{
+public:
+ void* allocate(std::size_t size)
+ {
+ return __coverity_alloc__(size);
+ }
+
+ void deallocate(const void* ptr)
+ {
+ __coverity_free__(ptr);
+ }
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/CMakeLists.txt Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,99 @@
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+cmake_minimum_required(VERSION 2.8)
+
+project(libuavcan_linux)
+
+#
+# Library (header only)
+#
+install(DIRECTORY include/uavcan_linux DESTINATION include)
+
+#
+# Scripts
+#
+install(DIRECTORY scripts/
+ USE_SOURCE_PERMISSIONS
+ DESTINATION bin)
+
+#
+# System dependecies
+#
+find_package(Threads REQUIRED)
+
+#
+# Finding libuavcan - it will be a target if we're running from the top-level CMakeLists.txt,
+# otherwise try to find it in the system directories.
+#
+if (TARGET uavcan)
+ message(STATUS "Using uavcan target; source dir: ${libuavcan_SOURCE_DIR}")
+ set(UAVCAN_LIB uavcan)
+ include_directories(${libuavcan_SOURCE_DIR}/include
+ ${libuavcan_SOURCE_DIR}/include/dsdlc_generated)
+ message(STATUS "POSIX source dir: ${libuavcan_posix_SOURCE_DIR}")
+ include_directories(${libuavcan_posix_SOURCE_DIR}/include)
+else ()
+ message(STATUS "Using installed uavcan library")
+ find_library(UAVCAN_LIB uavcan REQUIRED)
+endif ()
+
+#
+# Applications - tests, tools.
+#
+include_directories(include)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -pedantic -std=c++11") # GCC or Clang
+
+if(CMAKE_BUILD_TYPE STREQUAL "Debug")
+ add_definitions(-DUAVCAN_DEBUG=1)
+endif()
+
+#
+# Tests
+# These aren't installed, an average library user should not care about them.
+#
+add_executable(test_clock apps/test_clock.cpp)
+target_link_libraries(test_clock ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_socket apps/test_socket.cpp)
+target_link_libraries(test_socket ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_node apps/test_node.cpp)
+target_link_libraries(test_node ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_time_sync apps/test_time_sync.cpp)
+target_link_libraries(test_time_sync ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_system_utils apps/test_system_utils.cpp)
+target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_posix apps/test_posix.cpp)
+target_link_libraries(test_posix ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_dynamic_node_id_client apps/test_dynamic_node_id_client.cpp)
+target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_file_server apps/test_file_server.cpp)
+target_link_libraries(test_file_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_multithreading apps/test_multithreading.cpp)
+target_link_libraries(test_multithreading ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+#
+# Tools
+#
+add_executable(uavcan_monitor apps/uavcan_monitor.cpp)
+target_link_libraries(uavcan_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(uavcan_nodetool apps/uavcan_nodetool.cpp)
+target_link_libraries(uavcan_nodetool ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(uavcan_dynamic_node_id_server apps/uavcan_dynamic_node_id_server.cpp)
+target_link_libraries(uavcan_dynamic_node_id_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+install(TARGETS uavcan_monitor
+ uavcan_nodetool
+ uavcan_dynamic_node_id_server
+ RUNTIME DESTINATION bin)
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/debug.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <stdexcept>
+
+#ifndef STRINGIZE
+# define STRINGIZE2(x) #x
+# define STRINGIZE(x) STRINGIZE2(x)
+#endif
+#define ENFORCE(x) if (!(x)) { throw std::runtime_error(__FILE__ ":" STRINGIZE(__LINE__) ": " #x); }
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_clock.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <cerrno>
+#include <chrono>
+#include <uavcan_linux/uavcan_linux.hpp>
+
+static std::string systime2str(const std::chrono::system_clock::time_point& tp)
+{
+ const auto tt = std::chrono::system_clock::to_time_t(tp);
+ return std::ctime(&tt);
+}
+
+int main()
+{
+ uavcan_linux::SystemClock clock;
+
+ /*
+ * Auto-detected clock adjustment mode
+ */
+ std::cout << "Clock adjustment mode: ";
+ switch (clock.getAdjustmentMode())
+ {
+ case uavcan_linux::ClockAdjustmentMode::SystemWide:
+ {
+ std::cout << "SystemWide";
+ break;
+ }
+ case uavcan_linux::ClockAdjustmentMode::PerDriverPrivate:
+ {
+ std::cout << "PerDriverPrivate";
+ break;
+ }
+ default:
+ {
+ std::abort();
+ break;
+ }
+ }
+ std::cout << std::endl;
+
+ /*
+ * Test adjustment
+ */
+ double sec = 0;
+ std::cout << "Enter system time adjustment in seconds (fractions allowed): " << std::endl;
+ std::cin >> sec;
+
+ const auto before = std::chrono::system_clock::now();
+ try
+ {
+ clock.adjustUtc(uavcan::UtcDuration::fromUSec(sec * 1e6));
+ }
+ catch (const uavcan_linux::Exception& ex)
+ {
+ std::cout << ex.what() << std::endl;
+ std::cout << strerror(ex.getErrno()) << std::endl;
+ return 1;
+ }
+ const auto after = std::chrono::system_clock::now();
+
+ std::cout << "Time before: " << systime2str(before) << "\n"
+ << "Time after: " << systime2str(after) << "\n"
+ << "Millisecond diff (after - before): "
+ << std::chrono::duration_cast<std::chrono::milliseconds>(after - before).count() << std::endl;
+
+ return 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include "debug.hpp"
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include <uavcan_linux/uavcan_linux.hpp>
+
+namespace
+{
+
+uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector<std::string>& ifaces,
+ const std::uint8_t instance_id,
+ const uavcan::NodeID preferred_node_id,
+ const std::string& name)
+{
+ /*
+ * Initializing the node object
+ */
+ auto node = uavcan_linux::makeNode(ifaces);
+
+ node->setName(name.c_str());
+ node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+ {
+ const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, instance_id);
+
+ uavcan::protocol::HardwareVersion hwver;
+ std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
+ std::cout << hwver << std::endl;
+
+ node->setHardwareVersion(hwver);
+ }
+
+ /*
+ * Starting the node
+ */
+ const int start_res = node->start();
+ ENFORCE(0 == start_res);
+
+ /*
+ * Running the dynamic node ID client until it's done
+ */
+ uavcan::DynamicNodeIDClient client(*node);
+
+ ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion().unique_id, preferred_node_id));
+
+ std::cout << "Waiting for dynamic node ID allocation..." << std::endl;
+
+ while (!client.isAllocationComplete())
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+ if (res < 0)
+ {
+ std::cerr << "Spin error: " << res << std::endl;
+ }
+ }
+
+ std::cout << "Node ID " << int(client.getAllocatedNodeID().get())
+ << " allocated by " << int(client.getAllocatorNodeID().get()) << std::endl;
+
+ /*
+ * Finishing the node initialization
+ */
+ node->setNodeID(client.getAllocatedNodeID());
+
+ node->setModeOperational();
+
+ return node;
+}
+
+void runForever(const uavcan_linux::NodePtr& node)
+{
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+ if (res < 0)
+ {
+ std::cerr << "Spin error: " << res << std::endl;
+ }
+ }
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ if (argc < 3)
+ {
+ std::cerr << "Usage:\n\t"
+ << argv[0] << " <instance-id> <can-iface-name-1> [can-iface-name-N...]\n"
+ << "Where <instance-id> is used to augment the unique node ID and also indicates\n"
+ << "the preferred node ID value. Valid range is [0, 127]."
+ << std::endl;
+ return 1;
+ }
+
+ const int instance_id = std::stoi(argv[1]);
+ if (instance_id < 0 || instance_id > 127)
+ {
+ std::cerr << "Invalid instance ID: " << instance_id << std::endl;
+ std::exit(1);
+ }
+
+ uavcan_linux::NodePtr node = initNodeWithDynamicID(std::vector<std::string>(argv + 2, argv + argc),
+ std::uint8_t(instance_id),
+ std::uint8_t(instance_id),
+ "org.uavcan.linux_test_dynamic_node_id_client");
+ runForever(node);
+
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Error: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_file_server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <string>
+#include <cstdlib>
+#include <cstdio>
+#include <sys/types.h>
+#include <unistd.h>
+#include "debug.hpp"
+// UAVCAN
+#include <uavcan/protocol/file_server.hpp>
+// UAVCAN Linux drivers
+#include <uavcan_linux/uavcan_linux.hpp>
+// UAVCAN POSIX drivers
+#include <uavcan_posix/basic_file_server_backend.hpp>
+#include <uavcan_posix/firmware_version_checker.hpp> // Compilability test
+
+namespace
+{
+
+uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
+{
+ auto node = uavcan_linux::makeNode(ifaces);
+
+ node->setNodeID(nid);
+ node->setName(name.c_str());
+ node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+ {
+ const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get());
+
+ uavcan::protocol::HardwareVersion hwver;
+ std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
+ std::cout << hwver << std::endl;
+
+ node->setHardwareVersion(hwver);
+ }
+
+ const int start_res = node->start();
+ ENFORCE(0 == start_res);
+
+ node->setModeOperational();
+
+ return node;
+}
+
+void runForever(const uavcan_linux::NodePtr& node)
+{
+ uavcan_posix::BasicFileServerBackend backend(*node);
+
+ uavcan::FileServer server(*node, backend);
+
+ const int server_init_res = server.start();
+ if (server_init_res < 0)
+ {
+ throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res));
+ }
+
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+ if (res < 0)
+ {
+ std::cerr << "Spin error: " << res << std::endl;
+ }
+ }
+}
+
+struct Options
+{
+ uavcan::NodeID node_id;
+ std::vector<std::string> ifaces;
+};
+
+Options parseOptions(int argc, const char** argv)
+{
+ const char* const executable_name = *argv++;
+ argc--;
+
+ const auto enforce = [executable_name](bool condition, const char* error_text) {
+ if (!condition)
+ {
+ std::cerr << error_text << "\n"
+ << "Usage:\n\t"
+ << executable_name
+ << " <node-id> <can-iface-name-1> [can-iface-name-N...]"
+ << std::endl;
+ std::exit(1);
+ }
+ };
+
+ enforce(argc >= 2, "Not enough arguments");
+
+ /*
+ * Node ID is always at the first position
+ */
+ argc--;
+ const int node_id = std::stoi(*argv++);
+ enforce(node_id >= 1 && node_id <= 127, "Invalid node ID");
+
+ Options out;
+ out.node_id = uavcan::NodeID(std::uint8_t(node_id));
+
+ while (argc --> 0)
+ {
+ const std::string token(*argv++);
+
+ if (token[0] != '-')
+ {
+ out.ifaces.push_back(token);
+ }
+ else
+ {
+ enforce(false, "Unexpected argument");
+ }
+ }
+
+ return out;
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ auto options = parseOptions(argc, argv);
+
+ std::cout << "Self node ID: " << int(options.node_id.get()) << "\n"
+ "Num ifaces: " << options.ifaces.size() << "\n"
+#ifdef NDEBUG
+ "Build mode: Release"
+#else
+ "Build mode: Debug"
+#endif
+ << std::endl;
+
+ auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_test_file_server");
+ runForever(node);
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Error: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_multithreading.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,554 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef NDEBUG
+# define UAVCAN_DEBUG 1
+#endif
+
+#include <iostream>
+#include <thread>
+#include <condition_variable>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/node/sub_node.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/debug/KeyValue.hpp>
+#include "debug.hpp"
+
+/**
+ * Generic queue based on the linked list class defined in libuavcan.
+ * This class does not use heap memory.
+ */
+template <typename T>
+class Queue
+{
+ struct Item : public uavcan::LinkedListNode<Item>
+ {
+ T payload;
+
+ template <typename... Args>
+ Item(Args... args) : payload(args...) { }
+ };
+
+ uavcan::LimitedPoolAllocator allocator_;
+ uavcan::LinkedListRoot<Item> list_;
+
+public:
+ Queue(uavcan::IPoolAllocator& arg_allocator, std::size_t block_allocation_quota) :
+ allocator_(arg_allocator, block_allocation_quota)
+ {
+ uavcan::IsDynamicallyAllocatable<Item>::check();
+ }
+
+ bool isEmpty() const { return list_.isEmpty(); }
+
+ /**
+ * Creates one item in-place at the end of the list.
+ * Returns true if the item was appended successfully, false if there's not enough memory.
+ * Complexity is O(N) where N is queue length.
+ */
+ template <typename... Args>
+ bool tryEmplace(Args... args)
+ {
+ // Allocating memory
+ void* const ptr = allocator_.allocate(sizeof(Item));
+ if (ptr == nullptr)
+ {
+ return false;
+ }
+
+ // Constructing the new item
+ Item* const item = new (ptr) Item(args...);
+ assert(item != nullptr);
+
+ // Inserting the new item at the end of the list
+ Item* p = list_.get();
+ if (p == nullptr)
+ {
+ list_.insert(item);
+ }
+ else
+ {
+ while (p->getNextListNode() != nullptr)
+ {
+ p = p->getNextListNode();
+ }
+ assert(p->getNextListNode() == nullptr);
+ p->setNextListNode(item);
+ assert(p->getNextListNode()->getNextListNode() == nullptr);
+ }
+
+ return true;
+ }
+
+ /**
+ * Accesses the first element.
+ * Nullptr will be returned if the queue is empty.
+ * Complexity is O(1).
+ */
+ T* peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
+ const T* peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
+
+ /**
+ * Removes the first element.
+ * If the queue is empty, nothing will be done and assertion failure will be triggered.
+ * Complexity is O(1).
+ */
+ void pop()
+ {
+ Item* const item = list_.get();
+ assert(item != nullptr);
+ if (item != nullptr)
+ {
+ list_.remove(item);
+ item->~Item();
+ allocator_.deallocate(item);
+ }
+ }
+};
+
+/**
+ * Feel free to remove.
+ */
+static void testQueue()
+{
+ uavcan::PoolAllocator<1024, uavcan::MemPoolBlockSize> allocator;
+ Queue<typename uavcan::MakeString<50>::Type> q(allocator, 4);
+ ENFORCE(q.isEmpty());
+ ENFORCE(q.peek() == nullptr);
+ ENFORCE(q.tryEmplace("One"));
+ ENFORCE(q.tryEmplace("Two"));
+ ENFORCE(q.tryEmplace("Three"));
+ ENFORCE(q.tryEmplace("Four"));
+ ENFORCE(!q.tryEmplace("Five"));
+ ENFORCE(*q.peek() == "One");
+ q.pop();
+ ENFORCE(*q.peek() == "Two");
+ q.pop();
+ ENFORCE(*q.peek() == "Three");
+ q.pop();
+ ENFORCE(*q.peek() == "Four");
+ q.pop();
+ ENFORCE(q.isEmpty());
+ ENFORCE(q.peek() == nullptr);
+}
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ * This class does not use heap memory.
+ */
+class VirtualCanIface : public uavcan::ICanIface,
+ uavcan::Noncopyable
+{
+ struct RxItem
+ {
+ const uavcan::CanRxFrame frame;
+ const uavcan::CanIOFlags flags;
+
+ RxItem(const uavcan::CanRxFrame& arg_frame, uavcan::CanIOFlags arg_flags) :
+ frame(arg_frame),
+ flags(arg_flags)
+ { }
+ };
+
+ std::mutex& mutex_;
+ uavcan::CanTxQueue prioritized_tx_queue_;
+ Queue<RxItem> rx_queue_;
+
+ int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
+ return 1;
+ }
+
+ int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+
+ if (rx_queue_.isEmpty())
+ {
+ return 0;
+ }
+
+ const auto item = *rx_queue_.peek();
+ rx_queue_.pop();
+
+ out_frame = item.frame;
+ out_ts_monotonic = item.frame.ts_mono;
+ out_ts_utc = item.frame.ts_utc;
+ out_flags = item.flags;
+
+ return 1;
+ }
+
+ int16_t configureFilters(const uavcan::CanFilterConfig*, std::uint16_t) override { return -uavcan::ErrDriver; }
+ uint16_t getNumFilters() const override { return 0; }
+ uint64_t getErrorCount() const override { return 0; }
+
+public:
+ VirtualCanIface(uavcan::IPoolAllocator& allocator, uavcan::ISystemClock& clock,
+ std::mutex& arg_mutex, unsigned quota_per_queue) :
+ mutex_(arg_mutex),
+ prioritized_tx_queue_(allocator, clock, quota_per_queue),
+ rx_queue_(allocator, quota_per_queue)
+ { }
+
+ /**
+ * Note that RX queue overwrites oldest items when overflowed.
+ * Call this from the main thread only.
+ * No additional locking is required.
+ */
+ void addRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags)
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty())
+ {
+ rx_queue_.pop();
+ (void)rx_queue_.tryEmplace(frame, flags);
+ }
+ }
+
+ /**
+ * Call this from the main thread only.
+ * No additional locking is required.
+ */
+ void flushTxQueueTo(uavcan::INode& main_node, std::uint8_t iface_index)
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+
+ const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
+
+ while (auto e = prioritized_tx_queue_.peek())
+ {
+ UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
+ unsigned(iface_mask), e->toString().c_str());
+
+ const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
+ uavcan::CanTxQueue::Qos(e->qos), e->flags);
+ prioritized_tx_queue_.remove(e);
+ if (res <= 0)
+ {
+ break;
+ }
+ }
+ }
+
+ /**
+ * Call this from the sub-node thread only.
+ * No additional locking is required.
+ */
+ bool hasDataInRxQueue() const
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ return !rx_queue_.isEmpty();
+ }
+};
+
+/**
+ * This interface defines one method that will be called by the main node thread periodically in order to
+ * transfer contents of TX queue of the sub-node into the TX queue of the main node.
+ */
+class ITxQueueInjector
+{
+public:
+ virtual ~ITxQueueInjector() { }
+
+ /**
+ * Flush contents of TX queues into the main node.
+ * @param main_node Reference to the main node.
+ */
+ virtual void injectTxFramesInto(uavcan::INode& main_node) = 0;
+};
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ * This class does not use heap memory.
+ * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
+ * memory pool that will be shared across all interfaces for RX/TX queues.
+ * Typically this value should be no less than 4K per interface.
+ */
+template <unsigned SharedMemoryPoolSize>
+class VirtualCanDriver : public uavcan::ICanDriver,
+ public uavcan::IRxFrameListener,
+ public ITxQueueInjector,
+ uavcan::Noncopyable
+{
+ class Event
+ {
+ std::mutex m_;
+ std::condition_variable cv_;
+
+ public:
+ /**
+ * Note that this method may return spuriously.
+ */
+ void waitFor(uavcan::MonotonicDuration duration)
+ {
+ std::unique_lock<std::mutex> lk(m_);
+ (void)cv_.wait_for(lk, std::chrono::microseconds(duration.toUSec()));
+ }
+
+ void signal() { cv_.notify_all(); }
+ };
+
+ Event event_; ///< Used to unblock the select() call when IO happens.
+ std::mutex mutex_; ///< Shared across all ifaces
+ uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
+ uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
+ const unsigned num_ifaces_;
+ uavcan_linux::SystemClock clock_;
+
+ uavcan::ICanIface* getIface(uint8_t iface_index) override
+ {
+ return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface*() : nullptr;
+ }
+
+ uint8_t getNumIfaces() const override { return num_ifaces_; }
+
+ /**
+ * This and other methods of ICanDriver will be invoked by the sub-node thread.
+ */
+ int16_t select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime blocking_deadline) override
+ {
+ bool need_block = (inout_masks.write == 0); // Write queue is infinite
+ for (unsigned i = 0; need_block && (i < num_ifaces_); i++)
+ {
+ const bool need_read = inout_masks.read & (1U << i);
+ if (need_read && ifaces_[i]->hasDataInRxQueue())
+ {
+ need_block = false;
+ }
+ }
+
+ if (need_block)
+ {
+ event_.waitFor(blocking_deadline - clock_.getMonotonic());
+ }
+
+ inout_masks = uavcan::CanSelectMasks();
+ for (unsigned i = 0; i < num_ifaces_; i++)
+ {
+ const std::uint8_t iface_mask = 1U << i;
+ inout_masks.write |= iface_mask; // Always ready to write
+ if (ifaces_[i]->hasDataInRxQueue())
+ {
+ inout_masks.read |= iface_mask;
+ }
+ }
+
+ return num_ifaces_; // We're always ready to write, hence > 0.
+ }
+
+ /**
+ * This handler will be invoked by the main node thread.
+ */
+ void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags) override
+ {
+ UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
+ if (frame.iface_index < num_ifaces_)
+ {
+ ifaces_[frame.iface_index]->addRxFrame(frame, flags);
+ event_.signal();
+ }
+ }
+
+ /**
+ * This method will be invoked by the main node thread.
+ */
+ void injectTxFramesInto(uavcan::INode& main_node) override
+ {
+ for (unsigned i = 0; i < num_ifaces_; i++)
+ {
+ ifaces_[i]->flushTxQueueTo(main_node, i);
+ }
+ event_.signal();
+ }
+
+public:
+ VirtualCanDriver(unsigned arg_num_ifaces) : num_ifaces_(arg_num_ifaces)
+ {
+ assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
+
+ const unsigned quota_per_iface = allocator_.getBlockCapacity() / num_ifaces_;
+ const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
+
+ UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
+ unsigned(allocator_.getBlockCapacity()), unsigned(quota_per_queue));
+
+ for (unsigned i = 0; i < num_ifaces_; i++)
+ {
+ ifaces_[i].template construct<uavcan::IPoolAllocator&, uavcan::ISystemClock&,
+ std::mutex&, unsigned>(allocator_, clock_, mutex_, quota_per_queue);
+ }
+ }
+};
+
+static uavcan_linux::NodePtr initMainNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
+ const std::string& name)
+{
+ std::cout << "Initializing main node" << std::endl;
+
+ auto node = uavcan_linux::makeNode(ifaces, name.c_str(),
+ uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion(), nid);
+ node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+ node->setModeOperational();
+ return node;
+}
+
+template <unsigned QueuePoolSize>
+static uavcan_linux::SubNodePtr initSubNode(unsigned num_ifaces, uavcan::INode& main_node)
+{
+ std::cout << "Initializing sub node" << std::endl;
+
+ typedef VirtualCanDriver<QueuePoolSize> Driver;
+
+ std::shared_ptr<Driver> driver(new Driver(num_ifaces));
+
+ auto node = uavcan_linux::makeSubNode(driver, main_node.getNodeID());
+
+ main_node.getDispatcher().installRxFrameListener(driver.get());
+
+ return node;
+}
+
+static void runMainNode(const uavcan_linux::NodePtr& node)
+{
+ std::cout << "Running main node" << std::endl;
+
+ auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(10000), [&node](const uavcan::TimerEvent&)
+ {
+ node->logInfo("timer", "Your time is running out.");
+ // coverity[dont_call]
+ node->setVendorSpecificStatusCode(static_cast<std::uint16_t>(std::rand()));
+ });
+
+ /*
+ * We know that in this implementation, VirtualCanDriver inherits uavcan::IRxFrameListener, so we can simply
+ * restore the reference to ITxQueueInjector using dynamic_cast. In other implementations this may be
+ * unacceptable, so a reference to ITxQueueInjector will have to be passed using some other means.
+ */
+ if (node->getDispatcher().getRxFrameListener() == nullptr)
+ {
+ throw std::logic_error("RX frame listener is not configured");
+ }
+ ITxQueueInjector& tx_injector = dynamic_cast<ITxQueueInjector&>(*node->getDispatcher().getRxFrameListener());
+
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::fromMSec(1));
+ if (res < 0)
+ {
+ node->logError("spin", "Error %*", res);
+ }
+ // TX queue transfer occurs here.
+ tx_injector.injectTxFramesInto(*node);
+ }
+}
+
+static void runSubNode(const uavcan_linux::SubNodePtr& node)
+{
+ std::cout << "Running sub node" << std::endl;
+
+ /*
+ * Log subscriber
+ */
+ auto log_sub = node->makeSubscriber<uavcan::protocol::debug::LogMessage>(
+ [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg)
+ {
+ std::cout << msg << std::endl;
+ });
+
+ /*
+ * Node status monitor
+ */
+ struct NodeStatusMonitor : public uavcan::NodeStatusMonitor
+ {
+ explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { }
+
+ virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) override
+ {
+ std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: "
+ << event.old_status.toString() << " --> " << event.status.toString() << std::endl;
+ }
+ };
+ NodeStatusMonitor nsm(*node);
+ ENFORCE(0 == nsm.start());
+
+ /*
+ * KV subscriber
+ */
+ auto kv_sub = node->makeSubscriber<uavcan::protocol::debug::KeyValue>(
+ [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::KeyValue>& msg)
+ {
+ std::cout << msg << std::endl;
+ });
+
+ /*
+ * KV publisher
+ */
+ unsigned kv_value = 0;
+ auto kv_pub = node->makePublisher<uavcan::protocol::debug::KeyValue>();
+ auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(5000), [&](const uavcan::TimerEvent&)
+ {
+ uavcan::protocol::debug::KeyValue kv;
+ kv.key = "five_seconds";
+ kv.value = kv_value++;
+ const int res = kv_pub->broadcast(kv);
+ if (res < 0)
+ {
+ std::cerr << "Sub KV pub err " << res << std::endl;
+ }
+ });
+
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::fromMSec(1000));
+ if (res < 0)
+ {
+ std::cerr << "SubNode spin error: " << res << std::endl;
+ }
+ }
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ testQueue();
+
+ constexpr unsigned VirtualIfacePoolSize = 32768;
+
+ if (argc < 3)
+ {
+ std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+ return 1;
+ }
+
+ const int self_node_id = std::stoi(argv[1]);
+ std::vector<std::string> iface_names(argv + 2, argv + argc);
+
+ auto node = initMainNode(iface_names, self_node_id, "org.uavcan.linux_test_node");
+ auto sub_node = initSubNode<VirtualIfacePoolSize>(iface_names.size(), *node);
+
+ std::thread sub_thread([&sub_node](){ runSubNode(sub_node); });
+
+ runMainNode(node);
+
+ if (sub_thread.joinable())
+ {
+ std::cout << "Waiting for the sub thread to join" << std::endl;
+ sub_thread.join();
+ }
+
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Exception: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_node.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "debug.hpp"
+
+static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
+ const std::string& name)
+{
+ auto node = uavcan_linux::makeNode(ifaces);
+
+ /*
+ * Configuring the node.
+ */
+ node->setNodeID(nid);
+ node->setName(name.c_str());
+
+ node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+ /*
+ * Starting the node.
+ */
+ std::cout << "Starting the node..." << std::endl;
+ const int start_res = node->start();
+ std::cout << "Start returned: " << start_res << std::endl;
+ ENFORCE(0 == start_res);
+
+ std::cout << "Node started successfully" << std::endl;
+
+ /*
+ * Say Hi to the world.
+ */
+ node->setModeOperational();
+ node->logInfo("init", "Hello world! I'm [%*], NID %*",
+ node->getNodeStatusProvider().getName().c_str(), int(node->getNodeID().get()));
+ return node;
+}
+
+static void runForever(const uavcan_linux::NodePtr& node)
+{
+ /*
+ * Subscribing to the UAVCAN logging topic
+ */
+ auto log_handler = [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg)
+ {
+ std::cout << msg << std::endl;
+ };
+ auto log_sub = node->makeSubscriber<uavcan::protocol::debug::LogMessage>(log_handler);
+
+ /*
+ * Printing when other nodes enter the network or change status
+ */
+ struct NodeStatusMonitor : public uavcan::NodeStatusMonitor
+ {
+ explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { }
+
+ void handleNodeStatusChange(const NodeStatusChangeEvent& event) override
+ {
+ std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: "
+ << event.old_status.toString() << " --> "
+ << event.status.toString() << std::endl;
+ }
+ };
+
+ NodeStatusMonitor nsm(*node);
+ ENFORCE(0 == nsm.start());
+
+ /*
+ * Adding a stupid timer that does nothing once a minute
+ */
+ auto do_nothing_once_a_minute = [&node](const uavcan::TimerEvent&)
+ {
+ node->logInfo("timer", "Another minute passed...");
+ // coverity[dont_call]
+ node->setVendorSpecificStatusCode(static_cast<std::uint16_t>(std::rand())); // Setting to an arbitrary value
+ };
+ auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute);
+
+ /*
+ * Spinning forever
+ */
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
+ if (res < 0)
+ {
+ node->logError("spin", "Error %*", res);
+ }
+ }
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ if (argc < 3)
+ {
+ std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+ return 1;
+ }
+ const int self_node_id = std::stoi(argv[1]);
+ std::vector<std::string> iface_names;
+ for (int i = 2; i < argc; i++)
+ {
+ iface_names.emplace_back(argv[i]);
+ }
+ uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node");
+ std::cout << "Node initialized successfully" << std::endl;
+ runForever(node);
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Exception: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_posix.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <iostream>
+#include <iomanip>
+#include "debug.hpp"
+
+int main(int argc, const char** argv)
+{
+ (void)argc;
+ (void)argv;
+ try
+ {
+ ENFORCE(0 == std::system("mkdir -p /tmp/uavcan_posix/dynamic_node_id_server"));
+
+ /*
+ * Event tracer test
+ */
+ {
+ using namespace uavcan::dynamic_node_id_server;
+
+ const std::string event_log_file("/tmp/uavcan_posix/dynamic_node_id_server/event.log");
+
+ uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
+ ENFORCE(0 <= tracer.init(event_log_file.c_str()));
+
+ // Adding a line
+ static_cast<IEventTracer&>(tracer).onEvent(TraceError, 123456);
+ ENFORCE(0 == std::system(("cat " + event_log_file).c_str()));
+
+ // Removing the log file
+ ENFORCE(0 == std::system(("rm -f " + event_log_file).c_str()));
+
+ // Adding another line
+ static_cast<IEventTracer&>(tracer).onEvent(TraceError, 789123);
+ ENFORCE(0 == std::system(("cat " + event_log_file).c_str()));
+ }
+
+ /*
+ * Storage backend test
+ */
+ {
+ using namespace uavcan::dynamic_node_id_server;
+
+ uavcan_posix::dynamic_node_id_server::FileStorageBackend backend;
+ ENFORCE(0 <= backend.init("/tmp/uavcan_posix/dynamic_node_id_server/storage"));
+
+ auto print_key = [&](const char* key) {
+ std::cout << static_cast<IStorageBackend&>(backend).get(key).c_str() << std::endl;
+ };
+
+ print_key("foobar");
+
+ static_cast<IStorageBackend&>(backend).set("foobar", "0123456789abcdef0123456789abcdef");
+ static_cast<IStorageBackend&>(backend).set("the_answer", "42");
+
+ print_key("foobar");
+ print_key("the_answer");
+ print_key("nonexistent");
+ }
+
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Exception: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_socket.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,364 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <vector>
+#include <cerrno>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include "debug.hpp"
+
+static uavcan::CanFrame makeFrame(std::uint32_t id, const std::string& data)
+{
+ return uavcan::CanFrame(id, reinterpret_cast<const std::uint8_t*>(data.c_str()), data.length());
+}
+
+static uavcan::MonotonicTime tsMonoOffsetMs(std::int64_t ms)
+{
+ return uavcan_linux::SystemClock().getMonotonic() + uavcan::MonotonicDuration::fromMSec(ms);
+}
+
+static void testNonexistentIface()
+{
+ const int sock1 = uavcan_linux::SocketCanIface::openSocket("noif9");
+ ENFORCE(sock1 < 0);
+ const int sock2 = uavcan_linux::SocketCanIface::openSocket("verylongifacenameverylongifacenameverylongifacename");
+ ENFORCE(sock2 < 0);
+}
+
+static void testSocketRxTx(const std::string& iface_name)
+{
+ const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+ const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+ ENFORCE(sock1 >= 0 && sock2 >= 0);
+
+ /*
+ * Clocks will have some offset from the true system time
+ * SocketCAN driver must handle this correctly
+ */
+ uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+ clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(100000));
+ const uavcan_linux::SystemClock& clock = clock_impl;
+
+ uavcan_linux::SocketCanIface if1(clock, sock1);
+ uavcan_linux::SocketCanIface if2(clock, sock2);
+
+ /*
+ * Sending two frames, one of which must be returned back
+ */
+ ENFORCE(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
+ ENFORCE(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
+ if1.poll(true, true);
+ if1.poll(true, true);
+ ENFORCE(0 == if1.getErrorCount());
+ ENFORCE(!if1.hasReadyTx());
+ ENFORCE(if1.hasReadyRx()); // Second loopback
+
+ /*
+ * Second iface, same thing
+ */
+ ENFORCE(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
+ ENFORCE(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
+ ENFORCE(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(-1), uavcan::CanIOFlagLoopback)); // Will timeout
+ if2.poll(true, true);
+ if2.poll(true, true);
+ ENFORCE(1 == if2.getErrorCount()); // One timed out
+ ENFORCE(!if2.hasReadyTx());
+ ENFORCE(if2.hasReadyRx());
+
+ /*
+ * No-op
+ */
+ if1.poll(true, true);
+ if2.poll(true, true);
+
+ uavcan::CanFrame frame;
+ uavcan::MonotonicTime ts_mono;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags = 0;
+
+ /*
+ * Read first
+ */
+ ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(456, "if1-2"));
+ ENFORCE(flags == uavcan::CanIOFlagLoopback);
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(321, "if2-1"));
+ ENFORCE(flags == 0);
+ ENFORCE(!ts_mono.isZero());
+ ENFORCE(!ts_utc.isZero());
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(654, "if2-2"));
+ ENFORCE(flags == 0);
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(0 == if1.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(!if1.hasReadyTx());
+ ENFORCE(!if1.hasReadyRx());
+
+ /*
+ * Read second
+ */
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(123, "if1-1"));
+ ENFORCE(flags == 0);
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(456, "if1-2"));
+ ENFORCE(flags == 0);
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(654, "if2-2"));
+ ENFORCE(flags == uavcan::CanIOFlagLoopback);
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(0 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(!if2.hasReadyTx());
+ ENFORCE(!if2.hasReadyRx());
+}
+
+static void testSocketFilters(const std::string& iface_name)
+{
+ using uavcan::CanFrame;
+
+ const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+ const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
+ ENFORCE(sock1 >= 0 && sock2 >= 0);
+
+ /*
+ * Clocks will have some offset from the true system time
+ * SocketCAN driver must handle this correctly
+ */
+ uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+ clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(-1000));
+ const uavcan_linux::SystemClock& clock = clock_impl;
+
+ uavcan_linux::SocketCanIface if1(clock, sock1);
+ uavcan_linux::SocketCanIface if2(clock, sock2);
+
+ /*
+ * Configuring filters
+ */
+ uavcan::CanFilterConfig fcs[3];
+ // STD/EXT 123
+ fcs[0].id = 123;
+ fcs[0].mask = CanFrame::MaskExtID;
+ // Only EXT 456789
+ fcs[1].id = 456789 | CanFrame::FlagEFF;
+ fcs[1].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
+ // Only STD 0
+ fcs[2].id = 0;
+ fcs[2].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
+
+ ENFORCE(0 == if2.configureFilters(fcs, 3));
+
+ /*
+ * Sending data from 1 to 2, making sure only filtered data will be accepted
+ */
+ const auto EFF = CanFrame::FlagEFF;
+ ENFORCE(1 == if1.send(makeFrame(123, "1"), tsMonoOffsetMs(100), 0)); // Accept 0
+ ENFORCE(1 == if1.send(makeFrame(123 | EFF, "2"), tsMonoOffsetMs(100), 0)); // Accept 0
+ ENFORCE(1 == if1.send(makeFrame(456, "3"), tsMonoOffsetMs(100), 0)); // Drop
+ ENFORCE(1 == if1.send(makeFrame(456789, "4"), tsMonoOffsetMs(100), 0)); // Drop
+ ENFORCE(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
+ ENFORCE(1 == if1.send(makeFrame(0, "6"), tsMonoOffsetMs(100), 0)); // Accept 2
+ ENFORCE(1 == if1.send(makeFrame(EFF, "7"), tsMonoOffsetMs(100), 0)); // Drop
+
+ for (int i = 0; i < 7; i++)
+ {
+ if1.poll(true, true);
+ if2.poll(true, false);
+ }
+ ENFORCE(!if1.hasReadyTx());
+ ENFORCE(!if1.hasReadyRx());
+ ENFORCE(0 == if1.getErrorCount());
+ ENFORCE(if2.hasReadyRx());
+
+ /*
+ * Checking RX on 2
+ */
+ uavcan::CanFrame frame;
+ uavcan::MonotonicTime ts_mono;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags = 0;
+
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(123, "1"));
+
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(123 | EFF, "2"));
+
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(456789 | EFF, "5"));
+
+ ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(0, "6"));
+ ENFORCE(flags == 0);
+
+ ENFORCE(!if2.hasReadyRx());
+}
+
+static void testDriver(const std::vector<std::string>& iface_names)
+{
+ /*
+ * Clocks will have some offset from the true system time
+ * SocketCAN driver must handle this correctly
+ */
+ uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+ clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(9000000));
+ const uavcan_linux::SystemClock& clock = clock_impl;
+
+ uavcan_linux::SocketCanDriver driver(clock);
+ for (auto ifn : iface_names)
+ {
+ std::cout << "Adding iface " << ifn << std::endl;
+ ENFORCE(0 == driver.addIface(ifn));
+ }
+
+ ENFORCE(-1 == driver.addIface("noif9"));
+ ENFORCE(-1 == driver.addIface("noif9"));
+ ENFORCE(-1 == driver.addIface("noif9"));
+
+ ENFORCE(driver.getNumIfaces() == iface_names.size());
+ ENFORCE(nullptr == driver.getIface(255));
+ ENFORCE(nullptr == driver.getIface(driver.getNumIfaces()));
+
+ const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = {};
+
+ const unsigned AllIfacesMask = (1 << driver.getNumIfaces()) - 1;
+
+ /*
+ * Send, no loopback
+ */
+ std::cout << "select() 1" << std::endl;
+ uavcan::CanSelectMasks masks; // Driver provides masks for all available events
+ ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
+ ENFORCE(masks.read == 0);
+ ENFORCE(masks.write == AllIfacesMask);
+
+ for (int i = 0; i < driver.getNumIfaces(); i++)
+ {
+ ENFORCE(1 == driver.getIface(i)->send(makeFrame(123, std::to_string(i)), tsMonoOffsetMs(10), 0));
+ }
+
+ std::cout << "select() 2" << std::endl;
+ ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
+ ENFORCE(masks.read == 0);
+ ENFORCE(masks.write == AllIfacesMask);
+
+ /*
+ * Send with loopback
+ */
+ for (int i = 0; i < driver.getNumIfaces(); i++)
+ {
+ ENFORCE(1 == driver.getIface(i)->send(makeFrame(456, std::to_string(i)), tsMonoOffsetMs(10),
+ uavcan::CanIOFlagLoopback));
+ ENFORCE(1 == driver.getIface(i)->send(makeFrame(789, std::to_string(i)), tsMonoOffsetMs(-1), // Will timeout
+ uavcan::CanIOFlagLoopback));
+ }
+
+ std::cout << "select() 3" << std::endl;
+ ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
+ ENFORCE(masks.read == AllIfacesMask);
+ ENFORCE(masks.write == AllIfacesMask);
+
+ /*
+ * Receive loopback
+ */
+ for (int i = 0; i < driver.getNumIfaces(); i++)
+ {
+ uavcan::CanFrame frame;
+ uavcan::MonotonicTime ts_mono;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags = 0;
+ ENFORCE(1 == driver.getIface(i)->receive(frame, ts_mono, ts_utc, flags));
+ ENFORCE(frame == makeFrame(456, std::to_string(i)));
+ ENFORCE(flags == uavcan::CanIOFlagLoopback);
+ ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+ ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+ ENFORCE(!driver.getIface(i)->hasReadyTx());
+ ENFORCE(!driver.getIface(i)->hasReadyRx());
+ }
+
+ std::cout << "select() 4" << std::endl;
+ masks.write = 0;
+ ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
+ ENFORCE(masks.read == 0);
+ ENFORCE(masks.write == AllIfacesMask);
+
+ std::cout << "exit" << std::endl;
+
+ /*
+ * Error checks
+ */
+ for (int i = 0; i < driver.getNumIfaces(); i++)
+ {
+ for (auto kv : driver.getIface(i)->getErrors())
+ {
+ switch (kv.first)
+ {
+ case uavcan_linux::SocketCanError::SocketReadFailure:
+ case uavcan_linux::SocketCanError::SocketWriteFailure:
+ {
+ ENFORCE(kv.second == 0);
+ break;
+ }
+ case uavcan_linux::SocketCanError::TxTimeout:
+ {
+ ENFORCE(kv.second == 1); // One timed out frame from the above
+ break;
+ }
+ default:
+ {
+ ENFORCE(false);
+ break;
+ }
+ }
+ }
+ }
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ if (argc < 2)
+ {
+ std::cerr << "Usage:\n\t" << argv[0] << " <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+ return 1;
+ }
+
+ std::vector<std::string> iface_names;
+ for (int i = 1; i < argc; i++)
+ {
+ iface_names.emplace_back(argv[i]);
+ }
+
+ testNonexistentIface();
+ testSocketRxTx(iface_names[0]);
+ testSocketFilters(iface_names[0]);
+
+ testDriver(iface_names);
+
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Exception: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_system_utils.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <iostream>
+#include <iomanip>
+#include "debug.hpp"
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ const std::vector<std::string> iface_names(argv + 1, argv + argc);
+
+ const auto res = uavcan_linux::MachineIDReader(iface_names).readAndGetLocation();
+
+ const auto original_flags = std::cout.flags();
+
+ for (auto x : res.first)
+ {
+ std::cout << std::hex << std::setw(2) << std::setfill('0') << int(x);
+ }
+
+ std::cout.width(0);
+ std::cout.flags(original_flags);
+
+ std::cout << std::endl;
+
+ std::cout << res.second << std::endl;
+
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Exception: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/test_time_sync.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <cassert>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include "debug.hpp"
+
+
+static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
+ const std::string& name)
+{
+ auto node = uavcan_linux::makeNode(ifaces);
+ node->setNodeID(nid);
+ node->setName(name.c_str());
+
+ ENFORCE(0 == node->start());
+
+ node->setModeOperational();
+ return node;
+}
+
+static void runForever(const uavcan_linux::NodePtr& node)
+{
+ uavcan::GlobalTimeSyncMaster tsmaster(*node);
+ ENFORCE(0 == tsmaster.init());
+
+ uavcan::GlobalTimeSyncSlave tsslave(*node);
+ ENFORCE(0 == tsslave.start());
+
+ auto publish_sync_if_master = [&](const uavcan::TimerEvent&)
+ {
+ bool i_am_master = false;
+ if (tsslave.isActive())
+ {
+ const uavcan::NodeID master_node = tsslave.getMasterNodeID();
+ assert(master_node.isValid());
+ if (node->getNodeID() < master_node)
+ {
+ std::cout << "Overriding the lower priority master " << int(master_node.get()) << std::endl;
+ i_am_master = true;
+ }
+ else
+ {
+ std::cout << "There is other master of higher priority " << int(master_node.get()) << std::endl;
+ }
+ }
+ else
+ {
+ std::cout << "No other masters present" << std::endl;
+ i_am_master = true;
+ }
+
+ // Don't forget to disable slave adjustments if we're master
+ tsslave.suppress(i_am_master);
+
+ if (i_am_master)
+ {
+ ENFORCE(0 <= tsmaster.publish());
+ }
+ };
+
+ auto sync_publish_timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(1000), publish_sync_if_master);
+
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
+ if (res < 0)
+ {
+ node->logError("spin", "Error %*", res);
+ }
+ }
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ if (argc < 3)
+ {
+ std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+ return 1;
+ }
+ const int self_node_id = std::stoi(argv[1]);
+ std::vector<std::string> iface_names;
+ for (int i = 2; i < argc; i++)
+ {
+ iface_names.emplace_back(argv[i]);
+ }
+ uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor");
+ runForever(node);
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Exception: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,666 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <algorithm>
+#include <sstream>
+#include <iomanip>
+#include <string>
+#include <cstdlib>
+#include <cstdio>
+#include <deque>
+#include <unordered_map>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include "debug.hpp"
+// UAVCAN
+#include <uavcan/protocol/dynamic_node_id_server/distributed.hpp>
+// UAVCAN Linux drivers
+#include <uavcan_linux/uavcan_linux.hpp>
+// UAVCAN POSIX drivers
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+
+namespace
+{
+
+constexpr int MaxNumLastEvents = 30;
+constexpr int MinUpdateInterval = 100;
+
+uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
+{
+ const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get());
+
+ uavcan::protocol::HardwareVersion hwver;
+ std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
+ std::cout << hwver << std::endl;
+
+ auto node = uavcan_linux::makeNode(ifaces, name.c_str(), uavcan::protocol::SoftwareVersion(), hwver, nid);
+
+ node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+ node->setModeOperational();
+
+ return node;
+}
+
+
+class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer
+{
+public:
+ struct RecentEvent
+ {
+ const uavcan::MonotonicDuration time_since_startup;
+ const uavcan::UtcTime utc_timestamp;
+ const uavcan::dynamic_node_id_server::TraceCode code;
+ const std::int64_t argument;
+
+ RecentEvent(uavcan::MonotonicDuration arg_time_since_startup,
+ uavcan::UtcTime arg_utc_timestamp,
+ uavcan::dynamic_node_id_server::TraceCode arg_code,
+ std::int64_t arg_argument)
+ : time_since_startup(arg_time_since_startup)
+ , utc_timestamp(arg_utc_timestamp)
+ , code(arg_code)
+ , argument(arg_argument)
+ { }
+
+ uavcan::MakeString<81>::Type toString() const // Heapless return
+ {
+ char timebuf[12] = { };
+ {
+ const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6;
+ const auto tm = std::localtime(&rawtime);
+ std::strftime(timebuf, 10, "%H:%M:%S.", tm);
+ std::snprintf(&timebuf[9], 3, "%02u", static_cast<unsigned>((utc_timestamp.toMSec() % 1000U) / 10U));
+ }
+
+ decltype(toString()) out;
+ out.resize(out.capacity());
+ // coverity[overflow : FALSE]
+ (void)std::snprintf(reinterpret_cast<char*>(out.begin()), out.size() - 1U,
+ "%-11s %-28s %-20lld %016llx",
+ timebuf,
+ getEventName(code),
+ static_cast<long long>(argument),
+ static_cast<long long>(argument));
+ return out;
+ }
+
+ static const char* getTableHeader()
+ {
+ // Matches the string format above
+ return "Timestamp Event name Argument (dec) Argument (hex)";
+ }
+ };
+
+ struct EventStatisticsRecord
+ {
+ std::uint64_t count;
+ uavcan::MonotonicTime last_occurence;
+
+ EventStatisticsRecord()
+ : count(0)
+ { }
+
+ void hit(uavcan::MonotonicTime ts)
+ {
+ count++;
+ last_occurence = ts;
+ }
+ };
+
+private:
+ struct EnumKeyHash
+ {
+ template <typename T>
+ std::size_t operator()(T t) const { return static_cast<std::size_t>(t); }
+ };
+
+ uavcan_linux::SystemClock clock_;
+ const uavcan::MonotonicTime started_at_ = clock_.getMonotonic();
+ const unsigned num_last_events_;
+
+ std::deque<RecentEvent> last_events_;
+ std::unordered_map<uavcan::dynamic_node_id_server::TraceCode, EventStatisticsRecord, EnumKeyHash> event_counters_;
+
+ bool had_events_ = false;
+
+ void onEvent(uavcan::dynamic_node_id_server::TraceCode code, std::int64_t argument) override
+ {
+ uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument);
+
+ had_events_ = true;
+
+ const auto ts_m = clock_.getMonotonic();
+ const auto ts_utc = clock_.getUtc();
+ const auto time_since_startup = ts_m - started_at_;
+
+ last_events_.emplace_front(time_since_startup, ts_utc, code, argument);
+ if (last_events_.size() > num_last_events_)
+ {
+ last_events_.pop_back();
+ }
+
+ event_counters_[code].hit(ts_m);
+ }
+
+public:
+ EventTracer(unsigned num_last_events_to_keep)
+ : num_last_events_(num_last_events_to_keep)
+ { }
+
+ using uavcan_posix::dynamic_node_id_server::FileEventTracer::init;
+
+ const RecentEvent& getEventByIndex(unsigned index) const { return last_events_.at(index); }
+
+ unsigned getNumEvents() const { return last_events_.size(); }
+
+ const decltype(event_counters_)& getEventCounters() const { return event_counters_; }
+
+ bool hadEvents()
+ {
+ if (had_events_)
+ {
+ had_events_ = false;
+ return true;
+ }
+ return false;
+ }
+};
+
+
+::winsize getTerminalSize()
+{
+ auto w = ::winsize();
+ ENFORCE(0 >= ioctl(STDOUT_FILENO, TIOCGWINSZ, &w));
+ ENFORCE(w.ws_col > 0 && w.ws_row > 0);
+ return w;
+}
+
+
+std::vector<std::pair<uavcan::dynamic_node_id_server::TraceCode, EventTracer::EventStatisticsRecord>>
+collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events)
+{
+ // First, creating a vector of pairs (event code, count)
+ typedef std::pair<uavcan::dynamic_node_id_server::TraceCode, EventTracer::EventStatisticsRecord> Pair;
+ const auto counters = event_tracer.getEventCounters();
+ std::vector<Pair> pairs(counters.size());
+ std::copy(counters.begin(), counters.end(), pairs.begin());
+
+ // Now, sorting the pairs so that the most recent ones are on top of the list
+ std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) {
+ return a.second.last_occurence > b.second.last_occurence;
+ });
+
+ // Cutting the oldest events away
+ pairs.resize(std::min(num_events, unsigned(pairs.size())));
+
+ // Sorting so that the most frequent events are on top of the list
+ std::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) {
+ return a.second.count > b.second.count;
+ });
+
+ return pairs;
+}
+
+enum class CLIColor : unsigned
+{
+ Red = 31,
+ Green = 32,
+ Yellow = 33,
+ Blue = 34,
+ Magenta = 35,
+ Cyan = 36,
+ White = 37,
+ Default = 39
+};
+
+CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); }
+
+class CLIColorizer
+{
+ const CLIColor color_;
+public:
+ explicit CLIColorizer(CLIColor c) : color_(c)
+ {
+ std::printf("\033[%um", static_cast<unsigned>(color_));
+ }
+
+ ~CLIColorizer()
+ {
+ std::printf("\033[%um", static_cast<unsigned>(CLIColor::Default));
+ }
+};
+
+
+void redraw(const uavcan_linux::NodePtr& node,
+ const uavcan::MonotonicTime timestamp,
+ const EventTracer& event_tracer,
+ const uavcan::dynamic_node_id_server::DistributedServer& server)
+{
+ using uavcan::dynamic_node_id_server::distributed::RaftCore;
+
+ /*
+ * Constants that are permanent for the designed UI layout
+ */
+ constexpr unsigned NumRelevantEvents = 17;
+ constexpr unsigned NumRowsWithoutEvents = 3;
+
+ /*
+ * Collecting the data
+ */
+ const unsigned num_rows = getTerminalSize().ws_row;
+
+ const auto relevant_events = collectRelevantEvents(event_tracer, NumRelevantEvents);
+
+ const uavcan::dynamic_node_id_server::distributed::StateReport report(server);
+
+ const auto time_since_last_activity = timestamp - report.last_activity_timestamp;
+
+ /*
+ * Basic rendering functions
+ */
+ unsigned next_relevant_event_index = 0;
+
+ const auto render_next_event_counter = [&]()
+ {
+ const char* event_name = "";
+ char event_count_str[10] = { };
+ CLIColor event_color = CLIColor::Default;
+
+ if (next_relevant_event_index < relevant_events.size())
+ {
+ const auto e = relevant_events[next_relevant_event_index];
+ event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first);
+ std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu",
+ static_cast<unsigned long long>(e.second.count));
+ event_color = getColorHash(static_cast<unsigned>(e.first));
+ }
+ next_relevant_event_index++;
+
+ std::printf(" | ");
+ CLIColorizer izer(event_color);
+ std::printf("%-29s %-9s\n", event_name, event_count_str);
+ };
+
+ const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color)
+ {
+ {
+ CLIColorizer izer(color);
+ std::printf("%-20s %-16s", local_state_name, local_state_value);
+ }
+ render_next_event_counter();
+ };
+
+ const auto render_top_int = [&](const char* local_state_name, long long local_state_value, CLIColor color)
+ {
+ char buf[21];
+ std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value);
+ render_top_str(local_state_name, buf, color);
+ };
+
+ const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s)
+ {
+ switch (s)
+ {
+ case RaftCore::ServerStateFollower: return "Follower";
+ case RaftCore::ServerStateCandidate: return "Candidate";
+ case RaftCore::ServerStateLeader: return "Leader";
+ default: return "BADSTATE";
+ }
+ };
+
+ const auto duration_to_string = [](uavcan::MonotonicDuration dur)
+ {
+ uavcan::MakeString<16>::Type str; // This is much faster than std::string
+ str.appendFormatted("%.1f", dur.toUSec() / 1e6);
+ return str;
+ };
+
+ const auto colorize_if = [](bool condition, CLIColor color)
+ {
+ return condition ? color : CLIColor::Default;
+ };
+
+ /*
+ * Rendering the data to the CLI
+ */
+ std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning
+ std::printf("\x1b[H"); // Move cursor to the coordinates 1,1
+
+ // Local state and relevant event counters - two columns
+ std::printf(" Local state | Event counters\n");
+
+ render_top_int("Node ID",
+ node->getNodeID().get(),
+ CLIColor::Default);
+
+ render_top_str("State",
+ raft_state_to_string(report.state),
+ (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta :
+ (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green :
+ CLIColor::Default);
+
+ render_top_int("Last log index",
+ report.last_log_index,
+ CLIColor::Default);
+
+ render_top_int("Commit index",
+ report.commit_index,
+ colorize_if(report.commit_index != report.last_log_index, CLIColor::Magenta));
+
+ render_top_int("Last log term",
+ report.last_log_term,
+ CLIColor::Default);
+
+ render_top_int("Current term",
+ report.current_term,
+ CLIColor::Default);
+
+ render_top_int("Voted for",
+ report.voted_for.get(),
+ CLIColor::Default);
+
+ render_top_str("Since activity",
+ duration_to_string(time_since_last_activity).c_str(),
+ CLIColor::Default);
+
+ render_top_str("Random timeout",
+ duration_to_string(report.randomized_timeout).c_str(),
+ CLIColor::Default);
+
+ render_top_int("Unknown nodes",
+ report.num_unknown_nodes,
+ colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta));
+
+ render_top_int("Node failures",
+ node->getInternalFailureCount(),
+ colorize_if(node->getInternalFailureCount() != 0, CLIColor::Magenta));
+
+ const bool all_allocated = server.guessIfAllDynamicNodesAreAllocated();
+ render_top_str("All allocated",
+ all_allocated ? "Yes": "No",
+ colorize_if(!all_allocated, CLIColor::Magenta));
+
+ // Empty line before the next block
+ std::printf(" ");
+ render_next_event_counter();
+
+ // Followers block
+ std::printf(" Followers ");
+ render_next_event_counter();
+
+ const auto render_followers_state = [&](const char* name,
+ const std::function<int (std::uint8_t)> value_getter,
+ const std::function<CLIColor (std::uint8_t)> color_getter)
+ {
+ std::printf("%-17s", name);
+ for (std::uint8_t i = 0; i < 4; i++)
+ {
+ if (i < (report.cluster_size - 1))
+ {
+ CLIColorizer colorizer(color_getter(i));
+ const auto value = value_getter(i);
+ if (value >= 0)
+ {
+ std::printf("%-5d", value);
+ }
+ else
+ {
+ std::printf("N/A ");
+ }
+ }
+ else
+ {
+ std::printf(" ");
+ }
+ }
+ render_next_event_counter();
+ };
+
+ const auto follower_color_getter = [&](std::uint8_t i)
+ {
+ if (report.state != RaftCore::ServerStateLeader) { return CLIColor::Default; }
+ if (!report.followers[i].node_id.isValid()) { return CLIColor::Red; }
+ if (report.followers[i].match_index != report.last_log_index ||
+ report.followers[i].next_index <= report.last_log_index)
+ {
+ return CLIColor::Magenta;
+ }
+ return CLIColor::Default;
+ };
+
+ render_followers_state("Node ID", [&](std::uint8_t i)
+ {
+ const auto nid = report.followers[i].node_id;
+ return nid.isValid() ? nid.get() : -1;
+ },
+ follower_color_getter);
+
+ render_followers_state("Next index",
+ [&](std::uint8_t i) { return report.followers[i].next_index; },
+ follower_color_getter);
+
+ render_followers_state("Match index",
+ [&](std::uint8_t i) { return report.followers[i].match_index; },
+ follower_color_getter);
+
+ assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed
+
+ // Separator
+ std::printf("--------------------------------------+----------------------------------------\n");
+
+ // Event log
+ std::printf("%s\n", EventTracer::RecentEvent::getTableHeader());
+ const int num_events_to_render = static_cast<int>(num_rows) -
+ static_cast<int>(next_relevant_event_index) -
+ static_cast<int>(NumRowsWithoutEvents) -
+ 1; // This allows to keep the last line empty for stdout or UAVCAN_TRACE()
+ for (int i = 0;
+ i < num_events_to_render && i < static_cast<int>(event_tracer.getNumEvents());
+ i++)
+ {
+ const auto e = event_tracer.getEventByIndex(i);
+ CLIColorizer colorizer(getColorHash(static_cast<unsigned>(e.code)));
+ std::printf("%s\n", e.toString().c_str());
+ }
+
+ std::fflush(stdout);
+}
+
+
+void runForever(const uavcan_linux::NodePtr& node,
+ const std::uint8_t cluster_size,
+ const std::string& event_log_file,
+ const std::string& persistent_storage_path)
+{
+ /*
+ * Event tracer
+ */
+ EventTracer event_tracer(MaxNumLastEvents);
+ ENFORCE(0 <= event_tracer.init(event_log_file.c_str()));
+
+ /*
+ * Storage backend
+ */
+ uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
+ ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str()));
+
+ /*
+ * Server
+ */
+ uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer);
+
+ const int server_init_res = server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size);
+ if (server_init_res < 0)
+ {
+ throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res));
+ }
+
+ /*
+ * Preparing the CLI
+ */
+ std::printf("\x1b[2J"); // Clear entire screen; this will preserve initialization output in the scrollback
+
+ /*
+ * Spinning the node
+ */
+ uavcan::MonotonicTime last_redraw_at;
+
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval));
+ if (res < 0)
+ {
+ std::cerr << "Spin error: " << res << std::endl;
+ }
+
+ const auto ts = node->getMonotonicTime();
+
+ if (event_tracer.hadEvents() || (ts - last_redraw_at).toMSec() > 1000)
+ {
+ last_redraw_at = ts;
+ redraw(node, ts, event_tracer, server);
+ }
+ }
+}
+
+struct Options
+{
+ uavcan::NodeID node_id;
+ std::vector<std::string> ifaces;
+ std::uint8_t cluster_size = 0;
+ std::string storage_path;
+};
+
+Options parseOptions(int argc, const char** argv)
+{
+ const char* const executable_name = *argv++;
+ argc--;
+
+ const auto enforce = [executable_name](bool condition, const char* error_text) {
+ if (!condition)
+ {
+ std::cerr << error_text << "\n"
+ << "Usage:\n\t"
+ << executable_name
+ << " <node-id> <can-iface-name-1> [can-iface-name-N...] [-c <cluster-size>] -s <storage-path>"
+ << std::endl;
+ std::exit(1);
+ }
+ };
+
+ enforce(argc >= 3, "Not enough arguments");
+
+ /*
+ * Node ID is always at the first position
+ */
+ argc--;
+ const int node_id = std::stoi(*argv++);
+ enforce(node_id >= 1 && node_id <= 127, "Invalid node ID");
+
+ Options out;
+ out.node_id = uavcan::NodeID(std::uint8_t(node_id));
+
+ while (argc --> 0)
+ {
+ const std::string token(*argv++);
+
+ if (token[0] != '-')
+ {
+ out.ifaces.push_back(token);
+ }
+ else if (token[1] == 'c')
+ {
+ int cluster_size = 0;
+ if (token.length() > 2) // -c2
+ {
+ cluster_size = std::stoi(token.c_str() + 2);
+ }
+ else // -c 2
+ {
+ enforce(argc --> 0, "Expected cluster size");
+ cluster_size = std::stoi(*argv++);
+ }
+ enforce(cluster_size >= 1 &&
+ cluster_size <= uavcan::dynamic_node_id_server::distributed::ClusterManager::MaxClusterSize,
+ "Invalid cluster size");
+ out.cluster_size = std::uint8_t(cluster_size);
+ }
+ else if (token[1] == 's')
+ {
+ if (token.length() > 2) // -s/foo/bar
+ {
+ out.storage_path = token.c_str() + 2;
+ }
+ else // -s /foo/bar
+ {
+ enforce(argc --> 0, "Expected storage path");
+ out.storage_path = *argv++;
+ }
+ }
+ else
+ {
+ enforce(false, "Unexpected argument");
+ }
+ }
+
+ enforce(!out.storage_path.empty(), "Invalid storage path");
+
+ return out;
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ std::srand(std::time(nullptr));
+
+ if (isatty(STDOUT_FILENO) != 1)
+ {
+ std::cerr << "This application cannot run if stdout is not associated with a terminal" << std::endl;
+ std::exit(1);
+ }
+
+ auto options = parseOptions(argc, argv);
+
+ std::cout << "Self node ID: " << int(options.node_id.get()) << "\n"
+ "Cluster size: " << int(options.cluster_size) << "\n"
+ "Storage path: " << options.storage_path << "\n"
+ "Num ifaces: " << options.ifaces.size() << "\n"
+#ifdef NDEBUG
+ "Build mode: Release"
+#else
+ "Build mode: Debug"
+#endif
+ << std::endl;
+
+ /*
+ * Preparing the storage directory
+ */
+ options.storage_path += "/node_" + std::to_string(options.node_id.get());
+
+ int system_res = std::system(("mkdir -p '" + options.storage_path + "' &>/dev/null").c_str());
+ (void)system_res;
+
+ const auto event_log_file = options.storage_path + "/events.log";
+ const auto storage_path = options.storage_path + "/storage/";
+
+ /*
+ * Starting the node
+ */
+ auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_app.dynamic_node_id_server");
+ runForever(node, options.cluster_size, event_log_file, storage_path);
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Error: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/uavcan_monitor.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdio>
+#include <bitset>
+#include <unordered_map>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "debug.hpp"
+
+enum class CLIColor : unsigned
+{
+ Red = 31,
+ Green = 32,
+ Yellow = 33,
+ Blue = 34,
+ Magenta = 35,
+ Cyan = 36,
+ White = 37,
+ Default = 39
+};
+
+class CLIColorizer
+{
+ const CLIColor color_;
+public:
+ explicit CLIColorizer(CLIColor c) : color_(c)
+ {
+ std::printf("\033[%um", static_cast<unsigned>(color_));
+ }
+
+ ~CLIColorizer()
+ {
+ std::printf("\033[%um", static_cast<unsigned>(CLIColor::Default));
+ }
+};
+
+class Monitor : public uavcan::NodeStatusMonitor
+{
+ uavcan_linux::TimerPtr timer_;
+ std::unordered_map<int, uavcan::protocol::NodeStatus> status_registry_;
+
+ void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg) override
+ {
+ status_registry_[msg.getSrcNodeID().get()] = msg;
+ }
+
+ static std::pair<CLIColor, std::string> healthToColoredString(const std::uint8_t health)
+ {
+ static const std::unordered_map<std::uint8_t, std::pair<CLIColor, std::string>> map
+ {
+ { uavcan::protocol::NodeStatus::HEALTH_OK, { CLIColor(CLIColor::Green), "OK" }},
+ { uavcan::protocol::NodeStatus::HEALTH_WARNING, { CLIColor(CLIColor::Yellow), "WARNING" }},
+ { uavcan::protocol::NodeStatus::HEALTH_ERROR, { CLIColor(CLIColor::Magenta), "ERROR" }},
+ { uavcan::protocol::NodeStatus::HEALTH_CRITICAL, { CLIColor(CLIColor::Red), "CRITICAL" }}
+ };
+ try
+ {
+ return map.at(health);
+ }
+ catch (std::out_of_range&)
+ {
+ return { CLIColor(CLIColor::Red), std::to_string(health) };
+ }
+ }
+
+ static std::pair<CLIColor, std::string> modeToColoredString(const std::uint8_t mode)
+ {
+ static const std::unordered_map<std::uint8_t, std::pair<CLIColor, std::string>> map
+ {
+ { uavcan::protocol::NodeStatus::MODE_OPERATIONAL, { CLIColor(CLIColor::Green), "OPERATIONAL" }},
+ { uavcan::protocol::NodeStatus::MODE_INITIALIZATION, { CLIColor(CLIColor::Yellow), "INITIALIZATION" }},
+ { uavcan::protocol::NodeStatus::MODE_MAINTENANCE, { CLIColor(CLIColor::Cyan), "MAINTENANCE" }},
+ { uavcan::protocol::NodeStatus::MODE_SOFTWARE_UPDATE, { CLIColor(CLIColor::Magenta), "SOFTWARE_UPDATE" }},
+ { uavcan::protocol::NodeStatus::MODE_OFFLINE, { CLIColor(CLIColor::Red), "OFFLINE" }}
+ };
+ try
+ {
+ return map.at(mode);
+ }
+ catch (std::out_of_range&)
+ {
+ return { CLIColor(CLIColor::Red), std::to_string(mode) };
+ }
+ }
+
+ void printStatusLine(const uavcan::NodeID nid, const uavcan::NodeStatusMonitor::NodeStatus& status)
+ {
+ const auto health_and_color = healthToColoredString(status.health);
+ const auto mode_and_color = modeToColoredString(status.mode);
+
+ const int nid_int = nid.get();
+ const unsigned long uptime = status_registry_[nid_int].uptime_sec;
+ const unsigned vendor_code = status_registry_[nid_int].vendor_specific_status_code;
+
+ std::printf(" %-3d |", nid_int);
+ {
+ CLIColorizer clz(mode_and_color.first);
+ std::printf(" %-15s ", mode_and_color.second.c_str());
+ }
+ std::printf("|");
+ {
+ CLIColorizer clz(health_and_color.first);
+ std::printf(" %-8s ", health_and_color.second.c_str());
+ }
+ std::printf("| %-10lu | %04x %s'%s %u\n", uptime, vendor_code,
+ std::bitset<8>((vendor_code >> 8) & 0xFF).to_string().c_str(),
+ std::bitset<8>(vendor_code).to_string().c_str(),
+ vendor_code);
+ }
+
+ void redraw(const uavcan::TimerEvent&)
+ {
+ std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning
+ std::printf("\x1b[H"); // Move cursor to the coordinates 1,1
+ std::printf(" NID | Mode | Health | Uptime [s] | Vendor-specific status code\n");
+ std::printf("-----+-----------------+----------+------------+-hex---bin----------------dec--\n");
+
+ for (unsigned i = 1; i <= uavcan::NodeID::Max; i++)
+ {
+ if (isNodeKnown(i))
+ {
+ printStatusLine(i, getNodeStatus(i));
+ }
+ }
+ }
+
+public:
+ explicit Monitor(uavcan_linux::NodePtr node)
+ : uavcan::NodeStatusMonitor(*node)
+ , timer_(node->makeTimer(uavcan::MonotonicDuration::fromMSec(500),
+ std::bind(&Monitor::redraw, this, std::placeholders::_1)))
+ { }
+};
+
+
+static uavcan_linux::NodePtr initNodeInPassiveMode(const std::vector<std::string>& ifaces, const std::string& node_name)
+{
+ auto node = uavcan_linux::makeNode(ifaces, node_name.c_str(),
+ uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion());
+ node->setModeOperational();
+ return node;
+}
+
+static void runForever(const uavcan_linux::NodePtr& node)
+{
+ Monitor mon(node);
+ ENFORCE(0 == mon.start());
+ while (true)
+ {
+ const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
+ if (res < 0)
+ {
+ node->logError("spin", "Error %*", res);
+ }
+ }
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ if (argc < 2)
+ {
+ std::cerr << "Usage:\n\t" << argv[0] << " <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+ return 1;
+ }
+ std::vector<std::string> iface_names;
+ for (int i = 1; i < argc; i++)
+ {
+ iface_names.emplace_back(argv[i]);
+ }
+ uavcan_linux::NodePtr node = initNodeInPassiveMode(iface_names, "org.uavcan.linux_app.node_status_monitor");
+ runForever(node);
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Error: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/apps/uavcan_nodetool.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,300 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <mutex>
+#include <map>
+#include <algorithm>
+#include <iterator>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include "debug.hpp"
+
+#include <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/ExecuteOpcode.hpp>
+#include <uavcan/equipment/hardpoint/Command.hpp>
+
+namespace
+{
+
+class StdinLineReader
+{
+ mutable std::mutex mutex_;
+ std::thread thread_;
+ std::queue<std::string> queue_;
+
+ void worker()
+ {
+ while (true)
+ {
+ std::string input;
+ std::getline(std::cin, input);
+ std::lock_guard<std::mutex> lock(mutex_);
+ queue_.push(input);
+ }
+ }
+
+public:
+ StdinLineReader()
+ : thread_(&StdinLineReader::worker, this)
+ {
+ thread_.detach();
+ }
+
+ bool hasPendingInput() const
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ return !queue_.empty();
+ }
+
+ std::string getLine()
+ {
+ std::lock_guard<std::mutex> lock(mutex_);
+ if (queue_.empty())
+ {
+ throw std::runtime_error("Input queue is empty");
+ }
+ auto ret = queue_.front();
+ queue_.pop();
+ return ret;
+ }
+
+ std::vector<std::string> getSplitLine()
+ {
+ std::istringstream iss(getLine());
+ std::vector<std::string> out;
+ std::copy(std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>(),
+ std::back_inserter(out));
+ return out;
+ }
+};
+
+uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
+{
+ auto node = uavcan_linux::makeNode(ifaces, name.c_str(),
+ uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion(),
+ nid);
+ node->setModeOperational();
+ return node;
+}
+
+template <typename DataType>
+typename DataType::Response call(uavcan_linux::BlockingServiceClient<DataType>& client,
+ uavcan::NodeID server_node_id, const typename DataType::Request& request)
+{
+ const int res = client.blockingCall(server_node_id, request, uavcan::MonotonicDuration::fromMSec(100));
+ ENFORCE(res >= 0);
+ ENFORCE(client.wasSuccessful());
+ return client.getResponse();
+}
+
+/*
+ * Command table.
+ * The structure is:
+ * command_name : (command_usage_info, command_entry_point)
+ * This code was written while listening to some bad dubstep so I'm not sure about its quality.
+ */
+const std::map<std::string,
+ std::pair<std::string,
+ std::function<void(const uavcan_linux::NodePtr&, const uavcan::NodeID,
+ const std::vector<std::string>&)>
+ >
+ > commands =
+{
+ {
+ "param",
+ {
+ "No arguments supplied - requests all params from a remote node\n"
+ "<param_name> <param_value> - assigns parameter <param_name> to value <param_value>",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>& args)
+ {
+ auto client = node->makeBlockingServiceClient<uavcan::protocol::param::GetSet>();
+ uavcan::protocol::param::GetSet::Request request;
+ if (args.empty())
+ {
+ while (true)
+ {
+ auto response = call(*client, node_id, request);
+ if (response.name.empty())
+ {
+ break;
+ }
+ std::cout
+ << response
+ << "\n" << std::string(80, '-')
+ << std::endl;
+ request.index++;
+ }
+ }
+ else
+ {
+ request.name = args.at(0).c_str();
+ // TODO: add support for string parameters
+ request.value.to<uavcan::protocol::param::Value::Tag::real_value>() = std::stof(args.at(1));
+ std::cout << call(*client, node_id, request) << std::endl;
+ }
+ }
+ }
+ },
+ {
+ "param_save",
+ {
+ "Calls uavcan.protocol.param.ExecuteOpcode on a remote node with OPCODE_SAVE",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
+ {
+ auto client = node->makeBlockingServiceClient<uavcan::protocol::param::ExecuteOpcode>();
+ uavcan::protocol::param::ExecuteOpcode::Request request;
+ request.opcode = request.OPCODE_SAVE;
+ std::cout << call(*client, node_id, request) << std::endl;
+ }
+ }
+ },
+ {
+ "param_erase",
+ {
+ "Calls uavcan.protocol.param.ExecuteOpcode on a remote node with OPCODE_ERASE",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
+ {
+ auto client = node->makeBlockingServiceClient<uavcan::protocol::param::ExecuteOpcode>();
+ uavcan::protocol::param::ExecuteOpcode::Request request;
+ request.opcode = request.OPCODE_ERASE;
+ std::cout << call(*client, node_id, request) << std::endl;
+ }
+ }
+ },
+ {
+ "restart",
+ {
+ "Restarts a remote node using uavcan.protocol.RestartNode",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
+ {
+ auto client = node->makeBlockingServiceClient<uavcan::protocol::RestartNode>();
+ uavcan::protocol::RestartNode::Request request;
+ request.magic_number = request.MAGIC_NUMBER;
+ (void)client->blockingCall(node_id, request);
+ if (client->wasSuccessful())
+ {
+ std::cout << client->getResponse() << std::endl;
+ }
+ else
+ {
+ std::cout << "<NO RESPONSE>" << std::endl;
+ }
+ }
+ }
+ },
+ {
+ "info",
+ {
+ "Calls uavcan.protocol.GetNodeInfo on a remote node",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
+ {
+ auto client = node->makeBlockingServiceClient<uavcan::protocol::GetNodeInfo>();
+ std::cout << call(*client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl;
+ }
+ }
+ },
+ {
+ "transport_stats",
+ {
+ "Calls uavcan.protocol.GetTransportStats on a remote node",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID node_id, const std::vector<std::string>&)
+ {
+ auto client = node->makeBlockingServiceClient<uavcan::protocol::GetTransportStats>();
+ std::cout << call(*client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl;
+ }
+ }
+ },
+ {
+ "hardpoint",
+ {
+ "Publishes uavcan.equipment.hardpoint.Command\n"
+ "Expected argument: command",
+ [](const uavcan_linux::NodePtr& node, const uavcan::NodeID, const std::vector<std::string>& args)
+ {
+ uavcan::equipment::hardpoint::Command msg;
+ msg.command = std::stoi(args.at(0));
+ auto pub = node->makePublisher<uavcan::equipment::hardpoint::Command>();
+ (void)pub->broadcast(msg);
+ }
+ }
+ }
+};
+
+void runForever(const uavcan_linux::NodePtr& node)
+{
+ StdinLineReader stdin_reader;
+ std::cout << "> " << std::flush;
+ while (true)
+ {
+ ENFORCE(node->spin(uavcan::MonotonicDuration::fromMSec(10)) >= 0);
+ if (!stdin_reader.hasPendingInput())
+ {
+ continue;
+ }
+ const auto words = stdin_reader.getSplitLine();
+ bool command_is_known = false;
+
+ try
+ {
+ if (words.size() >= 2)
+ {
+ const auto cmd = words.at(0);
+ const uavcan::NodeID node_id(std::stoi(words.at(1)));
+ auto it = commands.find(cmd);
+ if (it != std::end(commands))
+ {
+ command_is_known = true;
+ it->second.second(node, node_id, std::vector<std::string>(words.begin() + 2, words.end()));
+ }
+ }
+ }
+ catch (std::exception& ex)
+ {
+ std::cout << "FAILURE\n" << ex.what() << std::endl;
+ }
+
+ if (!command_is_known)
+ {
+ std::cout << "<command> <remote node id> [args...]\n";
+ std::cout << "Say 'help' to get help.\n"; // I'll show myself out.
+
+ if (!words.empty() && words.at(0) == "help")
+ {
+ std::cout << "Usage:\n\n";
+ for (auto& cmd : commands)
+ {
+ std::cout << cmd.first << "\n" << cmd.second.first << "\n\n";
+ }
+ }
+ }
+ std::cout << "> " << std::flush;
+ }
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+ try
+ {
+ if (argc < 3)
+ {
+ std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
+ return 1;
+ }
+ const int self_node_id = std::stoi(argv[1]);
+ const std::vector<std::string> iface_names(argv + 2, argv + argc);
+ uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_app.nodetool");
+ runForever(node);
+ return 0;
+ }
+ catch (const std::exception& ex)
+ {
+ std::cerr << "Error: " << ex.what() << std::endl;
+ return 1;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/linux/cppcheck.sh Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,11 @@ +#!/bin/sh + +num_cores=$(grep -c ^processor /proc/cpuinfo) +if [ -z "$num_cores" ]; then + echo "num_cores=? WTF?" + num_cores=4 +fi + +cppcheck . --error-exitcode=1 --quiet --enable=all --platform=unix64 --std=c99 --std=c++11 \ + --inline-suppr --force --template=gcc -j$num_cores \ + -Iinclude $@
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/include/uavcan_linux/clock.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,196 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <ctime>
+#include <cstdint>
+
+#include <unistd.h>
+#include <sys/time.h>
+#include <sys/types.h>
+
+#include <uavcan/driver/system_clock.hpp>
+#include <uavcan_linux/exception.hpp>
+
+namespace uavcan_linux
+{
+/**
+ * Different adjustment modes can be used for time synchronization
+ */
+enum class ClockAdjustmentMode
+{
+ SystemWide, ///< Adjust the clock globally for the whole system; requires root privileges
+ PerDriverPrivate ///< Adjust the clock only for the current driver instance
+};
+
+/**
+ * Linux system clock driver.
+ * Requires librt.
+ */
+class SystemClock : public uavcan::ISystemClock
+{
+ uavcan::UtcDuration private_adj_;
+ uavcan::UtcDuration gradual_adj_limit_;
+ const ClockAdjustmentMode adj_mode_;
+ std::uint64_t step_adj_cnt_;
+ std::uint64_t gradual_adj_cnt_;
+
+ static constexpr std::int64_t Int1e6 = 1000000;
+ static constexpr std::uint64_t UInt1e6 = 1000000;
+
+ bool performStepAdjustment(const uavcan::UtcDuration adjustment)
+ {
+ step_adj_cnt_++;
+ const std::int64_t usec = adjustment.toUSec();
+ timeval tv;
+ if (gettimeofday(&tv, NULL) != 0)
+ {
+ return false;
+ }
+ tv.tv_sec += usec / Int1e6;
+ tv.tv_usec += usec % Int1e6;
+ return settimeofday(&tv, nullptr) == 0;
+ }
+
+ bool performGradualAdjustment(const uavcan::UtcDuration adjustment)
+ {
+ gradual_adj_cnt_++;
+ const std::int64_t usec = adjustment.toUSec();
+ timeval tv;
+ tv.tv_sec = usec / Int1e6;
+ tv.tv_usec = usec % Int1e6;
+ return adjtime(&tv, nullptr) == 0;
+ }
+
+public:
+ /**
+ * By default, the clock adjustment mode will be selected automatically - global if root, private otherwise.
+ */
+ explicit SystemClock(ClockAdjustmentMode adj_mode = detectPreferredClockAdjustmentMode())
+ : gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000))
+ , adj_mode_(adj_mode)
+ , step_adj_cnt_(0)
+ , gradual_adj_cnt_(0)
+ { }
+
+ /**
+ * Returns monotonic timestamp from librt.
+ * @throws uavcan_linux::Exception.
+ */
+ uavcan::MonotonicTime getMonotonic() const override
+ {
+ timespec ts;
+ if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
+ {
+ throw Exception("Failed to get monotonic time");
+ }
+ return uavcan::MonotonicTime::fromUSec(std::uint64_t(ts.tv_sec) * UInt1e6 + ts.tv_nsec / 1000);
+ }
+
+ /**
+ * Returns wall time from gettimeofday().
+ * @throws uavcan_linux::Exception.
+ */
+ uavcan::UtcTime getUtc() const override
+ {
+ timeval tv;
+ if (gettimeofday(&tv, NULL) != 0)
+ {
+ throw Exception("Failed to get UTC time");
+ }
+ uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * UInt1e6 + tv.tv_usec);
+ if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
+ {
+ utc += private_adj_;
+ }
+ return utc;
+ }
+
+ /**
+ * Adjusts the wall clock.
+ * Behavior depends on the selected clock adjustment mode - @ref ClockAdjustmentMode.
+ * Clock adjustment mode can be set only once via constructor.
+ *
+ * If the system wide adjustment mode is selected, two ways for performing adjustment exist:
+ * - Gradual adjustment using adjtime(), if the phase error is less than gradual adjustment limit.
+ * - Step adjustment using settimeofday(), if the phase error is above gradual adjustment limit.
+ * The gradual adjustment limit can be configured at any time via the setter method.
+ *
+ * @throws uavcan_linux::Exception.
+ */
+ void adjustUtc(const uavcan::UtcDuration adjustment) override
+ {
+ if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
+ {
+ private_adj_ += adjustment;
+ }
+ else
+ {
+ assert(private_adj_.isZero());
+ assert(!gradual_adj_limit_.isNegative());
+
+ bool success = false;
+ if (adjustment.getAbs() < gradual_adj_limit_)
+ {
+ success = performGradualAdjustment(adjustment);
+ }
+ else
+ {
+ success = performStepAdjustment(adjustment);
+ }
+ if (!success)
+ {
+ throw Exception("Clock adjustment failed");
+ }
+ }
+ }
+
+ /**
+ * Sets the maximum phase error to use adjtime().
+ * If the phase error exceeds this value, settimeofday() will be used instead.
+ */
+ void setGradualAdjustmentLimit(uavcan::UtcDuration limit)
+ {
+ if (limit.isNegative())
+ {
+ limit = uavcan::UtcDuration();
+ }
+ gradual_adj_limit_ = limit;
+ }
+
+ uavcan::UtcDuration getGradualAdjustmentLimit() const { return gradual_adj_limit_; }
+
+ ClockAdjustmentMode getAdjustmentMode() const { return adj_mode_; }
+
+ /**
+ * This is only applicable if the selected clock adjustment mode is private.
+ * In system wide mode this method will always return zero duration.
+ */
+ uavcan::UtcDuration getPrivateAdjustment() const { return private_adj_; }
+
+ /**
+ * Statistics that allows to evaluate clock sync preformance.
+ */
+ std::uint64_t getStepAdjustmentCount() const { return step_adj_cnt_; }
+ std::uint64_t getGradualAdjustmentCount() const { return gradual_adj_cnt_; }
+ std::uint64_t getAdjustmentCount() const
+ {
+ return getStepAdjustmentCount() + getGradualAdjustmentCount();
+ }
+
+ /**
+ * This static method decides what is the optimal clock sync adjustment mode for the current configuration.
+ * It selects system wide mode if the application is running as root; otherwise it prefers
+ * the private adjustment mode because the system wide mode requires root privileges.
+ */
+ static ClockAdjustmentMode detectPreferredClockAdjustmentMode()
+ {
+ const bool godmode = geteuid() == 0;
+ return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate;
+ }
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cerrno>
+#include <cstring>
+#include <stdexcept>
+
+namespace uavcan_linux
+{
+/**
+ * This is the root exception class for all exceptions that can be thrown from the libuavcan Linux driver.
+ */
+class Exception : public std::runtime_error
+{
+ const int errno_;
+
+ static std::string makeErrorString(const std::string& descr, int use_errno)
+ {
+ return descr + " [errno " + std::to_string(use_errno) + " \"" + std::strerror(use_errno) + "\"]";
+ }
+
+public:
+ explicit Exception(const std::string& descr, int use_errno = errno)
+ : std::runtime_error(makeErrorString(descr, use_errno))
+ , errno_(use_errno)
+ { }
+
+ /**
+ * Returns standard UNIX errno value captured at the moment
+ * when this exception object was constructed.
+ */
+ int getErrno() const { return errno_; }
+};
+
+/**
+ * This type is thrown when a Libuavcan API method exits with error.
+ * The error code is stored in the exception object and is avialable via @ref getLibuavcanErrorCode().
+ */
+class LibuavcanErrorException : public Exception
+{
+ const std::int16_t error_;
+
+ static std::string makeErrorString(std::int16_t e)
+ {
+ return "Libuavcan error (" + std::to_string(e) + ")";
+ }
+
+public:
+ explicit LibuavcanErrorException(std::int16_t uavcan_error_code) :
+ Exception(makeErrorString(uavcan_error_code)),
+ error_(std::abs(uavcan_error_code))
+ { }
+
+ std::int16_t getLibuavcanErrorCode() const { return error_; }
+};
+
+/**
+ * This exception is thrown when all available interfaces become down.
+ */
+class AllIfacesDownException : public Exception
+{
+public:
+ AllIfacesDownException() : Exception("All ifaces are down", ENETDOWN) { }
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,473 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <chrono>
+#include <iostream>
+#include <sstream>
+#include <uavcan/uavcan.hpp>
+#include <uavcan/node/sub_node.hpp>
+
+namespace uavcan_linux
+{
+/**
+ * Default log sink will dump everything into stderr.
+ * It is installed by default.
+ */
+class DefaultLogSink : public uavcan::ILogSink
+{
+ void log(const uavcan::protocol::debug::LogMessage& message) override
+ {
+ const auto tt = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+ const auto tstr = std::ctime(&tt);
+ std::cerr << "### UAVCAN " << tstr << message << std::endl;
+ }
+};
+
+/**
+ * Wrapper over uavcan::ServiceClient<> for blocking calls.
+ * Blocks on uavcan::Node::spin() internally until the call is complete.
+ */
+template <typename DataType>
+class BlockingServiceClient : public uavcan::ServiceClient<DataType>
+{
+ typedef uavcan::ServiceClient<DataType> Super;
+
+ typename DataType::Response response_;
+ bool call_was_successful_;
+
+ void callback(const uavcan::ServiceCallResult<DataType>& res)
+ {
+ response_ = res.getResponse();
+ call_was_successful_ = res.isSuccessful();
+ }
+
+ void setup()
+ {
+ Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1));
+ call_was_successful_ = false;
+ response_ = typename DataType::Response();
+ }
+
+public:
+ BlockingServiceClient(uavcan::INode& node)
+ : uavcan::ServiceClient<DataType>(node)
+ , call_was_successful_(false)
+ {
+ setup();
+ }
+
+ /**
+ * Performs a blocking service call using default timeout (see the specs).
+ * Use @ref getResponse() to get the actual response.
+ * Returns negative error code.
+ */
+ int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request)
+ {
+ return blockingCall(server_node_id, request, Super::getDefaultRequestTimeout());
+ }
+
+ /**
+ * Performs a blocking service call using the specified timeout. Please consider using default timeout instead.
+ * Use @ref getResponse() to get the actual response.
+ * Returns negative error code.
+ */
+ int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request,
+ uavcan::MonotonicDuration timeout)
+ {
+ const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2);
+ setup();
+ Super::setRequestTimeout(timeout);
+ const int call_res = Super::call(server_node_id, request);
+ if (call_res >= 0)
+ {
+ while (Super::hasPendingCalls())
+ {
+ const int spin_res = Super::getNode().spin(SpinDuration);
+ if (spin_res < 0)
+ {
+ return spin_res;
+ }
+ }
+ }
+ return call_res;
+ }
+
+ /**
+ * Whether the last blocking call was successful.
+ */
+ bool wasSuccessful() const { return call_was_successful_; }
+
+ /**
+ * Use this to retrieve the response of the last blocking service call.
+ * This method returns default constructed response object if the last service call was unsuccessful.
+ */
+ const typename DataType::Response& getResponse() const { return response_; }
+};
+
+/**
+ * Contains all drivers needed for uavcan::Node.
+ */
+struct DriverPack
+{
+ SystemClock clock;
+ std::shared_ptr<uavcan::ICanDriver> can;
+
+ explicit DriverPack(ClockAdjustmentMode clock_adjustment_mode,
+ const std::shared_ptr<uavcan::ICanDriver>& can_driver)
+ : clock(clock_adjustment_mode)
+ , can(can_driver)
+ { }
+
+ explicit DriverPack(ClockAdjustmentMode clock_adjustment_mode,
+ const std::vector<std::string>& iface_names)
+ : clock(clock_adjustment_mode)
+ {
+ std::shared_ptr<SocketCanDriver> socketcan(new SocketCanDriver(clock));
+ can = socketcan;
+ for (auto ifn : iface_names)
+ {
+ if (socketcan->addIface(ifn) < 0)
+ {
+ throw Exception("Failed to add iface " + ifn);
+ }
+ }
+ }
+};
+
+typedef std::shared_ptr<DriverPack> DriverPackPtr;
+
+typedef std::shared_ptr<uavcan::INode> INodePtr;
+
+typedef std::shared_ptr<uavcan::Timer> TimerPtr;
+
+template <typename T>
+using SubscriberPtr = std::shared_ptr<uavcan::Subscriber<T>>;
+
+template <typename T>
+using PublisherPtr = std::shared_ptr<uavcan::Publisher<T>>;
+
+template <typename T>
+using ServiceServerPtr = std::shared_ptr<uavcan::ServiceServer<T>>;
+
+template <typename T>
+using ServiceClientPtr = std::shared_ptr<uavcan::ServiceClient<T>>;
+
+template <typename T>
+using BlockingServiceClientPtr = std::shared_ptr<BlockingServiceClient<T>>;
+
+static constexpr std::size_t NodeMemPoolSize = 1024 * 512; ///< This shall be enough for any possible use case
+
+/**
+ * Generic wrapper for node objects with some additional convenience functions.
+ */
+template <typename NodeType>
+class NodeBase : public NodeType
+{
+protected:
+ DriverPackPtr driver_pack_;
+
+ static void enforce(int error, const std::string& msg)
+ {
+ if (error < 0)
+ {
+ std::ostringstream os;
+ os << msg << " [" << error << "]";
+ throw Exception(os.str());
+ }
+ }
+
+ template <typename DataType>
+ static std::string getDataTypeName()
+ {
+ return DataType::getDataTypeFullName();
+ }
+
+public:
+ /**
+ * Simple forwarding constructor, compatible with uavcan::Node.
+ */
+ NodeBase(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) :
+ NodeType(can_driver, clock)
+ { }
+
+ /**
+ * Takes ownership of the driver container via the shared pointer.
+ */
+ explicit NodeBase(DriverPackPtr driver_pack)
+ : NodeType(*driver_pack->can, driver_pack->clock)
+ , driver_pack_(driver_pack)
+ { }
+
+ /**
+ * Allocates @ref uavcan::Subscriber in the heap using shared pointer.
+ * The subscriber will be started immediately.
+ * @throws uavcan_linux::Exception.
+ */
+ template <typename DataType>
+ SubscriberPtr<DataType> makeSubscriber(const typename uavcan::Subscriber<DataType>::Callback& cb)
+ {
+ SubscriberPtr<DataType> p(new uavcan::Subscriber<DataType>(*this));
+ enforce(p->start(cb), "Subscriber start failure " + getDataTypeName<DataType>());
+ return p;
+ }
+
+ /**
+ * Allocates @ref uavcan::Publisher in the heap using shared pointer.
+ * The publisher will be initialized immediately.
+ * @throws uavcan_linux::Exception.
+ */
+ template <typename DataType>
+ PublisherPtr<DataType> makePublisher(uavcan::MonotonicDuration tx_timeout =
+ uavcan::Publisher<DataType>::getDefaultTxTimeout())
+ {
+ PublisherPtr<DataType> p(new uavcan::Publisher<DataType>(*this));
+ enforce(p->init(), "Publisher init failure " + getDataTypeName<DataType>());
+ p->setTxTimeout(tx_timeout);
+ return p;
+ }
+
+ /**
+ * Allocates @ref uavcan::ServiceServer in the heap using shared pointer.
+ * The server will be started immediately.
+ * @throws uavcan_linux::Exception.
+ */
+ template <typename DataType>
+ ServiceServerPtr<DataType> makeServiceServer(const typename uavcan::ServiceServer<DataType>::Callback& cb)
+ {
+ ServiceServerPtr<DataType> p(new uavcan::ServiceServer<DataType>(*this));
+ enforce(p->start(cb), "ServiceServer start failure " + getDataTypeName<DataType>());
+ return p;
+ }
+
+ /**
+ * Allocates @ref uavcan::ServiceClient in the heap using shared pointer.
+ * The service client will be initialized immediately.
+ * @throws uavcan_linux::Exception.
+ */
+ template <typename DataType>
+ ServiceClientPtr<DataType> makeServiceClient(const typename uavcan::ServiceClient<DataType>::Callback& cb)
+ {
+ ServiceClientPtr<DataType> p(new uavcan::ServiceClient<DataType>(*this));
+ enforce(p->init(), "ServiceClient init failure " + getDataTypeName<DataType>());
+ p->setCallback(cb);
+ return p;
+ }
+
+ /**
+ * Allocates @ref uavcan_linux::BlockingServiceClient in the heap using shared pointer.
+ * The service client will be initialized immediately.
+ * @throws uavcan_linux::Exception.
+ */
+ template <typename DataType>
+ BlockingServiceClientPtr<DataType> makeBlockingServiceClient()
+ {
+ BlockingServiceClientPtr<DataType> p(new BlockingServiceClient<DataType>(*this));
+ enforce(p->init(), "BlockingServiceClient init failure " + getDataTypeName<DataType>());
+ return p;
+ }
+
+ /**
+ * Allocates @ref uavcan::Timer in the heap using shared pointer.
+ * The timer will be started immediately in one-shot mode.
+ */
+ TimerPtr makeTimer(uavcan::MonotonicTime deadline, const typename uavcan::Timer::Callback& cb)
+ {
+ TimerPtr p(new uavcan::Timer(*this));
+ p->setCallback(cb);
+ p->startOneShotWithDeadline(deadline);
+ return p;
+ }
+
+ /**
+ * Allocates @ref uavcan::Timer in the heap using shared pointer.
+ * The timer will be started immediately in periodic mode.
+ */
+ TimerPtr makeTimer(uavcan::MonotonicDuration period, const typename uavcan::Timer::Callback& cb)
+ {
+ TimerPtr p(new uavcan::Timer(*this));
+ p->setCallback(cb);
+ p->startPeriodic(period);
+ return p;
+ }
+
+ const DriverPackPtr& getDriverPack() const { return driver_pack_; }
+ DriverPackPtr& getDriverPack() { return driver_pack_; }
+};
+
+/**
+ * Wrapper for uavcan::Node with some additional convenience functions.
+ * Note that this wrapper adds stderr log sink to @ref uavcan::Logger, which can be removed if needed.
+ * Do not instantiate this class directly; instead use the factory functions defined below.
+ */
+class Node : public NodeBase<uavcan::Node<NodeMemPoolSize>>
+{
+ typedef NodeBase<uavcan::Node<NodeMemPoolSize>> Base;
+
+ DefaultLogSink log_sink_;
+
+public:
+ /**
+ * Simple forwarding constructor, compatible with uavcan::Node.
+ */
+ Node(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) :
+ Base(can_driver, clock)
+ {
+ getLogger().setExternalSink(&log_sink_);
+ }
+
+ /**
+ * Takes ownership of the driver container via the shared pointer.
+ */
+ explicit Node(DriverPackPtr driver_pack) :
+ Base(driver_pack)
+ {
+ getLogger().setExternalSink(&log_sink_);
+ }
+};
+
+/**
+ * Wrapper for uavcan::SubNode with some additional convenience functions.
+ * Do not instantiate this class directly; instead use the factory functions defined below.
+ */
+class SubNode : public NodeBase<uavcan::SubNode<NodeMemPoolSize>>
+{
+ typedef NodeBase<uavcan::SubNode<NodeMemPoolSize>> Base;
+
+public:
+ /**
+ * Simple forwarding constructor, compatible with uavcan::Node.
+ */
+ SubNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : Base(can_driver, clock) { }
+
+ /**
+ * Takes ownership of the driver container via the shared pointer.
+ */
+ explicit SubNode(DriverPackPtr driver_pack) : Base(driver_pack) { }
+};
+
+typedef std::shared_ptr<Node> NodePtr;
+typedef std::shared_ptr<SubNode> SubNodePtr;
+
+/**
+ * Use this function to create a node instance with default SocketCAN driver.
+ * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0".
+ * Clock adjustment mode will be detected automatically unless provided explicitly.
+ * @throws uavcan_linux::Exception.
+ */
+static inline NodePtr makeNode(const std::vector<std::string>& iface_names,
+ ClockAdjustmentMode clock_adjustment_mode =
+ SystemClock::detectPreferredClockAdjustmentMode())
+{
+ DriverPackPtr dp(new DriverPack(clock_adjustment_mode, iface_names));
+ return NodePtr(new Node(dp));
+}
+
+/**
+ * Use this function to create a node instance with a custom driver.
+ * Clock adjustment mode will be detected automatically unless provided explicitly.
+ * @throws uavcan_linux::Exception.
+ */
+static inline NodePtr makeNode(const std::shared_ptr<uavcan::ICanDriver>& can_driver,
+ ClockAdjustmentMode clock_adjustment_mode =
+ SystemClock::detectPreferredClockAdjustmentMode())
+{
+ DriverPackPtr dp(new DriverPack(clock_adjustment_mode, can_driver));
+ return NodePtr(new Node(dp));
+}
+
+/**
+ * This function extends the other two overloads in such a way that it instantiates and initializes
+ * the node immediately; if initialization fails, it throws.
+ *
+ * If NodeID is not provided, it will not be initialized, and therefore the node will be started in
+ * listen-only (i.e. silent) mode. The node can be switched to normal (i.e. non-silent) mode at any
+ * later time by calling setNodeID() explicitly. Read the Node class docs for more info.
+ *
+ * Clock adjustment mode will be detected automatically unless provided explicitly.
+ *
+ * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException.
+ */
+template <typename DriverType>
+static inline NodePtr makeNode(const DriverType& driver,
+ const uavcan::NodeStatusProvider::NodeName& name,
+ const uavcan::protocol::SoftwareVersion& software_version,
+ const uavcan::protocol::HardwareVersion& hardware_version,
+ const uavcan::NodeID node_id = uavcan::NodeID(),
+ const uavcan::TransferPriority node_status_transfer_priority =
+ uavcan::TransferPriority::Default,
+ ClockAdjustmentMode clock_adjustment_mode =
+ SystemClock::detectPreferredClockAdjustmentMode())
+{
+ NodePtr node = makeNode(driver, clock_adjustment_mode);
+
+ node->setName(name);
+ node->setSoftwareVersion(software_version);
+ node->setHardwareVersion(hardware_version);
+
+ if (node_id.isValid())
+ {
+ node->setNodeID(node_id);
+ }
+
+ const auto res = node->start(node_status_transfer_priority);
+ if (res < 0)
+ {
+ throw LibuavcanErrorException(res);
+ }
+
+ return node;
+}
+
+/**
+ * Use this function to create a sub-node instance with default SocketCAN driver.
+ * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0".
+ * Clock adjustment mode will be detected automatically unless provided explicitly.
+ * @throws uavcan_linux::Exception.
+ */
+static inline SubNodePtr makeSubNode(const std::vector<std::string>& iface_names,
+ ClockAdjustmentMode clock_adjustment_mode =
+ SystemClock::detectPreferredClockAdjustmentMode())
+{
+ DriverPackPtr dp(new DriverPack(clock_adjustment_mode, iface_names));
+ return SubNodePtr(new SubNode(dp));
+}
+
+/**
+ * Use this function to create a sub-node instance with a custom driver.
+ * Clock adjustment mode will be detected automatically unless provided explicitly.
+ * @throws uavcan_linux::Exception.
+ */
+static inline SubNodePtr makeSubNode(const std::shared_ptr<uavcan::ICanDriver>& can_driver,
+ ClockAdjustmentMode clock_adjustment_mode =
+ SystemClock::detectPreferredClockAdjustmentMode())
+{
+ DriverPackPtr dp(new DriverPack(clock_adjustment_mode, can_driver));
+ return SubNodePtr(new SubNode(dp));
+}
+
+/**
+ * This function extends the other two overloads in such a way that it instantiates the node
+ * and sets its Node ID immediately.
+ *
+ * Clock adjustment mode will be detected automatically unless provided explicitly.
+ *
+ * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException.
+ */
+template <typename DriverType>
+static inline SubNodePtr makeSubNode(const DriverType& driver,
+ const uavcan::NodeID node_id,
+ ClockAdjustmentMode clock_adjustment_mode =
+ SystemClock::detectPreferredClockAdjustmentMode())
+{
+ SubNodePtr sub_node = makeSubNode(driver, clock_adjustment_mode);
+ sub_node->setNodeID(node_id);
+ return sub_node;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,804 @@
+/*
+ * Copyright (C) 2014-2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Ilia Sheremet <illia.sheremet@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <queue>
+#include <vector>
+#include <map>
+#include <unordered_set>
+#include <memory>
+#include <algorithm>
+
+#include <fcntl.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <poll.h>
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan_linux/clock.hpp>
+#include <uavcan_linux/exception.hpp>
+
+
+namespace uavcan_linux
+{
+/**
+ * SocketCan driver class keeps number of each kind of errors occurred since the object was created.
+ */
+enum class SocketCanError
+{
+ SocketReadFailure,
+ SocketWriteFailure,
+ TxTimeout
+};
+
+/**
+ * Single SocketCAN socket interface.
+ *
+ * SocketCAN socket adapter maintains TX and RX queues in user space. At any moment socket's buffer contains
+ * no more than 'max_frames_in_socket_tx_queue_' TX frames, rest is waiting in the user space TX queue; when the
+ * socket produces loopback for the previously sent TX frame the next frame from the user space TX queue will
+ * be sent into the socket.
+ *
+ * This approach allows to properly maintain TX timeouts (http://stackoverflow.com/questions/19633015/).
+ * TX timestamping is implemented by means of reading RX timestamps of loopback frames (see "TX timestamping" on
+ * linux-can mailing list, http://permalink.gmane.org/gmane.linux.can/5322).
+ *
+ * Note that if max_frames_in_socket_tx_queue_ is greater than one, frame reordering may occur (depending on the
+ * unrderlying logic).
+ *
+ * This class is too complex and needs to be refactored later. At least, basic socket IO and configuration
+ * should be extracted into a different class.
+ */
+class SocketCanIface : public uavcan::ICanIface
+{
+ static inline ::can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame)
+ {
+ ::can_frame sockcan_frame = ::can_frame();
+ sockcan_frame.can_id = uavcan_frame.id & uavcan::CanFrame::MaskExtID;
+ sockcan_frame.can_dlc = uavcan_frame.dlc;
+ (void)std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
+ if (uavcan_frame.isExtended())
+ {
+ sockcan_frame.can_id |= CAN_EFF_FLAG;
+ }
+ if (uavcan_frame.isErrorFrame())
+ {
+ sockcan_frame.can_id |= CAN_ERR_FLAG;
+ }
+ if (uavcan_frame.isRemoteTransmissionRequest())
+ {
+ sockcan_frame.can_id |= CAN_RTR_FLAG;
+ }
+ return sockcan_frame;
+ }
+
+ static inline uavcan::CanFrame makeUavcanFrame(const ::can_frame& sockcan_frame)
+ {
+ uavcan::CanFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
+ if (sockcan_frame.can_id & CAN_EFF_FLAG)
+ {
+ uavcan_frame.id |= uavcan::CanFrame::FlagEFF;
+ }
+ if (sockcan_frame.can_id & CAN_ERR_FLAG)
+ {
+ uavcan_frame.id |= uavcan::CanFrame::FlagERR;
+ }
+ if (sockcan_frame.can_id & CAN_RTR_FLAG)
+ {
+ uavcan_frame.id |= uavcan::CanFrame::FlagRTR;
+ }
+ return uavcan_frame;
+ }
+
+ struct TxItem
+ {
+ uavcan::CanFrame frame;
+ uavcan::MonotonicTime deadline;
+ uavcan::CanIOFlags flags = 0;
+ std::uint64_t order = 0;
+
+ TxItem(const uavcan::CanFrame& arg_frame, uavcan::MonotonicTime arg_deadline,
+ uavcan::CanIOFlags arg_flags, std::uint64_t arg_order)
+ : frame(arg_frame)
+ , deadline(arg_deadline)
+ , flags(arg_flags)
+ , order(arg_order)
+ { }
+
+ bool operator<(const TxItem& rhs) const
+ {
+ if (frame.priorityLowerThan(rhs.frame))
+ {
+ return true;
+ }
+ if (frame.priorityHigherThan(rhs.frame))
+ {
+ return false;
+ }
+ return order > rhs.order;
+ }
+ };
+
+ struct RxItem
+ {
+ uavcan::CanFrame frame;
+ uavcan::MonotonicTime ts_mono;
+ uavcan::UtcTime ts_utc;
+ uavcan::CanIOFlags flags;
+
+ RxItem()
+ : flags(0)
+ { }
+ };
+
+ const SystemClock& clock_;
+ const int fd_;
+
+ const unsigned max_frames_in_socket_tx_queue_;
+ unsigned frames_in_socket_tx_queue_ = 0;
+
+ std::uint64_t tx_frame_counter_ = 0; ///< Increments with every frame pushed into the TX queue
+
+ std::map<SocketCanError, std::uint64_t> errors_;
+
+ std::priority_queue<TxItem> tx_queue_; // TODO: Use pool allocator
+ std::queue<RxItem> rx_queue_; // TODO: Use pool allocator
+ std::unordered_multiset<std::uint32_t> pending_loopback_ids_; // TODO: Use pool allocator
+
+ std::vector<::can_filter> hw_filters_container_;
+
+ void registerError(SocketCanError e) { errors_[e]++; }
+
+ void incrementNumFramesInSocketTxQueue()
+ {
+ assert(frames_in_socket_tx_queue_ < max_frames_in_socket_tx_queue_);
+ frames_in_socket_tx_queue_++;
+ }
+
+ void confirmSentFrame()
+ {
+ if (frames_in_socket_tx_queue_ > 0)
+ {
+ frames_in_socket_tx_queue_--;
+ }
+ else
+ {
+ assert(0); // Loopback for a frame that we didn't send.
+ }
+ }
+
+ bool wasInPendingLoopbackSet(const uavcan::CanFrame& frame)
+ {
+ if (pending_loopback_ids_.count(frame.id) > 0)
+ {
+ (void)pending_loopback_ids_.erase(frame.id);
+ return true;
+ }
+ return false;
+ }
+
+ int write(const uavcan::CanFrame& frame) const
+ {
+ errno = 0;
+
+ const ::can_frame sockcan_frame = makeSocketCanFrame(frame);
+
+ const int res = ::write(fd_, &sockcan_frame, sizeof(sockcan_frame));
+ if (res <= 0)
+ {
+ if (errno == ENOBUFS || errno == EAGAIN) // Writing is not possible atm, not an error
+ {
+ return 0;
+ }
+ return res;
+ }
+ if (res != sizeof(sockcan_frame))
+ {
+ return -1;
+ }
+ return 1;
+ }
+
+ /**
+ * SocketCAN git show 1e55659ce6ddb5247cee0b1f720d77a799902b85
+ * MSG_DONTROUTE is set for any packet from localhost,
+ * MSG_CONFIRM is set for any pakcet of your socket.
+ * Diff: https://git.ucsd.edu/abuss/linux/commit/1e55659ce6ddb5247cee0b1f720d77a799902b85
+ * Man: https://www.kernel.org/doc/Documentation/networking/can.txt (chapter 4.1.6).
+ */
+ int read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const
+ {
+ auto iov = ::iovec();
+ auto sockcan_frame = ::can_frame();
+ iov.iov_base = &sockcan_frame;
+ iov.iov_len = sizeof(sockcan_frame);
+
+ static constexpr size_t ControlSize = sizeof(cmsghdr) + sizeof(::timeval);
+ using ControlStorage = typename std::aligned_storage<ControlSize>::type;
+ ControlStorage control_storage;
+ auto control = reinterpret_cast<std::uint8_t *>(&control_storage);
+ std::fill(control, control + ControlSize, 0x00);
+
+ auto msg = ::msghdr();
+ msg.msg_iov = &iov;
+ msg.msg_iovlen = 1;
+ msg.msg_control = control;
+ msg.msg_controllen = ControlSize;
+
+ const int res = ::recvmsg(fd_, &msg, MSG_DONTWAIT);
+ if (res <= 0)
+ {
+ return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
+ }
+ /*
+ * Flags
+ */
+ loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
+
+ if (!loopback && !checkHWFilters(sockcan_frame))
+ {
+ return 0;
+ }
+
+ frame = makeUavcanFrame(sockcan_frame);
+ /*
+ * Timestamp
+ */
+ const ::cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
+ assert(cmsg != nullptr);
+ if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP)
+ {
+ auto tv = ::timeval();
+ (void)std::memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); // Copy to avoid alignment problems
+ assert(tv.tv_sec >= 0 && tv.tv_usec >= 0);
+ ts_utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * 1000000ULL + tv.tv_usec);
+ }
+ else
+ {
+ assert(0);
+ return -1;
+ }
+ return 1;
+ }
+
+ void pollWrite()
+ {
+ while (hasReadyTx())
+ {
+ const TxItem tx = tx_queue_.top();
+
+ if (tx.deadline >= clock_.getMonotonic())
+ {
+ const int res = write(tx.frame);
+ if (res == 1) // Transmitted successfully
+ {
+ incrementNumFramesInSocketTxQueue();
+ if (tx.flags & uavcan::CanIOFlagLoopback)
+ {
+ (void)pending_loopback_ids_.insert(tx.frame.id);
+ }
+ }
+ else if (res == 0) // Not transmitted, nor is it an error
+ {
+ break; // Leaving the loop, the frame remains enqueued for the next retry
+ }
+ else // Transmission error
+ {
+ registerError(SocketCanError::SocketWriteFailure);
+ }
+ }
+ else
+ {
+ registerError(SocketCanError::TxTimeout);
+ }
+
+ // Removing the frame from the queue even if transmission failed
+ tx_queue_.pop();
+ }
+ }
+
+ void pollRead()
+ {
+ while (true)
+ {
+ RxItem rx;
+ rx.ts_mono = clock_.getMonotonic(); // Monotonic timestamp is not required to be precise (unlike UTC)
+ bool loopback = false;
+ const int res = read(rx.frame, rx.ts_utc, loopback);
+ if (res == 1)
+ {
+ assert(!rx.ts_utc.isZero());
+ bool accept = true;
+ if (loopback) // We receive loopback for all CAN frames
+ {
+ confirmSentFrame();
+ rx.flags |= uavcan::CanIOFlagLoopback;
+ accept = wasInPendingLoopbackSet(rx.frame); // Do we need to send this loopback into the lib?
+ }
+ if (accept)
+ {
+ rx.ts_utc += clock_.getPrivateAdjustment();
+ rx_queue_.push(rx);
+ }
+ }
+ else if (res == 0)
+ {
+ break;
+ }
+ else
+ {
+ registerError(SocketCanError::SocketReadFailure);
+ break;
+ }
+ }
+ }
+
+ /**
+ * Returns true if a frame accepted by HW filters
+ */
+ bool checkHWFilters(const ::can_frame& frame) const
+ {
+ if (!hw_filters_container_.empty())
+ {
+ for (auto& f : hw_filters_container_)
+ {
+ if (((frame.can_id & f.can_mask) ^ f.can_id) == 0)
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+ else
+ {
+ return true;
+ }
+ }
+
+public:
+ /**
+ * Takes ownership of socket's file descriptor.
+ *
+ * @ref max_frames_in_socket_tx_queue See a note in the class comment.
+ */
+ SocketCanIface(const SystemClock& clock, int socket_fd, int max_frames_in_socket_tx_queue = 2)
+ : clock_(clock)
+ , fd_(socket_fd)
+ , max_frames_in_socket_tx_queue_(max_frames_in_socket_tx_queue)
+ {
+ assert(fd_ >= 0);
+ }
+
+ /**
+ * Socket file descriptor will be closed.
+ */
+ virtual ~SocketCanIface()
+ {
+ UAVCAN_TRACE("SocketCAN", "SocketCanIface: Closing fd %d", fd_);
+ (void)::close(fd_);
+ }
+
+ /**
+ * Assumes that the socket is writeable
+ */
+ std::int16_t send(const uavcan::CanFrame& frame, const uavcan::MonotonicTime tx_deadline,
+ const uavcan::CanIOFlags flags) override
+ {
+ tx_queue_.emplace(frame, tx_deadline, flags, tx_frame_counter_);
+ tx_frame_counter_++;
+ pollRead(); // Read poll is necessary because it can release the pending TX flag
+ pollWrite();
+ return 1;
+ }
+
+ /**
+ * Will read the socket only if RX queue is empty.
+ * Normally, poll() needs to be executed first.
+ */
+ std::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override
+ {
+ if (rx_queue_.empty())
+ {
+ pollRead(); // This allows to use the socket not calling poll() explicitly.
+ if (rx_queue_.empty())
+ {
+ return 0;
+ }
+ }
+ {
+ const RxItem& rx = rx_queue_.front();
+ out_frame = rx.frame;
+ out_ts_monotonic = rx.ts_mono;
+ out_ts_utc = rx.ts_utc;
+ out_flags = rx.flags;
+ }
+ rx_queue_.pop();
+ return 1;
+ }
+
+ /**
+ * Performs socket read/write.
+ * @param read Socket is readable
+ * @param write Socket is writeable
+ */
+ void poll(bool read, bool write)
+ {
+ if (read)
+ {
+ pollRead(); // Read poll must be executed first because it may decrement frames_in_socket_tx_queue_
+ }
+ if (write)
+ {
+ pollWrite();
+ }
+ }
+
+ bool hasReadyRx() const { return !rx_queue_.empty(); }
+ bool hasReadyTx() const
+ {
+ return !tx_queue_.empty() && (frames_in_socket_tx_queue_ < max_frames_in_socket_tx_queue_);
+ }
+
+ std::int16_t configureFilters(const uavcan::CanFilterConfig* const filter_configs,
+ const std::uint16_t num_configs) override
+ {
+ if (filter_configs == nullptr)
+ {
+ assert(0);
+ return -1;
+ }
+ hw_filters_container_.clear();
+ hw_filters_container_.resize(num_configs);
+
+ for (unsigned i = 0; i < num_configs; i++)
+ {
+ const uavcan::CanFilterConfig& fc = filter_configs[i];
+ hw_filters_container_[i].can_id = fc.id & uavcan::CanFrame::MaskExtID;
+ hw_filters_container_[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
+ if (fc.id & uavcan::CanFrame::FlagEFF)
+ {
+ hw_filters_container_[i].can_id |= CAN_EFF_FLAG;
+ }
+ if (fc.id & uavcan::CanFrame::FlagRTR)
+ {
+ hw_filters_container_[i].can_id |= CAN_RTR_FLAG;
+ }
+ if (fc.mask & uavcan::CanFrame::FlagEFF)
+ {
+ hw_filters_container_[i].can_mask |= CAN_EFF_FLAG;
+ }
+ if (fc.mask & uavcan::CanFrame::FlagRTR)
+ {
+ hw_filters_container_[i].can_mask |= CAN_RTR_FLAG;
+ }
+ }
+
+ return 0;
+ }
+
+ /**
+ * SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
+ * This method returns a constant value.
+ */
+ static constexpr unsigned NumFilters = 8;
+ std::uint16_t getNumFilters() const override { return NumFilters; }
+
+
+ /**
+ * Returns total number of errors of each kind detected since the object was created.
+ */
+ std::uint64_t getErrorCount() const override
+ {
+ std::uint64_t ec = 0;
+ for (auto& kv : errors_) { ec += kv.second; }
+ return ec;
+ }
+
+ /**
+ * Returns number of errors of each kind in a map.
+ */
+ const decltype(errors_) & getErrors() const { return errors_; }
+
+ int getFileDescriptor() const { return fd_; }
+
+ /**
+ * Open and configure a CAN socket on iface specified by name.
+ * @param iface_name String containing iface name, e.g. "can0", "vcan1", "slcan0"
+ * @return Socket descriptor or negative number on error.
+ */
+ static int openSocket(const std::string& iface_name)
+ {
+ errno = 0;
+
+ const int s = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
+ if (s < 0)
+ {
+ return s;
+ }
+
+ class RaiiCloser
+ {
+ int fd_;
+ public:
+ RaiiCloser(int filedesc) : fd_(filedesc)
+ {
+ assert(fd_ >= 0);
+ }
+ ~RaiiCloser()
+ {
+ if (fd_ >= 0)
+ {
+ UAVCAN_TRACE("SocketCAN", "RaiiCloser: Closing fd %d", fd_);
+ (void)::close(fd_);
+ }
+ }
+ void disarm() { fd_ = -1; }
+ } raii_closer(s);
+
+ // Detect the iface index
+ auto ifr = ::ifreq();
+ if (iface_name.length() >= IFNAMSIZ)
+ {
+ errno = ENAMETOOLONG;
+ return -1;
+ }
+ (void)std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length());
+ if (::ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0)
+ {
+ return -1;
+ }
+
+ // Bind to the specified CAN iface
+ {
+ auto addr = ::sockaddr_can();
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifr.ifr_ifindex;
+ if (::bind(s, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
+ {
+ return -1;
+ }
+ }
+
+ // Configure
+ {
+ const int on = 1;
+ // Timestamping
+ if (::setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0)
+ {
+ return -1;
+ }
+ // Socket loopback
+ if (::setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0)
+ {
+ return -1;
+ }
+ // Non-blocking
+ if (::fcntl(s, F_SETFL, O_NONBLOCK) < 0)
+ {
+ return -1;
+ }
+ }
+
+ // Validate the resulting socket
+ {
+ int socket_error = 0;
+ ::socklen_t errlen = sizeof(socket_error);
+ (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
+ if (socket_error != 0)
+ {
+ errno = socket_error;
+ return -1;
+ }
+ }
+
+ raii_closer.disarm();
+ return s;
+ }
+};
+
+/**
+ * Multiplexing container for multiple SocketCAN sockets.
+ * Uses ppoll() for multiplexing.
+ *
+ * When an interface becomes down/disconnected while the node is running,
+ * the driver will silently exclude it from the IO loop and continue to run on the remaining interfaces.
+ * When all interfaces become down/disconnected, the driver will throw @ref AllIfacesDownException
+ * from @ref SocketCanDriver::select().
+ * Whether a certain interface is down can be checked with @ref SocketCanDriver::isIfaceDown().
+ */
+class SocketCanDriver : public uavcan::ICanDriver
+{
+ class IfaceWrapper : public SocketCanIface
+ {
+ bool down_ = false;
+
+ public:
+ IfaceWrapper(const SystemClock& clock, int fd) : SocketCanIface(clock, fd) { }
+
+ void updateDownStatusFromPollResult(const ::pollfd& pfd)
+ {
+ assert(pfd.fd == this->getFileDescriptor());
+ if (!down_ && (pfd.revents & POLLERR))
+ {
+ int error = 0;
+ ::socklen_t errlen = sizeof(error);
+ (void)::getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&error), &errlen);
+
+ down_ = error == ENETDOWN || error == ENODEV;
+
+ UAVCAN_TRACE("SocketCAN", "Iface %d is dead; error %d", this->getFileDescriptor(), error);
+ }
+ }
+
+ bool isDown() const { return down_; }
+ };
+
+ const SystemClock& clock_;
+ std::vector<std::unique_ptr<IfaceWrapper>> ifaces_;
+
+public:
+ /**
+ * Reference to the clock object shall remain valid.
+ */
+ explicit SocketCanDriver(const SystemClock& clock)
+ : clock_(clock)
+ {
+ ifaces_.reserve(uavcan::MaxCanIfaces);
+ }
+
+ /**
+ * This function may return before deadline expiration even if no requested IO operations become possible.
+ * This behavior makes implementation way simpler, and it is OK since libuavcan can properly handle such
+ * early returns.
+ * Also it can return more events than were originally requested by uavcan, which is also acceptable.
+ */
+ std::int16_t select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime blocking_deadline) override
+ {
+ // Detecting whether we need to block at all
+ bool need_block = (inout_masks.write == 0); // Write queue is infinite
+ for (unsigned i = 0; need_block && (i < ifaces_.size()); i++)
+ {
+ const bool need_read = inout_masks.read & (1 << i);
+ if (need_read && ifaces_[i]->hasReadyRx())
+ {
+ need_block = false;
+ }
+ }
+
+ if (need_block)
+ {
+ // Poll FD set setup
+ ::pollfd pollfds[uavcan::MaxCanIfaces] = {};
+ unsigned num_pollfds = 0;
+ IfaceWrapper* pollfd_index_to_iface[uavcan::MaxCanIfaces] = { };
+
+ for (unsigned i = 0; i < ifaces_.size(); i++)
+ {
+ if (!ifaces_[i]->isDown())
+ {
+ pollfds[num_pollfds].fd = ifaces_[i]->getFileDescriptor();
+ pollfds[num_pollfds].events = POLLIN;
+ if (ifaces_[i]->hasReadyTx() || (inout_masks.write & (1U << i)))
+ {
+ pollfds[num_pollfds].events |= POLLOUT;
+ }
+ pollfd_index_to_iface[num_pollfds] = ifaces_[i].get();
+ num_pollfds++;
+ }
+ }
+
+ // This is where we abort when the last iface goes down
+ if (num_pollfds == 0)
+ {
+ throw AllIfacesDownException();
+ }
+
+ // Timeout conversion
+ const std::int64_t timeout_usec = (blocking_deadline - clock_.getMonotonic()).toUSec();
+ auto ts = ::timespec();
+ if (timeout_usec > 0)
+ {
+ ts.tv_sec = timeout_usec / 1000000LL;
+ ts.tv_nsec = (timeout_usec % 1000000LL) * 1000;
+ }
+
+ // Blocking here
+ const int res = ::ppoll(pollfds, num_pollfds, &ts, nullptr);
+ if (res < 0)
+ {
+ return res;
+ }
+
+ // Handling poll output
+ for (unsigned i = 0; i < num_pollfds; i++)
+ {
+ pollfd_index_to_iface[i]->updateDownStatusFromPollResult(pollfds[i]);
+
+ const bool poll_read = pollfds[i].revents & POLLIN;
+ const bool poll_write = pollfds[i].revents & POLLOUT;
+ pollfd_index_to_iface[i]->poll(poll_read, poll_write);
+ }
+ }
+
+ // Writing the output masks
+ inout_masks = uavcan::CanSelectMasks();
+ for (unsigned i = 0; i < ifaces_.size(); i++)
+ {
+ if (!ifaces_[i]->isDown())
+ {
+ inout_masks.write |= std::uint8_t(1U << i); // Always ready to write if not down
+ }
+ if (ifaces_[i]->hasReadyRx())
+ {
+ inout_masks.read |= std::uint8_t(1U << i); // Readability depends only on RX buf, even if down
+ }
+ }
+
+ // Return value is irrelevant as long as it's non-negative
+ return ifaces_.size();
+ }
+
+ SocketCanIface* getIface(std::uint8_t iface_index) override
+ {
+ return (iface_index >= ifaces_.size()) ? nullptr : ifaces_[iface_index].get();
+ }
+
+ std::uint8_t getNumIfaces() const override { return ifaces_.size(); }
+
+ /**
+ * Adds one iface by name. Will fail if there are @ref MaxIfaces ifaces registered already.
+ * @param iface_name E.g. "can0", "vcan1"
+ * @return Negative on error, interface index on success.
+ * @throws uavcan_linux::Exception.
+ */
+ int addIface(const std::string& iface_name)
+ {
+ if (ifaces_.size() >= uavcan::MaxCanIfaces)
+ {
+ return -1;
+ }
+
+ // Open the socket
+ const int fd = SocketCanIface::openSocket(iface_name);
+ if (fd < 0)
+ {
+ return fd;
+ }
+
+ // Construct the iface - upon successful construction the iface will take ownership of the fd.
+ try
+ {
+ ifaces_.emplace_back(new IfaceWrapper(clock_, fd));
+ }
+ catch (...)
+ {
+ (void)::close(fd);
+ throw;
+ }
+
+ UAVCAN_TRACE("SocketCAN", "New iface '%s' fd %d", iface_name.c_str(), fd);
+
+ return ifaces_.size() - 1;
+ }
+
+ /**
+ * Returns false if the specified interface is functioning, true if it became unavailable.
+ */
+ bool isIfaceDown(std::uint8_t iface_index) const
+ {
+ return ifaces_.at(iface_index)->isDown();
+ }
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <array>
+#include <cassert>
+#include <cctype>
+#include <fstream>
+#include <string>
+#include <vector>
+#include <cstdint>
+#include <algorithm>
+#include <utility>
+#include <uavcan_linux/exception.hpp>
+#include <uavcan/data_type.hpp>
+
+namespace uavcan_linux
+{
+/**
+ * This class can find and read machine ID from a text file, represented as 32-char (16-byte) long hexadecimal string,
+ * possibly with separators (like dashes or colons). If the available ID is more than 16 bytes, extra bytes will be
+ * ignored. A shorter ID will not be accepted as valid.
+ * In order to be read, the ID must be located on the first line of the file and must not contain any whitespace
+ * characters.
+ *
+ * Examples of valid ID:
+ * 0123456789abcdef0123456789abcdef
+ * 20CE0b1E-8C03-07C8-13EC-00242C491652
+ */
+class MachineIDReader
+{
+public:
+ static constexpr int MachineIDSize = 16;
+
+ typedef std::array<std::uint8_t, MachineIDSize> MachineID;
+
+ static std::vector<std::string> getDefaultSearchLocations()
+ {
+ return
+ {
+ "/etc/machine-id",
+ "/var/lib/dbus/machine-id",
+ "/sys/class/dmi/id/product_uuid"
+ };
+ }
+
+private:
+ const std::vector<std::string> search_locations_;
+
+ static std::vector<std::string> mergeLists(const std::vector<std::string>& a, const std::vector<std::string>& b)
+ {
+ std::vector<std::string> ab;
+ ab.reserve(a.size() + b.size());
+ ab.insert(ab.end(), a.begin(), a.end());
+ ab.insert(ab.end(), b.begin(), b.end());
+ return ab;
+ }
+
+ bool tryRead(const std::string& location, MachineID& out_id) const
+ {
+ /*
+ * Reading the file
+ */
+ std::string token;
+ try
+ {
+ std::ifstream infile(location);
+ infile >> token;
+ }
+ catch (std::exception&)
+ {
+ return false;
+ }
+
+ /*
+ * Preprocessing the input string - convert to lowercase, remove all non-hex characters, limit to 32 chars
+ */
+ std::transform(token.begin(), token.end(), token.begin(), [](char x) { return std::tolower(x); });
+ token.erase(std::remove_if(token.begin(), token.end(),
+ [](char x){ return (x < 'a' || x > 'f') && !std::isdigit(x); }),
+ token.end());
+
+ if (token.length() < (MachineIDSize * 2))
+ {
+ return false;
+ }
+ token.resize(MachineIDSize * 2); // Truncating
+
+ /*
+ * Parsing the string as hex bytes
+ */
+ auto sym = std::begin(token);
+ for (auto& byte : out_id)
+ {
+ assert(sym != std::end(token));
+ byte = std::stoi(std::string{*sym++, *sym++}, nullptr, 16);
+ }
+
+ return true;
+ }
+
+public:
+ /**
+ * This class can use extra seach locations. If provided, they will be checked first, before default ones.
+ */
+ MachineIDReader(const std::vector<std::string>& extra_search_locations = {})
+ : search_locations_(mergeLists(extra_search_locations, getDefaultSearchLocations()))
+ { }
+
+ /**
+ * Just like @ref readAndGetLocation(), but this one doesn't return location where this ID was obtained from.
+ */
+ MachineID read() const { return readAndGetLocation().first; }
+
+ /**
+ * This function checks available search locations and reads the ID from the first valid location.
+ * It returns std::pair<> with ID and the file path where it was read from.
+ * In case if none of the search locations turned out to be valid, @ref uavcan_linux::Exception will be thrown.
+ */
+ std::pair<MachineID, std::string> readAndGetLocation() const
+ {
+ for (auto x : search_locations_)
+ {
+ auto out = MachineID();
+ if (tryRead(x, out))
+ {
+ return {out, x};
+ }
+ }
+ throw Exception("Failed to read machine ID");
+ }
+};
+
+/**
+ * This class computes unique ID for a UAVCAN node in a Linux application.
+ * It takes the following inputs:
+ * - Unique machine ID
+ * - Node name string (e.g. "org.uavcan.linux_app.dynamic_node_id_server")
+ * - Instance ID byte, e.g. node ID (optional)
+ */
+inline std::array<std::uint8_t, 16> makeApplicationID(const MachineIDReader::MachineID& machine_id,
+ const std::string& node_name,
+ const std::uint8_t instance_id = 0)
+{
+ union HalfID
+ {
+ std::uint64_t num;
+ std::uint8_t bytes[8];
+
+ HalfID(std::uint64_t arg_num) : num(arg_num) { }
+ };
+
+ std::array<std::uint8_t, 16> out;
+
+ // First 8 bytes of the application ID are CRC64 of the machine ID in native byte order
+ {
+ uavcan::DataTypeSignatureCRC crc;
+ crc.add(machine_id.data(), static_cast<unsigned>(machine_id.size()));
+ HalfID half(crc.get());
+ std::copy_n(half.bytes, 8, out.begin());
+ }
+
+ // Last 8 bytes of the application ID are CRC64 of the node name and optionally node ID
+ {
+ uavcan::DataTypeSignatureCRC crc;
+ crc.add(reinterpret_cast<const std::uint8_t*>(node_name.c_str()), static_cast<unsigned>(node_name.length()));
+ crc.add(instance_id);
+ HalfID half(crc.get());
+ std::copy_n(half.bytes, 8, out.begin() + 8);
+ }
+
+ return out;
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,12 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include <uavcan/uavcan.hpp> + +#include <uavcan_linux/clock.hpp> +#include <uavcan_linux/socketcan.hpp> +#include <uavcan_linux/helpers.hpp> +#include <uavcan_linux/system_utils.hpp>
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/scripts/uavcan_add_slcan Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,173 @@
+#!/bin/bash
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@zubax.com>
+#
+
+HELP="Register slcan-enabled Serial-to-CAN adapters as network interfaces.
+Usage:
+ `basename $0` [options] <tty0> [[options] <tty1> ...]
+
+Interface indexes will be assigned automatically in ascending order, i.e.
+first device will be mapped to the interface slcan0, second will be mapped to
+slcan1, and so on. Each added option affects only the interfaces that follow
+it, which means that options must be properly ordered (see examples below).
+This tool requires superuser priveleges.
+
+The package 'can-utils' must be installed. On Debian/Ubuntu-based systems it
+can be installed via APT: apt-get install can-utils
+
+Options:
+ --speed-code <X> (where X is a number in range [0, 8]; default: 8)
+ -s<X>
+ Set CAN speed to:
+ 0 - 10 Kbps
+ 1 - 20 Kbps
+ 2 - 50 Kbps
+ 3 - 100 Kbps
+ 4 - 125 Kbps (UAVCAN recommended)
+ 5 - 250 Kbps (UAVCAN recommended)
+ 6 - 500 Kbps (UAVCAN recommended)
+ 7 - 800 Kbps
+ 8 - 1 Mbps (UAVCAN recommended, default)
+
+ --remove-all
+ -r
+ Remove all SLCAN interfaces.
+ If this option is used, it MUST be provided FIRST, otherwise it
+ will remove the interfaces added earlier.
+
+ --basename <X> (where X is a string containing [a-z], default: slcan)
+ -b<X>
+ Base name to use for the interfaces that follow this option.
+ Default value is 'slcan'. This option can be provided multiple times,
+ it will only affect the interfaces that were provided after it. If you
+ want to affect all added interfaces, provide this option first (see
+ examples below).
+
+ --baudrate <X> (where X is an integer, default: 921600)
+ -S<X>
+ Configure baud rate to use on the interface.
+ This option is mostly irrelevant for USB to CAN adapters.
+
+Example 1:
+ `basename $0` --remove-all /dev/ttyUSB3 --basename can --baudrate 115200 \\
+ /dev/ttyUSB0 --speed-code 4 /dev/ttyACM0
+The example above initializes the interfaces as follows:
+ /dev/ttyUSB3 --> slcan0 1 Mbps baudrate 921600
+ /dev/ttyUSB0 --> can0 1 Mbps baudrate 115200
+ /dev/ttyACM0 --> can1 125 kbps baudrate 115200
+
+Example 2:
+ `basename $0` --remove-all
+The example above only removes all SLCAN interfaces without adding new ones."
+
+function die() { echo $@ >&2; exit 1; }
+
+if [ "$1" == '--help' ] || [ "$1" == '-h' ]; then echo "$HELP"; exit; fi
+
+[ -n "$1" ] || die "Invalid usage. Use --help to get help."
+
+[ "$(id -u)" == "0" ] || die "Must be root."
+
+which slcan_attach > /dev/null || die "Please install can-utils first."
+
+# ---------------------------------------------------------
+
+function deinitialize() {
+ echo "Stopping slcand..." >&2
+ # Trying SIGINT first
+ killall -INT slcand &> /dev/null
+ sleep 0.3
+ # Then trying the default signal, which is SIGTERM, if SIGINT didn't help
+ slcand_kill_retries=10
+ while killall slcand &> /dev/null
+ do
+ (( slcand_kill_retries -= 1 ))
+ [[ "$slcand_kill_retries" > 0 ]] || die "Failed to stop slcand"
+ sleep 1
+ done
+}
+
+function handle_tty() {
+ tty=$(readlink -f $1)
+ tty=${tty/'/dev/'}
+
+ iface_index=0
+ while ifconfig "$IFACE_BASENAME$iface_index" &> /dev/null
+ do
+ iface_index=$((iface_index + 1))
+ done
+
+ slcan_iface_index=0
+ while ifconfig "slcan$slcan_iface_index" &> /dev/null
+ do
+ slcan_iface_index=$((slcan_iface_index + 1))
+ done
+
+ iface="$IFACE_BASENAME$iface_index"
+ slcan_iface="slcan$slcan_iface_index"
+
+ echo "Attaching $tty to $iface speed code $SPEED_CODE baudrate $BAUDRATE" >&2
+
+ # Configuring the baudrate
+ stty -F /dev/$tty ispeed $BAUDRATE ospeed $BAUDRATE || return 1
+
+ # Attaching the line discipline. Note that slcan_attach has option -n but it doesn't work.
+ slcan_attach -f -o -s$SPEED_CODE /dev/$tty > /dev/null || return 2
+ slcand $tty || return 3
+ sleep 1 # FIXME
+
+ # ...therefore we need to rename the interface manually
+ ip link set $slcan_iface name $iface
+
+ ifconfig $iface up || return 4
+}
+
+IFACE_BASENAME='slcan'
+SPEED_CODE=8
+BAUDRATE=921600
+
+next_option=''
+while [ -n "$1" ]; do
+ case $1 in
+ -r | --remove-all)
+ deinitialize
+ ;;
+
+ -b*)
+ IFACE_BASENAME=${1:2}
+ ;;
+
+ -S*)
+ BAUDRATE=${1:2}
+ ;;
+
+ -s[0-8])
+ SPEED_CODE=${1:2}
+ ;;
+
+ --*)
+ next_option=${1:2}
+ ;;
+
+ -*)
+ die "Invalid option: $1"
+ ;;
+
+ *)
+ if [ "$next_option" = 'basename' ]; then IFACE_BASENAME=$1
+ elif [ "$next_option" = 'speed-code' ]; then SPEED_CODE=$1
+ elif [ "$next_option" = 'baudrate' ]; then BAUDRATE=$1
+ elif [ "$next_option" = '' ]
+ then
+ handle_tty $1 || die "Failed to configure the interface $1"
+ else
+ die "Invalid option '$next_option'"
+ fi
+ next_option=''
+ ;;
+ esac
+ shift
+done
+
+[ "$next_option" = '' ] || die "Expected argument for option '$next_option'"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/linux/scripts/uavcan_add_vcan Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,37 @@
+#!/bin/bash
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+HELP="Initializes and brings up a virtual CAN interface.
+Usage:
+ `basename $0` <iface-name>
+Example:
+ `basename $0` vcan0"
+
+function die() { echo $@ >&2; exit 1; }
+
+if [ "$1" == '--help' ] || [ "$1" == '-h' ]; then echo "$HELP"; exit; fi
+[ -n "$1" ] || die "Invalid usage. Use --help to get help."
+[ "$(id -u)" == "0" ] || die "Must be root"
+
+# ---------------------------------------------------------
+
+IFACE="$1"
+
+if [ $(ifconfig -a | grep -c "^$IFACE") -eq "1" ]; then
+ ifconfig $IFACE up
+ exit
+fi
+
+modprobe can
+modprobe can_raw
+modprobe can_bcm
+modprobe vcan
+
+ip link add dev $IFACE type vcan
+ip link set up $IFACE
+
+ifconfig $IFACE up || exit 1
+
+echo "New iface $IFACE added successfully. To delete: ip link delete $IFACE"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/lpc11c24/driver/include.mk Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,9 @@ +# +# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> +# + +LIBUAVCAN_LPC11C24_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_LPC11C24_SRC := $(shell find $(LIBUAVCAN_LPC11C24_DIR)/src -type f -name '*.cpp') + +LIBUAVCAN_LPC11C24_INC := $(LIBUAVCAN_LPC11C24_DIR)/include/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/driver/can.hpp>
+
+namespace uavcan_lpc11c24
+{
+/**
+ * This class implements CAN driver interface for libuavcan.
+ * No configuration needed other than CAN baudrate.
+ * This class is a singleton.
+ */
+class CanDriver
+ : public uavcan::ICanDriver
+ , public uavcan::ICanIface
+ , uavcan::Noncopyable
+{
+ static CanDriver self;
+
+ CanDriver() { }
+
+public:
+ /**
+ * Returns the singleton reference.
+ * No other copies can be created.
+ */
+ static CanDriver& instance() { return self; }
+
+ /**
+ * Attempts to detect bit rate of the CAN bus.
+ * This function may block for up to X seconds, where X is the number of bit rates to try.
+ * This function is NOT guaranteed to reset the CAN controller upon return.
+ * @return On success: detected bit rate, in bits per second.
+ * On failure: zero.
+ */
+ static uavcan::uint32_t detectBitRate(void (*idle_callback)() = nullptr);
+
+ /**
+ * Returns negative value if the requested baudrate can't be used.
+ * Returns zero if OK.
+ */
+ int init(uavcan::uint32_t bitrate);
+
+ bool hasReadyRx() const;
+ bool hasEmptyTx() const;
+
+ /**
+ * This method will return true only if there was any CAN bus activity since previous call of this method.
+ * This is intended to be used for LED iface activity indicators.
+ */
+ bool hadActivity();
+
+ /**
+ * Returns the number of times the RX queue was overrun.
+ */
+ uavcan::uint32_t getRxQueueOverflowCount() const;
+
+ /**
+ * Whether the controller is currently in bus off state.
+ * Note that the driver recovers the CAN controller from the bus off state automatically!
+ * Therefore, this method serves only monitoring purposes and is not necessary to use.
+ */
+ bool isInBusOffState() const;
+
+ uavcan::int16_t send(const uavcan::CanFrame& frame,
+ uavcan::MonotonicTime tx_deadline,
+ uavcan::CanIOFlags flags) override;
+
+ uavcan::int16_t receive(uavcan::CanFrame& out_frame,
+ uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc,
+ uavcan::CanIOFlags& out_flags) override;
+
+ uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime blocking_deadline) override;
+
+ uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
+ uavcan::uint16_t num_configs) override;
+
+ uavcan::uint64_t getErrorCount() const override;
+
+ uavcan::uint16_t getNumFilters() const override;
+
+ uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) override;
+
+ uavcan::uint8_t getNumIfaces() const override;
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/driver/system_clock.hpp>
+
+namespace uavcan_lpc11c24
+{
+namespace clock
+{
+/**
+ * Starts the clock.
+ * Can be called multiple times, only the first call will be effective.
+ */
+void init();
+
+/**
+ * Returns current monotonic time passed since the moment when clock::init() was called.
+ * Note that both monotonic and UTC clocks are implemented using SysTick timer.
+ */
+uavcan::MonotonicTime getMonotonic();
+
+/**
+ * Returns UTC time if it has been set, otherwise returns zero time.
+ * Note that both monotonic and UTC clocks are implemented using SysTick timer.
+ */
+uavcan::UtcTime getUtc();
+
+/**
+ * Performs UTC time adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment);
+
+/**
+ * Returns clock error sampled at previous UTC adjustment.
+ * Positive if the hardware timer is slower than reference time.
+ */
+uavcan::UtcDuration getPrevUtcAdjustment();
+
+}
+
+/**
+ * Adapter for uavcan::ISystemClock.
+ */
+class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
+{
+ static SystemClock self;
+
+ SystemClock() { }
+
+ uavcan::MonotonicTime getMonotonic() const override { return clock::getMonotonic(); }
+ uavcan::UtcTime getUtc() const override { return clock::getUtc(); }
+ void adjustUtc(uavcan::UtcDuration adjustment) override { clock::adjustUtc(adjustment); }
+
+public:
+ /**
+ * Calls clock::init() as needed.
+ */
+ static SystemClock& instance();
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/uavcan_lpc11c24.hpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,9 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include <uavcan/uavcan.hpp> +#include <uavcan_lpc11c24/can.hpp> +#include <uavcan_lpc11c24/clock.hpp>
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,191 @@
+/*
+ * Bosch C_CAN controller API.
+ *
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+namespace uavcan_lpc11c24
+{
+namespace c_can
+{
+
+struct MsgIfaceType
+{
+ std::uint32_t CMDREQ;
+
+ union
+ {
+ std::uint32_t W;
+ std::uint32_t R;
+ } CMDMSK;
+
+ std::uint32_t MSK1;
+ std::uint32_t MSK2;
+
+ std::uint32_t ARB1;
+ std::uint32_t ARB2;
+
+ std::uint32_t MCTRL;
+
+ std::uint32_t DA1;
+ std::uint32_t DA2;
+ std::uint32_t DB1;
+ std::uint32_t DB2;
+
+ const std::uint32_t _skip[13];
+};
+
+static_assert(offsetof(MsgIfaceType, CMDMSK) == 0x04, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, MSK1) == 0x08, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, ARB1) == 0x10, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, MCTRL) == 0x18, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, DA1) == 0x1c, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, DB2) == 0x28, "C_CAN offset");
+
+static_assert(sizeof(MsgIfaceType) == 96, "C_CAN size");
+
+
+struct Type
+{
+ std::uint32_t CNTL;
+ std::uint32_t STAT;
+ const std::uint32_t EC;
+ std::uint32_t BT;
+ const std::uint32_t INT;
+ std::uint32_t TEST;
+ std::uint32_t BRPE;
+
+ const std::uint32_t _skip_a[1];
+
+ MsgIfaceType IF[2]; // [0] @ 0x020, [1] @ 0x080
+
+ const std::uint32_t _skip_b[8];
+
+ const std::uint32_t TXREQ[2]; // 0x100
+
+ const std::uint32_t _skip_c[6];
+
+ const std::uint32_t ND[2]; // 0x120
+
+ const std::uint32_t _skip_d[6];
+
+ const std::uint32_t IR[2]; // 0x140
+
+ const std::uint32_t _skip_e[6];
+
+ const std::uint32_t MSGV[2]; // 0x160
+
+ const std::uint32_t _skip_f[6];
+
+ std::uint32_t CLKDIV; // 0x180
+};
+
+static_assert(offsetof(Type, CNTL) == 0x000, "C_CAN offset");
+static_assert(offsetof(Type, STAT) == 0x004, "C_CAN offset");
+static_assert(offsetof(Type, TEST) == 0x014, "C_CAN offset");
+static_assert(offsetof(Type, BRPE) == 0x018, "C_CAN offset");
+static_assert(offsetof(Type, IF[0]) == 0x020, "C_CAN offset");
+static_assert(offsetof(Type, IF[1]) == 0x080, "C_CAN offset");
+static_assert(offsetof(Type, TXREQ) == 0x100, "C_CAN offset");
+static_assert(offsetof(Type, ND) == 0x120, "C_CAN offset");
+static_assert(offsetof(Type, IR) == 0x140, "C_CAN offset");
+static_assert(offsetof(Type, MSGV) == 0x160, "C_CAN offset");
+static_assert(offsetof(Type, CLKDIV) == 0x180, "C_CAN offset");
+
+static_assert(offsetof(Type, IF[0].DB2) == 0x048, "C_CAN offset");
+static_assert(offsetof(Type, IF[1].DB2) == 0x0A8, "C_CAN offset");
+
+
+volatile Type& CAN = *reinterpret_cast<volatile Type*>(0x40050000);
+
+
+/*
+ * CNTL
+ */
+static constexpr std::uint32_t CNTL_TEST = 1 << 7;
+static constexpr std::uint32_t CNTL_CCE = 1 << 6;
+static constexpr std::uint32_t CNTL_DAR = 1 << 5;
+static constexpr std::uint32_t CNTL_EIE = 1 << 3;
+static constexpr std::uint32_t CNTL_SIE = 1 << 2;
+static constexpr std::uint32_t CNTL_IE = 1 << 1;
+static constexpr std::uint32_t CNTL_INIT = 1 << 0;
+
+static constexpr std::uint32_t CNTL_IRQ_MASK = CNTL_EIE | CNTL_IE | CNTL_SIE;
+
+/*
+ * TEST
+ */
+static constexpr std::uint32_t TEST_RX = 1 << 7;
+static constexpr std::uint32_t TEST_LBACK = 1 << 4;
+static constexpr std::uint32_t TEST_SILENT = 1 << 3;
+static constexpr std::uint32_t TEST_BASIC = 1 << 2;
+static constexpr std::uint32_t TEST_TX_SHIFT = 5;
+
+enum class TestTx : std::uint32_t
+{
+ Controller = 0,
+ SamplePoint = 1,
+ LowDominant = 2,
+ HighRecessive = 3
+};
+
+/*
+ * STAT
+ */
+static constexpr std::uint32_t STAT_BOFF = 1 << 7;
+static constexpr std::uint32_t STAT_EWARN = 1 << 6;
+static constexpr std::uint32_t STAT_EPASS = 1 << 5;
+static constexpr std::uint32_t STAT_RXOK = 1 << 4;
+static constexpr std::uint32_t STAT_TXOK = 1 << 3;
+static constexpr std::uint32_t STAT_LEC_MASK = 7;
+static constexpr std::uint32_t STAT_LEC_SHIFT = 0;
+
+enum class StatLec : std::uint32_t
+{
+ NoError = 0,
+ StuffError = 1,
+ FormError = 2,
+ AckError = 3,
+ Bit1Error = 4,
+ Bit0Error = 5,
+ CRCError = 6,
+ Unused = 7
+};
+
+/*
+ * IF.CMDREQ
+ */
+static constexpr std::uint32_t IF_CMDREQ_BUSY = 1 << 15;
+
+/*
+ * IF.CMDMSK
+ */
+static constexpr std::uint32_t IF_CMDMSK_W_DATA_A = 1 << 0;
+static constexpr std::uint32_t IF_CMDMSK_W_DATA_B = 1 << 1;
+static constexpr std::uint32_t IF_CMDMSK_W_TXRQST = 1 << 2;
+static constexpr std::uint32_t IF_CMDMSK_W_CTRL = 1 << 4;
+static constexpr std::uint32_t IF_CMDMSK_W_ARB = 1 << 5;
+static constexpr std::uint32_t IF_CMDMSK_W_MASK = 1 << 6;
+static constexpr std::uint32_t IF_CMDMSK_W_WR_RD = 1 << 7;
+
+/*
+ * IF.MCTRL
+ */
+static constexpr std::uint32_t IF_MCTRL_NEWDAT = 1 << 15;
+static constexpr std::uint32_t IF_MCTRL_MSGLST = 1 << 14;
+static constexpr std::uint32_t IF_MCTRL_INTPND = 1 << 13;
+static constexpr std::uint32_t IF_MCTRL_UMASK = 1 << 12;
+static constexpr std::uint32_t IF_MCTRL_TXIE = 1 << 11;
+static constexpr std::uint32_t IF_MCTRL_RXIE = 1 << 10;
+static constexpr std::uint32_t IF_MCTRL_RMTEN = 1 << 9;
+static constexpr std::uint32_t IF_MCTRL_TXRQST = 1 << 8;
+static constexpr std::uint32_t IF_MCTRL_EOB = 1 << 7;
+static constexpr std::uint32_t IF_MCTRL_DLC_MASK = 15;
+
+}
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/driver/src/can.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,649 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_lpc11c24/can.hpp>
+#include <uavcan_lpc11c24/clock.hpp>
+#include <uavcan/util/templates.hpp>
+#include <chip.h>
+#include "c_can.hpp"
+#include "internal.hpp"
+
+/**
+ * The default value should be OK for any use case.
+ */
+#ifndef UAVCAN_LPC11C24_RX_QUEUE_LEN
+# define UAVCAN_LPC11C24_RX_QUEUE_LEN 8
+#endif
+
+#if UAVCAN_LPC11C24_RX_QUEUE_LEN > 254
+# error UAVCAN_LPC11C24_RX_QUEUE_LEN is too large
+#endif
+
+extern "C" void canRxCallback(std::uint8_t msg_obj_num);
+extern "C" void canTxCallback(std::uint8_t msg_obj_num);
+extern "C" void canErrorCallback(std::uint32_t error_info);
+
+namespace uavcan_lpc11c24
+{
+namespace
+{
+/**
+ * Hardware message objects are allocated as follows:
+ * - 1 - Single TX object
+ * - 2..32 - RX objects
+ * TX priority is defined by the message object number, not by the CAN ID (chapter 16.7.3.5 of the user manual),
+ * hence we can't use more than one object because that would cause priority inversion on long transfers.
+ */
+constexpr unsigned NumberOfMessageObjects = 32;
+constexpr unsigned NumberOfTxMessageObjects = 1;
+constexpr unsigned NumberOfRxMessageObjects = NumberOfMessageObjects - NumberOfTxMessageObjects;
+constexpr unsigned TxMessageObjectNumber = 1;
+
+/**
+ * Total number of CAN errors.
+ * Does not overflow.
+ */
+volatile std::uint32_t error_cnt = 0;
+
+/**
+ * False if there's no pending TX frame, i.e. write is possible.
+ */
+volatile bool tx_pending = false;
+
+/**
+ * Currently pending frame must be aborted on first error.
+ */
+volatile bool tx_abort_on_error = false;
+
+/**
+ * Gets updated every time the CAN IRQ handler is being called.
+ */
+volatile std::uint64_t last_irq_utc_timestamp = 0;
+
+/**
+ * Set by the driver on every successful TX or RX; reset by the application.
+ */
+volatile bool had_activity = false;
+
+/**
+ * After a received message gets extracted from C_CAN, it will be stored in the RX queue until libuavcan
+ * reads it via select()/receive() calls.
+ */
+class RxQueue
+{
+ struct Item
+ {
+ std::uint64_t utc_usec = 0;
+ uavcan::CanFrame frame;
+ };
+
+ Item buf_[UAVCAN_LPC11C24_RX_QUEUE_LEN];
+ std::uint32_t overflow_cnt_ = 0;
+ std::uint8_t in_ = 0;
+ std::uint8_t out_ = 0;
+ std::uint8_t len_ = 0;
+
+public:
+ void push(const uavcan::CanFrame& frame, const volatile std::uint64_t& utc_usec)
+ {
+ buf_[in_].frame = frame;
+ buf_[in_].utc_usec = utc_usec;
+ in_++;
+ if (in_ >= UAVCAN_LPC11C24_RX_QUEUE_LEN)
+ {
+ in_ = 0;
+ }
+ len_++;
+ if (len_ > UAVCAN_LPC11C24_RX_QUEUE_LEN)
+ {
+ len_ = UAVCAN_LPC11C24_RX_QUEUE_LEN;
+ if (overflow_cnt_ < 0xFFFFFFFF)
+ {
+ overflow_cnt_++;
+ }
+ out_++;
+ if (out_ >= UAVCAN_LPC11C24_RX_QUEUE_LEN)
+ {
+ out_ = 0;
+ }
+ }
+ }
+
+ void pop(uavcan::CanFrame& out_frame, std::uint64_t& out_utc_usec)
+ {
+ if (len_ > 0)
+ {
+ out_frame = buf_[out_].frame;
+ out_utc_usec = buf_[out_].utc_usec;
+ out_++;
+ if (out_ >= UAVCAN_LPC11C24_RX_QUEUE_LEN)
+ {
+ out_ = 0;
+ }
+ len_--;
+ }
+ }
+
+ unsigned getLength() const { return len_; }
+
+ std::uint32_t getOverflowCount() const { return overflow_cnt_; }
+};
+
+RxQueue rx_queue;
+
+
+struct BitTimingSettings
+{
+ std::uint32_t canclkdiv;
+ std::uint32_t canbtr;
+
+ bool isValid() const { return canbtr != 0; }
+};
+
+/**
+ * http://www.bittiming.can-wiki.info
+ */
+BitTimingSettings computeBitTimings(std::uint32_t bitrate)
+{
+ if (Chip_Clock_GetSystemClockRate() == 48000000) // 48 MHz is optimal for CAN timings
+ {
+ switch (bitrate)
+ {
+ case 1000000: return BitTimingSettings{ 0, 0x0505 }; // 8 quanta, 87.5%
+ case 500000: return BitTimingSettings{ 0, 0x1c05 }; // 16 quanta, 87.5%
+ case 250000: return BitTimingSettings{ 0, 0x1c0b }; // 16 quanta, 87.5%
+ case 125000: return BitTimingSettings{ 0, 0x1c17 }; // 16 quanta, 87.5%
+ case 100000: return BitTimingSettings{ 0, 0x1c1d }; // 16 quanta, 87.5%
+ default: return BitTimingSettings{ 0, 0 };
+ }
+ }
+ else
+ {
+ return BitTimingSettings{ 0, 0 };
+ }
+}
+
+} // namespace
+
+CanDriver CanDriver::self;
+
+uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)())
+{
+ static constexpr uavcan::uint32_t BitRatesToTry[] =
+ {
+ 1000000,
+ 500000,
+ 250000,
+ 125000,
+ 100000
+ };
+
+ const auto ListeningDuration = uavcan::MonotonicDuration::fromMSec(1050);
+
+ NVIC_DisableIRQ(CAN_IRQn);
+ Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
+
+ for (auto bitrate : BitRatesToTry)
+ {
+ // Computing bit timings
+ const auto bit_timings = computeBitTimings(bitrate);
+ if (!bit_timings.isValid())
+ {
+ return 0;
+ }
+
+ // Configuring the CAN controller
+ {
+ CriticalSectionLocker locker;
+
+ LPC_SYSCTL->PRESETCTRL |= (1U << RESET_CAN0);
+
+ // Entering initialization mode
+ c_can::CAN.CNTL = c_can::CNTL_INIT | c_can::CNTL_CCE;
+
+ while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0)
+ {
+ ; // Nothing to do
+ }
+
+ // Configuring bit rate
+ c_can::CAN.CLKDIV = bit_timings.canclkdiv;
+ c_can::CAN.BT = bit_timings.canbtr;
+ c_can::CAN.BRPE = 0;
+
+ // Configuring silent mode
+ c_can::CAN.CNTL |= c_can::CNTL_TEST;
+ c_can::CAN.TEST = c_can::TEST_SILENT;
+
+ // Configuring status monitor
+ c_can::CAN.STAT = (unsigned(c_can::StatLec::Unused) << c_can::STAT_LEC_SHIFT);
+
+ // Leaving initialization mode
+ c_can::CAN.CNTL &= ~(c_can::CNTL_INIT | c_can::CNTL_CCE);
+
+ while ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0)
+ {
+ ; // Nothing to do
+ }
+ }
+
+ // Listening
+ const auto deadline = clock::getMonotonic() + ListeningDuration;
+ bool match_detected = false;
+ while (clock::getMonotonic() < deadline)
+ {
+ if (idle_callback != nullptr)
+ {
+ idle_callback();
+ }
+
+ const auto LastErrorCode = (c_can::CAN.STAT >> c_can::STAT_LEC_SHIFT) & c_can::STAT_LEC_MASK;
+
+ if (LastErrorCode == unsigned(c_can::StatLec::NoError))
+ {
+ match_detected = true;
+ break;
+ }
+ }
+
+ // De-configuring the CAN controller back to reset state
+ {
+ CriticalSectionLocker locker;
+
+ c_can::CAN.CNTL = c_can::CNTL_INIT;
+
+ while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0)
+ {
+ ; // Nothing to do
+ }
+
+ LPC_SYSCTL->PRESETCTRL &= ~(1U << RESET_CAN0);
+ }
+
+ // Termination condition
+ if (match_detected)
+ {
+ return bitrate;
+ }
+ }
+
+ return 0; // No match
+}
+
+int CanDriver::init(uavcan::uint32_t bitrate)
+{
+ {
+ auto bit_timings = computeBitTimings(bitrate);
+ if (!bit_timings.isValid())
+ {
+ return -1;
+ }
+
+ CriticalSectionLocker locker;
+
+ error_cnt = 0;
+ tx_abort_on_error = false;
+ tx_pending = false;
+ last_irq_utc_timestamp = 0;
+ had_activity = false;
+
+ /*
+ * C_CAN init
+ */
+ Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
+
+ LPC_CCAN_API->init_can(reinterpret_cast<std::uint32_t*>(&bit_timings), true);
+
+ static CCAN_CALLBACKS_T ccan_callbacks =
+ {
+ canRxCallback,
+ canTxCallback,
+ canErrorCallback,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr
+ };
+ LPC_CCAN_API->config_calb(&ccan_callbacks);
+
+ /*
+ * Interrupts
+ */
+ c_can::CAN.CNTL |= c_can::CNTL_SIE; // This is necessary for transmission aborts on error
+
+ NVIC_EnableIRQ(CAN_IRQn);
+ }
+
+ /*
+ * Applying default filter configuration (accept all)
+ */
+ if (configureFilters(nullptr, 0) < 0)
+ {
+ return -1;
+ }
+
+ return 0;
+}
+
+bool CanDriver::hasReadyRx() const
+{
+ CriticalSectionLocker locker;
+ return rx_queue.getLength() > 0;
+}
+
+bool CanDriver::hasEmptyTx() const
+{
+ CriticalSectionLocker locker;
+ return !tx_pending;
+}
+
+bool CanDriver::hadActivity()
+{
+ CriticalSectionLocker locker;
+ const bool ret = had_activity;
+ had_activity = false;
+ return ret;
+}
+
+uavcan::uint32_t CanDriver::getRxQueueOverflowCount() const
+{
+ CriticalSectionLocker locker;
+ return rx_queue.getOverflowCount();
+}
+
+bool CanDriver::isInBusOffState() const
+{
+ return (c_can::CAN.STAT & c_can::STAT_BOFF) != 0;
+}
+
+uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+ uavcan::CanIOFlags flags)
+{
+ if (frame.isErrorFrame() ||
+ frame.dlc > 8 ||
+ (flags & uavcan::CanIOFlagLoopback) != 0) // TX timestamping is not supported by this driver.
+ {
+ return -1;
+ }
+
+ /*
+ * Frame conversion
+ */
+ CCAN_MSG_OBJ_T msgobj = CCAN_MSG_OBJ_T();
+ msgobj.mode_id = frame.id & uavcan::CanFrame::MaskExtID;
+ if (frame.isExtended())
+ {
+ msgobj.mode_id |= CAN_MSGOBJ_EXT;
+ }
+ if (frame.isRemoteTransmissionRequest())
+ {
+ msgobj.mode_id |= CAN_MSGOBJ_RTR;
+ }
+ msgobj.dlc = frame.dlc;
+ uavcan::copy(frame.data, frame.data + frame.dlc, msgobj.data);
+
+ /*
+ * Transmission
+ */
+ (void)tx_deadline; // TX timeouts are not supported by this driver yet (and hardly going to be).
+
+ CriticalSectionLocker locker;
+
+ if (!tx_pending)
+ {
+ tx_pending = true; // Mark as pending - will be released in TX callback
+ tx_abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
+ msgobj.msgobj = TxMessageObjectNumber;
+ LPC_CCAN_API->can_transmit(&msgobj);
+ return 1;
+ }
+ return 0;
+}
+
+uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
+{
+ out_ts_monotonic = clock::getMonotonic();
+ out_flags = 0; // We don't support any IO flags
+
+ CriticalSectionLocker locker;
+ if (rx_queue.getLength() == 0)
+ {
+ return 0;
+ }
+ std::uint64_t ts_utc = 0;
+ rx_queue.pop(out_frame, ts_utc);
+ out_ts_utc = uavcan::UtcTime::fromUSec(ts_utc);
+ return 1;
+}
+
+uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime blocking_deadline)
+{
+ const bool bus_off = isInBusOffState();
+ if (bus_off) // Recover automatically on bus-off
+ {
+ CriticalSectionLocker locker;
+ if ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0)
+ {
+ c_can::CAN.CNTL &= ~c_can::CNTL_INIT;
+ }
+ }
+
+ const bool noblock = ((inout_masks.read == 1) && hasReadyRx()) ||
+ ((inout_masks.write == 1) && hasEmptyTx());
+
+ if (!noblock && (clock::getMonotonic() > blocking_deadline))
+ {
+#if defined(UAVCAN_LPC11C24_USE_WFE) && UAVCAN_LPC11C24_USE_WFE
+ /*
+ * It's not cool (literally) to burn cycles in a busyloop, and we have no OS to pass control to other
+ * tasks, thus solution is to halt the core until a hardware event occurs - e.g. clock timer overflow.
+ * Upon such event the select() call will return, even if no requested IO operations became available.
+ * It's OK to do that, libuavcan can handle such behavior.
+ *
+ * Note that it is not possible to precisely control the sleep duration with WFE, since we can't predict when
+ * the next hardware event occurs. Worst case conditions:
+ * - WFE gets executed right after the clock timer interrupt;
+ * - CAN bus is completely silent (no traffic);
+ * - User's application has no interrupts and generates no hardware events.
+ * In such scenario execution will stuck here for one period of the clock timer interrupt, even if
+ * blocking_deadline expires sooner.
+ * If the user's application requires higher timing precision, an extra dummy IRQ can be added just to
+ * break WFE every once in a while.
+ */
+ __WFE();
+#endif
+ }
+
+ inout_masks.read = hasReadyRx() ? 1 : 0;
+
+ inout_masks.write = (hasEmptyTx() && !bus_off) ? 1 : 0; // Disable write while in bus-off
+
+ return 0; // Return value doesn't matter as long as it is non-negative
+}
+
+uavcan::int16_t CanDriver::configureFilters(const uavcan::CanFilterConfig* filter_configs,
+ uavcan::uint16_t num_configs)
+{
+ CriticalSectionLocker locker;
+
+ /*
+ * If C_CAN is active (INIT=0) and the CAN bus has intensive traffic, RX object configuration may fail.
+ * The solution is to disable the controller while configuration is in progress.
+ * The documentation, as always, doesn't bother to mention this detail. Shame on you, NXP.
+ */
+ struct RAIIDisabler
+ {
+ RAIIDisabler()
+ {
+ c_can::CAN.CNTL |= c_can::CNTL_INIT;
+ }
+ ~RAIIDisabler()
+ {
+ c_can::CAN.CNTL &= ~c_can::CNTL_INIT;
+ }
+ } can_disabler; // Must be instantiated AFTER the critical section locker
+
+ if (num_configs == 0)
+ {
+ auto msg_obj = CCAN_MSG_OBJ_T();
+ msg_obj.msgobj = NumberOfTxMessageObjects + 1;
+ LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all STD frames
+
+ msg_obj.mode_id = CAN_MSGOBJ_EXT;
+ msg_obj.msgobj = NumberOfTxMessageObjects + 2;
+ LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all EXT frames
+ }
+ else if (num_configs <= NumberOfRxMessageObjects)
+ {
+ // Making sure the configs use only EXT frames; otherwise we can't accept them
+ for (unsigned i = 0; i < num_configs; i++)
+ {
+ auto& f = filter_configs[i];
+ if ((f.id & f.mask & uavcan::CanFrame::FlagEFF) == 0)
+ {
+ return -1;
+ }
+ }
+
+ // Installing the configuration
+ for (unsigned i = 0; i < NumberOfRxMessageObjects; i++)
+ {
+ auto msg_obj = CCAN_MSG_OBJ_T();
+ msg_obj.msgobj = std::uint8_t(i + 1U + NumberOfTxMessageObjects); // Message objects are numbered from 1
+
+ if (i < num_configs)
+ {
+ msg_obj.mode_id = (filter_configs[i].id & uavcan::CanFrame::MaskExtID) | CAN_MSGOBJ_EXT; // Only EXT
+ msg_obj.mask = filter_configs[i].mask & uavcan::CanFrame::MaskExtID;
+ }
+ else
+ {
+ msg_obj.mode_id = CAN_MSGOBJ_RTR; // Using this configuration to disable the object
+ msg_obj.mask = uavcan::CanFrame::MaskStdID;
+ }
+
+ LPC_CCAN_API->config_rxmsgobj(&msg_obj);
+ }
+ }
+ else
+ {
+ return -1;
+ }
+
+ return 0;
+}
+
+uavcan::uint64_t CanDriver::getErrorCount() const
+{
+ CriticalSectionLocker locker;
+ return std::uint64_t(error_cnt) + std::uint64_t(rx_queue.getOverflowCount());
+}
+
+uavcan::uint16_t CanDriver::getNumFilters() const
+{
+ return NumberOfRxMessageObjects;
+}
+
+uavcan::ICanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
+{
+ return (iface_index == 0) ? this : nullptr;
+}
+
+uavcan::uint8_t CanDriver::getNumIfaces() const
+{
+ return 1;
+}
+
+}
+
+/*
+ * C_CAN handlers
+ */
+extern "C"
+{
+
+void canRxCallback(std::uint8_t msg_obj_num)
+{
+ using namespace uavcan_lpc11c24;
+
+ auto msg_obj = CCAN_MSG_OBJ_T();
+ msg_obj.msgobj = msg_obj_num;
+ LPC_CCAN_API->can_receive(&msg_obj);
+
+ uavcan::CanFrame frame;
+
+ // CAN ID, EXT or not
+ if (msg_obj.mode_id & CAN_MSGOBJ_EXT)
+ {
+ frame.id = msg_obj.mode_id & uavcan::CanFrame::MaskExtID;
+ frame.id |= uavcan::CanFrame::FlagEFF;
+ }
+ else
+ {
+ frame.id = msg_obj.mode_id & uavcan::CanFrame::MaskStdID;
+ }
+
+ // RTR
+ if (msg_obj.mode_id & CAN_MSGOBJ_RTR)
+ {
+ frame.id |= uavcan::CanFrame::FlagRTR;
+ }
+
+ // Payload
+ frame.dlc = msg_obj.dlc;
+ uavcan::copy(msg_obj.data, msg_obj.data + msg_obj.dlc, frame.data);
+
+ rx_queue.push(frame, last_irq_utc_timestamp);
+ had_activity = true;
+}
+
+void canTxCallback(std::uint8_t msg_obj_num)
+{
+ using namespace uavcan_lpc11c24;
+
+ (void)msg_obj_num;
+
+ tx_pending = false;
+ had_activity = true;
+}
+
+void canErrorCallback(std::uint32_t error_info)
+{
+ using namespace uavcan_lpc11c24;
+
+ // Updating the error counter
+ if ((error_info != 0) && (error_cnt < 0xFFFFFFFFUL))
+ {
+ error_cnt++;
+ }
+
+ // Serving abort requests
+ if (tx_pending && tx_abort_on_error)
+ {
+ tx_pending = false;
+ tx_abort_on_error = false;
+
+ // Using the first interface, because this approach seems to be compliant with the BASIC mode (just in case)
+ c_can::CAN.IF[0].CMDREQ = TxMessageObjectNumber;
+ c_can::CAN.IF[0].CMDMSK.W = c_can::IF_CMDMSK_W_WR_RD; // Clearing IF_CMDMSK_W_TXRQST
+ c_can::CAN.IF[0].MCTRL &= ~c_can::IF_MCTRL_TXRQST; // Clearing IF_MCTRL_TXRQST
+ }
+}
+
+void CAN_IRQHandler();
+
+void CAN_IRQHandler()
+{
+ using namespace uavcan_lpc11c24;
+
+ last_irq_utc_timestamp = clock::getUtcUSecFromCanInterrupt();
+
+ LPC_CCAN_API->isr();
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/driver/src/clock.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_lpc11c24/clock.hpp>
+#include <uavcan/util/templates.hpp>
+#include <chip.h>
+#include "internal.hpp"
+
+namespace uavcan_lpc11c24
+{
+namespace clock
+{
+namespace
+{
+
+bool initialized = false;
+bool utc_set = false;
+
+std::int32_t utc_correction_usec_per_overflow_x16 = 0;
+std::int64_t prev_adjustment = 0;
+
+std::uint64_t time_mono = 0;
+std::uint64_t time_utc = 0;
+
+/**
+ * If this value is too large for the given core clock, reload value will be out of the 24-bit integer range.
+ * This will be detected at run time during timer initialization - refer to SysTick_Config().
+ */
+constexpr std::uint32_t USecPerOverflow = 65536 * 2;
+constexpr std::int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
+
+}
+
+#if __GNUC__
+__attribute__((noreturn))
+#endif
+static void fail()
+{
+ while (true) { }
+}
+
+void init()
+{
+ CriticalSectionLocker lock;
+ if (!initialized)
+ {
+ initialized = true;
+
+ if ((SystemCoreClock % 1000000) != 0) // Core clock frequency validation
+ {
+ fail();
+ }
+
+ if (SysTick_Config((SystemCoreClock / 1000000) * USecPerOverflow) != 0)
+ {
+ fail();
+ }
+ }
+}
+
+static std::uint64_t sampleFromCriticalSection(const volatile std::uint64_t* const value)
+{
+ const std::uint32_t reload = SysTick->LOAD + 1; // SysTick counts downwards, hence the value subtracted from reload
+
+ volatile std::uint64_t time = *value;
+ volatile std::uint32_t cycles = reload - SysTick->VAL;
+
+ if ((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) == SCB_ICSR_PENDSTSET_Msk)
+ {
+ cycles = reload - SysTick->VAL;
+ time += USecPerOverflow;
+ }
+ const std::uint32_t cycles_per_usec = SystemCoreClock / 1000000;
+ return time + (cycles / cycles_per_usec);
+}
+
+std::uint64_t getUtcUSecFromCanInterrupt()
+{
+ return utc_set ? sampleFromCriticalSection(&time_utc) : 0;
+}
+
+uavcan::MonotonicTime getMonotonic()
+{
+ if (!initialized)
+ {
+ fail();
+ }
+ std::uint64_t usec = 0;
+ {
+ CriticalSectionLocker locker;
+ usec = sampleFromCriticalSection(&time_mono);
+ }
+ return uavcan::MonotonicTime::fromUSec(usec);
+}
+
+uavcan::UtcTime getUtc()
+{
+ if (!initialized)
+ {
+ fail();
+ }
+ std::uint64_t usec = 0;
+ if (utc_set)
+ {
+ CriticalSectionLocker locker;
+ usec = sampleFromCriticalSection(&time_utc);
+ }
+ return uavcan::UtcTime::fromUSec(usec);
+}
+
+uavcan::UtcDuration getPrevUtcAdjustment()
+{
+ return uavcan::UtcDuration::fromUSec(prev_adjustment);
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+ const std::int64_t adj_delta = adjustment.toUSec() - prev_adjustment; // This is the P term
+ prev_adjustment = adjustment.toUSec();
+
+ utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
+ utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1; // P
+
+ utc_correction_usec_per_overflow_x16 =
+ uavcan::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
+ utc_correction_usec_per_overflow_x16 =
+ uavcan::min(utc_correction_usec_per_overflow_x16, MaxUtcSpeedCorrectionX16);
+
+ if (adjustment.getAbs().toMSec() > 9 || !utc_set)
+ {
+ const std::int64_t adj_usec = adjustment.toUSec();
+ {
+ CriticalSectionLocker locker;
+ if ((adj_usec < 0) && std::uint64_t(-adj_usec) > time_utc)
+ {
+ time_utc = 1;
+ }
+ else
+ {
+ time_utc = std::uint64_t(std::int64_t(time_utc) + adj_usec);
+ }
+ }
+ if (!utc_set)
+ {
+ utc_set = true;
+ utc_correction_usec_per_overflow_x16 = 0;
+ }
+ }
+}
+
+} // namespace clock
+
+SystemClock SystemClock::self;
+
+SystemClock& SystemClock::instance()
+{
+ clock::init();
+ return self;
+}
+
+}
+
+/*
+ * Timer interrupt handler
+ */
+extern "C"
+{
+
+void SysTick_Handler();
+
+void SysTick_Handler()
+{
+ using namespace uavcan_lpc11c24::clock;
+ if (initialized)
+ {
+ time_mono += USecPerOverflow;
+ if (utc_set)
+ {
+ // Values below 16 are ignored
+ time_utc += std::uint64_t(std::int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
+ }
+ }
+ else
+ {
+ fail();
+ }
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/driver/src/internal.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <chip.h>
+
+/*
+ * Compiler version check
+ */
+#ifdef __GNUC__
+# if (__GNUC__ * 10 + __GNUC_MINOR__) < 49
+# error "Use GCC 4.9 or newer"
+# endif
+#endif
+
+
+namespace uavcan_lpc11c24
+{
+
+/**
+ * Locks UAVCAN driver interrupts.
+ * TODO: priority.
+ */
+struct CriticalSectionLocker
+{
+ CriticalSectionLocker()
+ {
+ __disable_irq();
+ }
+ ~CriticalSectionLocker()
+ {
+ __enable_irq();
+ }
+};
+
+/**
+ * Internal for the driver
+ */
+namespace clock
+{
+
+std::uint64_t getUtcUSecFromCanInterrupt();
+
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,123 @@ +# +# Pavel Kirienko, 2014 <pavel.kirienko@gmail.com> +# + +CPPSRC := $(wildcard src/*.cpp) \ + $(wildcard src/sys/*.cpp) + +CSRC := $(wildcard lpc_chip_11cxx_lib/src/*.c) \ + $(wildcard src/sys/*.c) + +DEF = -DFW_VERSION_MAJOR=1 -DFW_VERSION_MINOR=0 + +INC = -Isrc/sys \ + -isystem lpc_chip_11cxx_lib/inc + +# +# UAVCAN library +# + +DEF += -DUAVCAN_TINY=1 + +include ../../../libuavcan/include.mk +CPPSRC += $(LIBUAVCAN_SRC) +INC += -I$(LIBUAVCAN_INC) + +include ../driver/include.mk +CPPSRC += $(LIBUAVCAN_LPC11C24_SRC) +INC += -I$(LIBUAVCAN_LPC11C24_INC) + +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +INC += -Idsdlc_generated + +# +# Git commit hash +# + +GIT_HASH := $(shell git rev-parse --short HEAD) +ifeq ($(words $(GIT_HASH)),1) + DEF += -DGIT_HASH=0x$(GIT_HASH) +endif + +# +# Build configuration +# + +BUILDDIR = build +OBJDIR = $(BUILDDIR)/obj +DEPDIR = $(BUILDDIR)/dep + +DEF += -DNDEBUG -DCHIP_LPC11CXX -DCORE_M0 -DTHUMB_NO_INTERWORKING -U__STRICT_ANSI__ + +FLAGS = -mthumb -mcpu=cortex-m0 -mno-thumb-interwork -flto -Os -g3 -Wall -Wextra -Werror -Wundef -ffunction-sections \ + -fdata-sections -fno-common -fno-exceptions -fno-unwind-tables -fno-stack-protector -fomit-frame-pointer \ + -Wfloat-equal -Wconversion -Wsign-conversion -Wmissing-declarations + +C_CPP_FLAGS = $(FLAGS) -MD -MP -MF $(DEPDIR)/$(@F).d + +CFLAGS = $(C_CPP_FLAGS) -std=c99 + +CPPFLAGS = $(C_CPP_FLAGS) -pedantic -std=c++11 -fno-rtti -fno-threadsafe-statics + +LDFLAGS = $(FLAGS) -nodefaultlibs -lc -lgcc -nostartfiles -Tlpc11c24.ld -Xlinker --gc-sections \ + -Wl,-Map,$(BUILDDIR)/output.map + +# Link with nano newlib. Other toolchains may not support this option, so it can be safely removed. +LDFLAGS += --specs=nano.specs + +COBJ = $(addprefix $(OBJDIR)/, $(notdir $(CSRC:.c=.o))) +CPPOBJ = $(addprefix $(OBJDIR)/, $(notdir $(CPPSRC:.cpp=.o))) +OBJ = $(COBJ) $(CPPOBJ) + +VPATH = $(sort $(dir $(CSRC)) $(dir $(CPPSRC))) + +ELF = $(BUILDDIR)/firmware.elf +BIN = $(BUILDDIR)/firmware.bin + +# +# Rules +# + +TOOLCHAIN ?= arm-none-eabi- +CC = $(TOOLCHAIN)gcc +CPPC = $(TOOLCHAIN)g++ +AS = $(TOOLCHAIN)gcc +LD = $(TOOLCHAIN)g++ +CP = $(TOOLCHAIN)objcopy +SIZE = $(TOOLCHAIN)size + +all: $(OBJ) $(ELF) $(BIN) size + +$(OBJ): | $(BUILDDIR) + +$(BUILDDIR): + @mkdir -p $(BUILDDIR) + @mkdir -p $(DEPDIR) + @mkdir -p $(OBJDIR) + +$(BIN): $(ELF) + @echo + $(CP) -O binary $(ELF) $@ + +$(ELF): $(OBJ) + @echo + $(LD) $(OBJ) $(LDFLAGS) -o $@ + +$(COBJ): $(OBJDIR)/%.o: %.c + @echo + $(CC) -c $(DEF) $(INC) $(CFLAGS) $< -o $@ + +$(CPPOBJ): $(OBJDIR)/%.o: %.cpp + @echo + $(CPPC) -c $(DEF) $(INC) $(CPPFLAGS) $< -o $@ + +clean: + rm -rf $(BUILDDIR) dsdlc_generated + +size: $(ELF) + @if [ -f $(ELF) ]; then echo; $(SIZE) $(ELF); echo; fi; + +.PHONY: all clean size $(BUILDDIR) + +# Include the dependency files, should be the last of the makefile +-include $(shell mkdir $(DEPDIR) 2>/dev/null) $(wildcard $(DEPDIR)/*)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/blackmagic.gdbinit Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,11 @@ +# +# Template for .gdbinit +# Copy the file to .gdbinit in your project root, and adjust the path below to match your system +# + +target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE578CC-if00 +# target extended /dev/ttyACM0 + +monitor swdp_scan +attach 1 +monitor vector_catch disable hard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/blackmagic_flash.sh Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,23 @@
+#!/bin/bash
+#
+# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+PORT=${1:-'/dev/ttyACM0'}
+#/dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE578CC-if00
+
+elf=build/firmware.elf
+
+arm-none-eabi-size $elf || exit 1
+
+tmpfile=fwupload.tempfile
+cat > $tmpfile <<EOF
+target extended-remote $PORT
+mon swdp_scan
+attach 1
+load
+kill
+EOF
+
+arm-none-eabi-gdb $elf --batch -x $tmpfile
+rm $tmpfile
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc11c24.ld Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,104 @@
+/*
+ * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ * Linker script for LPC11C24
+ */
+
+MEMORY
+{
+ FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 32K
+ /* Notice RAM offset - this is needed for on-chip CCAN */
+ RAM (rwx) : ORIGIN = 0x100000C0, LENGTH = 0x1F40
+}
+
+ENTRY(Reset_Handler)
+
+SECTIONS
+{
+ . = 0;
+ _text = .;
+
+ startup :
+ {
+ KEEP(*(vectors))
+ } > FLASH
+
+ constructors : ALIGN(4) SUBALIGN(4)
+ {
+ PROVIDE(__init_array_start = .);
+ KEEP(*(SORT(.init_array.*)))
+ KEEP(*(.init_array))
+ PROVIDE(__init_array_end = .);
+ } > FLASH
+
+ /* NO DESTRUCTORS */
+
+ .text :
+ {
+ *(.text.startup.*)
+ *(.text)
+ *(.text.*)
+ *(.rodata)
+ *(.rodata.*)
+ *(.glue_7t)
+ *(.glue_7)
+ *(.gcc*)
+ } > FLASH
+
+ .ARM.extab :
+ {
+ *(.ARM.extab* .gnu.linkonce.armextab.*)
+ } > FLASH
+
+ .ARM.exidx : {
+ PROVIDE(__exidx_start = .);
+ *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+ PROVIDE(__exidx_end = .);
+ } > FLASH
+
+ .eh_frame_hdr :
+ {
+ *(.eh_frame_hdr)
+ } > FLASH
+
+ .eh_frame : ONLY_IF_RO
+ {
+ *(.eh_frame)
+ } > FLASH
+
+ .textalign : ONLY_IF_RO
+ {
+ . = ALIGN(8);
+ } > FLASH
+
+ . = ALIGN(4);
+ _etext = .;
+ _textdata = _etext;
+
+ .data :
+ {
+ . = ALIGN(4);
+ PROVIDE(_data = .);
+ *(.data)
+ . = ALIGN(4);
+ *(.data.*)
+ . = ALIGN(4);
+ *(.ramtext)
+ . = ALIGN(4);
+ PROVIDE(_edata = .);
+ } > RAM AT > FLASH
+
+ .bss :
+ {
+ . = ALIGN(4);
+ PROVIDE(_bss = .);
+ *(.bss)
+ . = ALIGN(4);
+ *(.bss.*)
+ . = ALIGN(4);
+ *(COMMON)
+ . = ALIGN(4);
+ PROVIDE(_ebss = .);
+ } > RAM
+
+ PROVIDE(__stack_end = ORIGIN(RAM) + LENGTH(RAM));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/adc_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,271 @@
+/*
+ * @brief LPC11xx A/D conversion driver (except LPC1125)
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __ADC_11XX_H_
+#define __ADC_11XX_H_
+
+#if !defined(CHIP_LPC1125)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup ADC_11XX CHIP: LPC11xx A/D conversion driver
+ * @ingroup CHIP_11XX_Drivers
+ * This ADC driver is for LPC11xx variants except for LPC1125.
+ * @{
+ */
+
+#define ADC_MAX_SAMPLE_RATE 400000
+
+/**
+ * @brief 10 or 12-bit ADC register block structure
+ */
+typedef struct { /*!< ADCn Structure */
+ __IO uint32_t CR; /*!< A/D Control Register. The AD0CR register must be written to select the operating mode before A/D conversion can occur. */
+ __I uint32_t GDR; /*!< A/D Global Data Register. Contains the result of the most recent A/D conversion. */
+ __I uint32_t RESERVED0;
+ __IO uint32_t INTEN; /*!< A/D Interrupt Enable Register. This register contains enable bits that allow the DONE flag of each A/D channel to be included or excluded from contributing to the generation of an A/D interrupt. */
+ __I uint32_t DR[8]; /*!< A/D Channel Data Register. This register contains the result of the most recent conversion completed on channel n. */
+ __I uint32_t STAT; /*!< A/D Status Register. This register contains DONE and OVERRUN flags for all of the A/D channels, as well as the A/D interrupt flag. */
+} LPC_ADC_T;
+
+/**
+ * @brief ADC register support bitfields and mask
+ */
+ #define ADC_DR_RESULT(n) ((((n) >> 6) & 0x3FF)) /*!< Mask for getting the 10 bits ADC data read value */
+ #define ADC_CR_BITACC(n) ((((n) & 0x7) << 17)) /*!< Number of ADC accuracy bits */
+
+#define ADC_DR_DONE(n) (((n) >> 31)) /*!< Mask for reading the ADC done status */
+#define ADC_DR_OVERRUN(n) ((((n) >> 30) & (1UL))) /*!< Mask for reading the ADC overrun status */
+#define ADC_CR_CH_SEL(n) ((1UL << (n))) /*!< Selects which of the AD0.0:7 pins is (are) to be sampled and converted */
+#define ADC_CR_CLKDIV(n) ((((n) & 0xFF) << 8)) /*!< The APB clock (PCLK) is divided by (this value plus one) to produce the clock for the A/D */
+#define ADC_CR_BURST ((1UL << 16)) /*!< Repeated conversions A/D enable bit */
+#define ADC_CR_PDN ((1UL << 21)) /*!< ADC convert is operational */
+#define ADC_CR_START_MASK ((7UL << 24)) /*!< ADC start mask bits */
+#define ADC_CR_START_MODE_SEL(SEL) ((SEL << 24)) /*!< Select Start Mode */
+#define ADC_CR_START_NOW ((1UL << 24)) /*!< Start conversion now */
+#define ADC_CR_START_CTOUT15 ((2UL << 24)) /*!< Start conversion when the edge selected by bit 27 occurs on CTOUT_15 */
+#define ADC_CR_START_CTOUT8 ((3UL << 24)) /*!< Start conversion when the edge selected by bit 27 occurs on CTOUT_8 */
+#define ADC_CR_START_ADCTRIG0 ((4UL << 24)) /*!< Start conversion when the edge selected by bit 27 occurs on ADCTRIG0 */
+#define ADC_CR_START_ADCTRIG1 ((5UL << 24)) /*!< Start conversion when the edge selected by bit 27 occurs on ADCTRIG1 */
+#define ADC_CR_START_MCOA2 ((6UL << 24)) /*!< Start conversion when the edge selected by bit 27 occurs on Motocon PWM output MCOA2 */
+#define ADC_CR_EDGE ((1UL << 27)) /*!< Start conversion on a falling edge on the selected CAP/MAT signal */
+#define ADC_SAMPLE_RATE_CONFIG_MASK (ADC_CR_CLKDIV(0xFF) | ADC_CR_BITACC(0x07))
+
+/**
+ * @brief ADC status register used for IP drivers
+ */
+typedef enum IP_ADC_STATUS {
+ ADC_DR_DONE_STAT, /*!< ADC data register staus */
+ ADC_DR_OVERRUN_STAT,/*!< ADC data overrun staus */
+ ADC_DR_ADINT_STAT /*!< ADC interrupt status */
+} ADC_STATUS_T;
+
+/** The channels on one ADC peripheral*/
+typedef enum CHIP_ADC_CHANNEL {
+ ADC_CH0 = 0, /**< ADC channel 0 */
+ ADC_CH1, /**< ADC channel 1 */
+ ADC_CH2, /**< ADC channel 2 */
+ ADC_CH3, /**< ADC channel 3 */
+ ADC_CH4, /**< ADC channel 4 */
+ ADC_CH5, /**< ADC channel 5 */
+ ADC_CH6, /**< ADC channel 6 */
+ ADC_CH7, /**< ADC channel 7 */
+} ADC_CHANNEL_T;
+
+/** The number of bits of accuracy of the result in the LS bits of ADDR*/
+typedef enum CHIP_ADC_RESOLUTION {
+ ADC_10BITS = 0, /**< ADC 10 bits */
+ ADC_9BITS, /**< ADC 9 bits */
+ ADC_8BITS, /**< ADC 8 bits */
+ ADC_7BITS, /**< ADC 7 bits */
+ ADC_6BITS, /**< ADC 6 bits */
+ ADC_5BITS, /**< ADC 5 bits */
+ ADC_4BITS, /**< ADC 4 bits */
+ ADC_3BITS, /**< ADC 3 bits */
+} ADC_RESOLUTION_T;
+
+/** Edge configuration, which controls rising or falling edge on the selected signal for the start of a conversion */
+typedef enum CHIP_ADC_EDGE_CFG {
+ ADC_TRIGGERMODE_RISING = 0, /**< Trigger event: rising edge */
+ ADC_TRIGGERMODE_FALLING, /**< Trigger event: falling edge */
+} ADC_EDGE_CFG_T;
+
+/** Start mode, which controls the start of an A/D conversion when the BURST bit is 0. */
+typedef enum CHIP_ADC_START_MODE {
+ ADC_NO_START = 0,
+ ADC_START_NOW, /*!< Start conversion now */
+ ADC_START_ON_CTOUT15, /*!< Start conversion when the edge selected by bit 27 occurs on CTOUT_15 */
+ ADC_START_ON_CTOUT8, /*!< Start conversion when the edge selected by bit 27 occurs on CTOUT_8 */
+ ADC_START_ON_ADCTRIG0, /*!< Start conversion when the edge selected by bit 27 occurs on ADCTRIG0 */
+ ADC_START_ON_ADCTRIG1, /*!< Start conversion when the edge selected by bit 27 occurs on ADCTRIG1 */
+ ADC_START_ON_MCOA2 /*!< Start conversion when the edge selected by bit 27 occurs on Motocon PWM output MCOA2 */
+} ADC_START_MODE_T;
+
+/** Clock setup structure for ADC controller passed to the initialize function */
+typedef struct {
+ uint32_t adcRate; /*!< ADC rate */
+ uint8_t bitsAccuracy; /*!< ADC bit accuracy */
+ bool burstMode; /*!< ADC Burt Mode */
+} ADC_CLOCK_SETUP_T;
+
+/**
+ * @brief Initialize the ADC peripheral and the ADC setup structure to default value
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param ADCSetup : ADC setup structure to be set
+ * @return Nothing
+ * @note Default setting for ADC is 400kHz - 10bits
+ */
+void Chip_ADC_Init(LPC_ADC_T *pADC, ADC_CLOCK_SETUP_T *ADCSetup);
+
+/**
+ * @brief Shutdown ADC
+ * @param pADC : The base of ADC peripheral on the chip
+ * @return Nothing
+ */
+void Chip_ADC_DeInit(LPC_ADC_T *pADC);
+
+/**
+ * @brief Read the ADC value from a channel
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param channel : ADC channel to read
+ * @param data : Pointer to where to put data
+ * @return SUCCESS or ERROR if no conversion is ready
+ */
+Status Chip_ADC_ReadValue(LPC_ADC_T *pADC, uint8_t channel, uint16_t *data);
+
+/**
+ * @brief Read the ADC value and convert it to 8bits value
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param channel: selected channel
+ * @param data : Storage for data
+ * @return Status : ERROR or SUCCESS
+ */
+Status Chip_ADC_ReadByte(LPC_ADC_T *pADC, ADC_CHANNEL_T channel, uint8_t *data);
+
+/**
+ * @brief Read the ADC channel status
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param channel : ADC channel to read
+ * @param StatusType : Status type of ADC_DR_*
+ * @return SET or RESET
+ */
+FlagStatus Chip_ADC_ReadStatus(LPC_ADC_T *pADC, uint8_t channel, uint32_t StatusType);
+
+/**
+ * @brief Enable/Disable interrupt for ADC channel
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param channel : ADC channel to read
+ * @param NewState : New state, ENABLE or DISABLE
+ * @return SET or RESET
+ */
+void Chip_ADC_Int_SetChannelCmd(LPC_ADC_T *pADC, uint8_t channel, FunctionalState NewState);
+
+/**
+ * @brief Enable/Disable global interrupt for ADC channel
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param NewState : New state, ENABLE or DISABLE
+ * @return Nothing
+ */
+STATIC INLINE void Chip_ADC_Int_SetGlobalCmd(LPC_ADC_T *pADC, FunctionalState NewState)
+{
+ Chip_ADC_Int_SetChannelCmd(pADC, 8, NewState);
+}
+
+/**
+ * @brief Select the mode starting the AD conversion
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param mode : Stating mode, should be :
+ * - ADC_NO_START : Must be set for Burst mode
+ * - ADC_START_NOW : Start conversion now
+ * - ADC_START_ON_CTOUT15 : Start conversion when the edge selected by bit 27 occurs on CTOUT_15
+ * - ADC_START_ON_CTOUT8 : Start conversion when the edge selected by bit 27 occurs on CTOUT_8
+ * - ADC_START_ON_ADCTRIG0 : Start conversion when the edge selected by bit 27 occurs on ADCTRIG0
+ * - ADC_START_ON_ADCTRIG1 : Start conversion when the edge selected by bit 27 occurs on ADCTRIG1
+ * - ADC_START_ON_MCOA2 : Start conversion when the edge selected by bit 27 occurs on Motocon PWM output MCOA2
+ * @param EdgeOption : Stating Edge Condition, should be :
+ * - ADC_TRIGGERMODE_RISING : Trigger event on rising edge
+ * - ADC_TRIGGERMODE_FALLING : Trigger event on falling edge
+ * @return Nothing
+ */
+void Chip_ADC_SetStartMode(LPC_ADC_T *pADC, ADC_START_MODE_T mode, ADC_EDGE_CFG_T EdgeOption);
+
+/**
+ * @brief Set the ADC Sample rate
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param ADCSetup : ADC setup structure to be modified
+ * @param rate : Sample rate, should be set so the clock for A/D converter is less than or equal to 4.5MHz.
+ * @return Nothing
+ */
+void Chip_ADC_SetSampleRate(LPC_ADC_T *pADC, ADC_CLOCK_SETUP_T *ADCSetup, uint32_t rate);
+
+/**
+ * @brief Set the ADC accuracy bits
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param ADCSetup : ADC setup structure to be modified
+ * @param resolution : The resolution, should be ADC_10BITS -> ADC_3BITS
+ * @return Nothing
+ */
+void Chip_ADC_SetResolution(LPC_ADC_T *pADC, ADC_CLOCK_SETUP_T *ADCSetup, ADC_RESOLUTION_T resolution);
+
+/**
+ * @brief Enable or disable the ADC channel on ADC peripheral
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param channel : Channel to be enable or disable
+ * @param NewState : New state, should be:
+ * - ENABLE
+ * - DISABLE
+ * @return Nothing
+ */
+void Chip_ADC_EnableChannel(LPC_ADC_T *pADC, ADC_CHANNEL_T channel, FunctionalState NewState);
+
+/**
+ * @brief Enable burst mode
+ * @param pADC : The base of ADC peripheral on the chip
+ * @param NewState : New state, should be:
+ * - ENABLE
+ * - DISABLE
+ * @return Nothing
+ */
+void Chip_ADC_SetBurstCmd(LPC_ADC_T *pADC, FunctionalState NewState);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* !defined(CHIP_LPC1125) */
+
+#endif /* __ADC_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/ccand_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,170 @@
+/*
+ * @brief LPC11xx CCAN ROM API declarations and functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __CCAND_11XX_H_
+#define __CCAND_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup CCANROM_11XX CHIP: LPC11xx CCAN ROM Driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * CCAN ROM error status bits
+ */
+#define CAN_ERROR_NONE 0x00000000UL
+#define CAN_ERROR_PASS 0x00000001UL
+#define CAN_ERROR_WARN 0x00000002UL
+#define CAN_ERROR_BOFF 0x00000004UL
+#define CAN_ERROR_STUF 0x00000008UL
+#define CAN_ERROR_FORM 0x00000010UL
+#define CAN_ERROR_ACK 0x00000020UL
+#define CAN_ERROR_BIT1 0x00000040UL
+#define CAN_ERROR_BIT0 0x00000080UL
+#define CAN_ERROR_CRC 0x00000100UL
+
+/**
+ * CCAN ROM control bits for CAN_MSG_OBJ.mode_id
+ */
+#define CAN_MSGOBJ_STD 0x00000000UL /* CAN 2.0a 11-bit ID */
+#define CAN_MSGOBJ_EXT 0x20000000UL /* CAN 2.0b 29-bit ID */
+#define CAN_MSGOBJ_DAT 0x00000000UL /* data frame */
+#define CAN_MSGOBJ_RTR 0x40000000UL /* rtr frame */
+
+typedef struct CCAN_MSG_OBJ {
+ uint32_t mode_id;
+ uint32_t mask;
+ uint8_t data[8];
+ uint8_t dlc;
+ uint8_t msgobj;
+} CCAN_MSG_OBJ_T;
+
+/**************************************************************************
+ SDO Abort Codes
+**************************************************************************/
+#define SDO_ABORT_TOGGLE 0x05030000UL // Toggle bit not alternated
+#define SDO_ABORT_SDOTIMEOUT 0x05040000UL // SDO protocol timed out
+#define SDO_ABORT_UNKNOWN_COMMAND 0x05040001UL // Client/server command specifier not valid or unknown
+#define SDO_ABORT_UNSUPPORTED 0x06010000UL // Unsupported access to an object
+#define SDO_ABORT_WRITEONLY 0x06010001UL // Attempt to read a write only object
+#define SDO_ABORT_READONLY 0x06010002UL // Attempt to write a read only object
+#define SDO_ABORT_NOT_EXISTS 0x06020000UL // Object does not exist in the object dictionary
+#define SDO_ABORT_PARAINCOMP 0x06040043UL // General parameter incompatibility reason
+#define SDO_ABORT_ACCINCOMP 0x06040047UL // General internal incompatibility in the device
+#define SDO_ABORT_TYPEMISMATCH 0x06070010UL // Data type does not match, length of service parameter does not match
+#define SDO_ABORT_UNKNOWNSUB 0x06090011UL // Sub-index does not exist
+#define SDO_ABORT_VALUE_RANGE 0x06090030UL // Value range of parameter exceeded (only for write access)
+#define SDO_ABORT_TRANSFER 0x08000020UL // Data cannot be transferred or stored to the application
+#define SDO_ABORT_LOCAL 0x08000021UL // Data cannot be transferred or stored to the application because of local control
+#define SDO_ABORT_DEVSTAT 0x08000022UL // Data cannot be transferred or stored to the application because of the present device state
+
+typedef struct CCAN_ODCONSTENTRY {
+ uint16_t index;
+ uint8_t subindex;
+ uint8_t len;
+ uint32_t val;
+} CCAN_ODCONSTENTRY_T;
+
+// upper-nibble values for CAN_ODENTRY.entrytype_len
+#define OD_NONE 0x00 // Object Dictionary entry doesn't exist
+#define OD_EXP_RO 0x10 // Object Dictionary entry expedited, read-only
+#define OD_EXP_WO 0x20 // Object Dictionary entry expedited, write-only
+#define OD_EXP_RW 0x30 // Object Dictionary entry expedited, read-write
+#define OD_SEG_RO 0x40 // Object Dictionary entry segmented, read-only
+#define OD_SEG_WO 0x50 // Object Dictionary entry segmented, write-only
+#define OD_SEG_RW 0x60 // Object Dictionary entry segmented, read-write
+
+typedef struct CCAN_ODENTRY {
+ uint16_t index;
+ uint8_t subindex;
+ uint8_t entrytype_len;
+ uint8_t *val;
+} CCAN_ODENTRY_T;
+
+typedef struct CCAN_CANOPENCFG {
+ uint8_t node_id;
+ uint8_t msgobj_rx;
+ uint8_t msgobj_tx;
+ uint8_t isr_handled;
+ uint32_t od_const_num;
+ CCAN_ODCONSTENTRY_T *od_const_table;
+ uint32_t od_num;
+ CCAN_ODENTRY_T *od_table;
+} CCAN_CANOPENCFG_T;
+
+// Return values for CANOPEN_sdo_req() callback
+#define CAN_SDOREQ_NOTHANDLED 0 // process regularly, no impact
+#define CAN_SDOREQ_HANDLED_SEND 1 // processed in callback, auto-send returned msg
+#define CAN_SDOREQ_HANDLED_NOSEND 2 // processed in callback, don't send response
+
+// Values for CANOPEN_sdo_seg_read/write() callback 'openclose' parameter
+#define CAN_SDOSEG_SEGMENT 0 // segment read/write
+#define CAN_SDOSEG_OPEN 1 // channel is opened
+#define CAN_SDOSEG_CLOSE 2 // channel is closed
+
+typedef struct CCAN_CALLBACKS {
+ void (*CAN_rx)(uint8_t msg_obj_num);
+ void (*CAN_tx)(uint8_t msg_obj_num);
+ void (*CAN_error)(uint32_t error_info);
+ uint32_t (*CANOPEN_sdo_read)(uint16_t index, uint8_t subindex);
+ uint32_t (*CANOPEN_sdo_write)(uint16_t index, uint8_t subindex, uint8_t *dat_ptr);
+ uint32_t (*CANOPEN_sdo_seg_read)(uint16_t index, uint8_t subindex, uint8_t openclose, uint8_t *length,
+ uint8_t *data, uint8_t *last);
+ uint32_t (*CANOPEN_sdo_seg_write)(uint16_t index, uint8_t subindex, uint8_t openclose, uint8_t length,
+ uint8_t *data, uint8_t *fast_resp);
+ uint8_t (*CANOPEN_sdo_req)(uint8_t length_req, uint8_t *req_ptr, uint8_t *length_resp, uint8_t *resp_ptr);
+} CCAN_CALLBACKS_T;
+
+typedef struct CCAN_API {
+ void (*init_can)(uint32_t *can_cfg, uint8_t isr_ena);
+ void (*isr)(void);
+ void (*config_rxmsgobj)(CCAN_MSG_OBJ_T *msg_obj);
+ uint8_t (*can_receive)(CCAN_MSG_OBJ_T *msg_obj);
+ void (*can_transmit)(CCAN_MSG_OBJ_T *msg_obj);
+ void (*config_canopen)(CCAN_CANOPENCFG_T *canopen_cfg);
+ void (*canopen_handler)(void);
+ void (*config_calb)(CCAN_CALLBACKS_T *callback_cfg);
+} CCAN_API_T;
+
+#define LPC_CCAN_API ((CCAN_API_T *) (LPC_ROM_API->candApiBase))
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CCAND_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/chip.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,309 @@
+/*
+ * @brief LPC11xx basic chip inclusion file
+ *
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __CHIP_H_
+#define __CHIP_H_
+
+#include "lpc_types.h"
+#include "sys_config.h"
+#include "cmsis.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CORE_M0
+#error CORE_M0 is not defined for the LPC11xx architecture
+#error CORE_M0 should be defined as part of your compiler define list
+#endif
+
+#if !defined(ENABLE_UNTESTED_CODE)
+#if defined(CHIP_LPC110X)
+#warning The LCP110X code has not been tested with a platform. This code should \
+build without errors but may not work correctly for the device. To disable this \
+#warning message, define ENABLE_UNTESTED_CODE.
+#endif
+#if defined(CHIP_LPC11XXLV)
+#warning The LPC11XXLV code has not been tested with a platform. This code should \
+build without errors but may not work correctly for the device. To disable this \
+#warning message, define ENABLE_UNTESTED_CODE.
+#endif
+#if defined(CHIP_LPC11AXX)
+#warning The LPC11AXX code has not been tested with a platform. This code should \
+build without errors but may not work correctly for the device. To disable this \
+#warning message, define ENABLE_UNTESTED_CODE.
+#endif
+#if defined(CHIP_LPC11EXX)
+#warning The LPC11EXX code has not been tested with a platform. This code should \
+build without errors but may not work correctly for the device. To disable this \
+warning message, define ENABLE_UNTESTED_CODE.
+#endif
+#endif
+
+#if !defined(CHIP_LPC110X) && !defined(CHIP_LPC11XXLV) && !defined(CHIP_LPC11AXX) && \
+ !defined(CHIP_LPC11CXX) && !defined(CHIP_LPC11EXX) && !defined(CHIP_LPC11UXX) && \
+ !defined(CHIP_LPC1125)
+#error CHIP_LPC110x/CHIP_LPC11XXLV/CHIP_LPC11AXX/CHIP_LPC11CXX/CHIP_LPC11EXX/CHIP_LPC11UXX/CHIP_LPC1125 is not defined!
+#endif
+
+/* Peripheral mapping per device
+ Peripheral Device(s)
+ ---------------------------- -------------------------------------------------------------------------------------------------------------
+ I2C(40000000) CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ WDT(40004000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ UART0(40008000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX CHIP_LPC1125
+ UART1(40020000) CHIP_LPC1125
+ UART2(40024000) CHIP_LPC1125
+ USART/SMARTCARD(40008000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX
+ TIMER0_16(4000C000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ TIMER1_16(40010000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ TIMER0_32(40014000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ TIMER1_32(40018000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ ADC(4001C000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ DAC(40024000) CHIP_LPC11AXX
+ ACMP(40028000) CHIP_LPC11AXX
+ PMU(40038000) CHIP_LPC110x/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX CHIP_LPC1125
+ FLASH_CTRL(4003C000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX CHIP_LPC1125
+ FLASH_EEPROM(4003C000) CHIP_LPC11EXX/ CHIP_LPC11AXX
+ SPI0(40040000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX
+ SSP0(40040000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ IOCONF(40044000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ SYSCON(40048000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX/ CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ GPIOINTS(4004C000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX
+ USB(40080000) CHIP_LPC11UXX
+ CCAN(40050000) CHIP_LPC11CXX
+ SPI1(40058000) CHIP_LPC11CXX
+ SSP1(40058000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX CHIP_LPC1125
+ GPIO_GRP0_INT(4005C000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX
+ GPIO_GRP1_INT(40060000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX
+ GPIO_PORT(50000000) CHIP_LPC11UXX/ CHIP_LPC11EXX/ CHIP_LPC11AXX
+ GPIO_PIO0(50000000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX CHIP_LPC1125
+ GPIO_PIO1(50010000) CHIP_LPC110x/ CHIP_LPC11XXLV/ CHIP_LPC11CXX CHIP_LPC1125
+ GPIO_PIO2(50020000) CHIP_LPC11XXLV/ CHIP_LPC11CXX CHIP_LPC1125
+ GPIO_PIO3(50030000) CHIP_LPC11XXLV/ CHIP_LPC11CXX CHIP_LPC1125
+ */
+
+/** @defgroup PERIPH_11XX_BASE CHIP: LPC11xx Peripheral addresses and register set declarations
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+#define LPC_I2C_BASE 0x40000000
+#define LPC_WWDT_BASE 0x40004000
+#define LPC_USART_BASE 0x40008000
+#define LPC_TIMER16_0_BASE 0x4000C000
+#define LPC_TIMER16_1_BASE 0x40010000
+#define LPC_TIMER32_0_BASE 0x40014000
+#define LPC_TIMER32_1_BASE 0x40018000
+#define LPC_ADC_BASE 0x4001C000
+#define LPC_DAC_BASE 0x40024000
+#define LPC_ACMP_BASE 0x40028000
+#define LPC_PMU_BASE 0x40038000
+#define LPC_FLASH_BASE 0x4003C000
+#define LPC_SSP0_BASE 0x40040000
+#define LPC_IOCON_BASE 0x40044000
+#define LPC_SYSCTL_BASE 0x40048000
+#define LPC_USB0_BASE 0x40080000
+#define LPC_CAN0_BASE 0x40050000
+#define LPC_SSP1_BASE 0x40058000
+#if defined(CHIP_LPC1125)
+#define LPC_USART1_BASE 0x40020000
+#define LPC_USART2_BASE 0x40024000
+#endif
+#if defined(CHIP_LPC11UXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX)
+#define LPC_GPIO_PIN_INT_BASE 0x4004C000
+#define LPC_GPIO_GROUP_INT0_BASE 0x4005C000
+#define LPC_GPIO_GROUP_INT1_BASE 0x40060000
+#define LPC_GPIO_PORT_BASE 0x50000000
+#else
+#define LPC_GPIO_PORT0_BASE 0x50000000
+#define LPC_GPIO_PORT1_BASE 0x50010000
+#if defined(CHIP_LPC11XXLV) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC1125)
+#define LPC_GPIO_PORT2_BASE 0x50020000
+#define LPC_GPIO_PORT3_BASE 0x50030000
+#endif /* defined(CHIP_LPC11XXLV) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC1125) */
+#endif /* defined(CHIP_LPC11UXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX) */
+#define IAP_ENTRY_LOCATION 0X1FFF1FF1
+#define LPC_ROM_API_BASE_LOC 0x1FFF1FF8
+
+#if !defined(CHIP_LPC110x)
+#define LPC_I2C ((LPC_I2C_T *) LPC_I2C_BASE)
+#endif
+
+#define LPC_WWDT ((LPC_WWDT_T *) LPC_WWDT_BASE)
+#define LPC_USART ((LPC_USART_T *) LPC_USART_BASE)
+#define LPC_TIMER16_0 ((LPC_TIMER_T *) LPC_TIMER16_0_BASE)
+#define LPC_TIMER16_1 ((LPC_TIMER_T *) LPC_TIMER16_1_BASE)
+#define LPC_TIMER32_0 ((LPC_TIMER_T *) LPC_TIMER32_0_BASE)
+#define LPC_TIMER32_1 ((LPC_TIMER_T *) LPC_TIMER32_1_BASE)
+#define LPC_ADC ((LPC_ADC_T *) LPC_ADC_BASE)
+
+#if defined(CHIP_LPC1125)
+#define LPC_USART0 LPC_USART
+#define LPC_USART1 ((LPC_USART_T *) LPC_USART1_BASE)
+#define LPC_USART2 ((LPC_USART_T *) LPC_USART2_BASE)
+#endif
+
+#if defined(CHIP_LPC11AXX)
+#define LPC_DAC ((LPC_DAC_T *) LPC_DAC_BASE)
+#define LPC_CMP ((LPC_CMP_T *) LPC_ACMP_BASE)
+#endif
+
+#define LPC_PMU ((LPC_PMU_T *) LPC_PMU_BASE)
+#define LPC_FMC ((LPC_FMC_T *) LPC_FLASH_BASE)
+#define LPC_SSP0 ((LPC_SSP_T *) LPC_SSP0_BASE)
+#define LPC_IOCON ((LPC_IOCON_T *) LPC_IOCON_BASE)
+#define LPC_SYSCTL ((LPC_SYSCTL_T *) LPC_SYSCTL_BASE)
+#if defined(CHIP_LPC11CXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX) || defined(CHIP_LPC1125)
+#define LPC_SSP1 ((LPC_SSP_T *) LPC_SSP1_BASE)
+#endif
+#define LPC_USB ((LPC_USB_T *) LPC_USB0_BASE)
+
+#if defined(CHIP_LPC11UXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX)
+#define LPC_PININT ((LPC_PIN_INT_T *) LPC_GPIO_PIN_INT_BASE)
+#define LPC_GPIOGROUP ((LPC_GPIOGROUPINT_T *) LPC_GPIO_GROUP_INT0_BASE)
+#define LPC_GPIO ((LPC_GPIO_T *) LPC_GPIO_PORT_BASE)
+#else
+#define LPC_GPIO ((LPC_GPIO_T *) LPC_GPIO_PORT0_BASE)
+#endif
+
+#define LPC_ROM_API (*((LPC_ROM_API_T * *) LPC_ROM_API_BASE_LOC))
+
+
+/**
+ * @}
+ */
+
+/** @ingroup CHIP_11XX_DRIVER_OPTIONS
+ */
+
+/**
+ * @brief System oscillator rate
+ * This value is defined externally to the chip layer and contains
+ * the value in Hz for the external oscillator for the board. If using the
+ * internal oscillator, this rate can be 0.
+ */
+extern const uint32_t OscRateIn;
+
+/**
+ * @}
+ */
+
+/** @ingroup CHIP_11XX_DRIVER_OPTIONS
+ */
+
+/**
+ * @brief Clock rate on the CLKIN pin
+ * This value is defined externally to the chip layer and contains
+ * the value in Hz for the CLKIN pin for the board. If this pin isn't used,
+ * this rate can be 0.
+ */
+extern const uint32_t ExtRateIn;
+
+/**
+ * @}
+ */
+
+#include "pmu_11xx.h"
+#include "fmc_11xx.h"
+#include "sysctl_11xx.h"
+#include "clock_11xx.h"
+#include "iocon_11xx.h"
+#include "timer_11xx.h"
+#include "uart_11xx.h"
+#include "wwdt_11xx.h"
+#include "ssp_11xx.h"
+#include "romapi_11xx.h"
+
+#if !defined(CHIP_LPC1125)
+/* All LPC1xx devices except the LPC1125 */
+#include "adc_11xx.h"
+
+#else
+/* LPC1125 has different IP than other LPC11xx devices */
+#include "adc_1125.h"
+#endif
+
+/* Different GPIO/GPIOGROUP/PININT blocks for parts with similar numbers */
+#if defined(CHIP_LPC11CXX) || defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC1125)
+#include "gpio_11xx_2.h"
+#else
+#include "gpio_11xx_1.h"
+#include "gpiogroup_11xx.h"
+#include "pinint_11xx.h"
+#endif
+
+/* Family specific drivers */
+#if defined(CHIP_LPC11AXX)
+#include "acmp_11xx.h"
+#include "dac_11xx.h"
+#endif
+#if !defined(CHIP_LPC110X)
+#include "i2c_11xx.h"
+#endif
+#if defined(CHIP_LPC11CXX)
+#include "ccand_11xx.h"
+#endif
+#if defined(CHIP_LPC11UXX)
+#include "usbd_11xx.h"
+#endif
+
+/** @defgroup SUPPORT_11XX_FUNC CHIP: LPC11xx support functions
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief Current system clock rate, mainly used for sysTick
+ */
+extern uint32_t SystemCoreClock;
+
+/**
+ * @brief Update system core clock rate, should be called if the
+ * system has a clock rate change
+ * @return None
+ */
+void SystemCoreClockUpdate(void);
+
+/**
+ * @brief Set up and initialize hardware prior to call to main()
+ * @return None
+ * @note Chip_SystemInit() is called prior to the application and sets up
+ * system clocking prior to the application starting.
+ */
+void Chip_SystemInit(void);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CHIP_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/clock_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,547 @@
+/*
+ * @brief LPC11XX Clock control functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __CLOCK_11XX_H_
+#define __CLOCK_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup CLOCK_11XX CHIP: LPC11xx Clock Control block driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/** Internal oscillator frequency */
+#define SYSCTL_IRC_FREQ (12000000)
+
+/**
+ * @brief Set System PLL divider values
+ * @param msel : PLL feedback divider value. M = msel + 1.
+ * @param psel : PLL post divider value. P = (1<<psel).
+ * @return Nothing
+ * @note See the user manual for how to setup the PLL.
+ */
+STATIC INLINE void Chip_Clock_SetupSystemPLL(uint8_t msel, uint8_t psel)
+{
+ LPC_SYSCTL->SYSPLLCTRL = (msel & 0x1F) | ((psel & 0x3) << 5);
+}
+
+/**
+ * @brief Read System PLL lock status
+ * @return true of the PLL is locked. false if not locked
+ */
+STATIC INLINE bool Chip_Clock_IsSystemPLLLocked(void)
+{
+ return (bool) ((LPC_SYSCTL->SYSPLLSTAT & 1) != 0);
+}
+
+/**
+ * Clock sources for system and USB PLLs
+ */
+typedef enum CHIP_SYSCTL_PLLCLKSRC {
+ SYSCTL_PLLCLKSRC_IRC = 0, /*!< Internal oscillator in */
+ SYSCTL_PLLCLKSRC_MAINOSC, /*!< Crystal (main) oscillator in */
+#if defined(CHIP_LPC11AXX)
+ SYSCTL_PLLCLKSRC_EXT_CLKIN, /*!< External clock in (11Axx only) */
+#else
+ SYSCTL_PLLCLKSRC_RESERVED1, /*!< Reserved */
+#endif
+ SYSCTL_PLLCLKSRC_RESERVED2, /*!< Reserved */
+} CHIP_SYSCTL_PLLCLKSRC_T;
+
+/**
+ * @brief Set System PLL clock source
+ * @param src : Clock source for system PLL
+ * @return Nothing
+ * @note This function will also toggle the clock source update register
+ * to update the clock source.
+ */
+void Chip_Clock_SetSystemPLLSource(CHIP_SYSCTL_PLLCLKSRC_T src);
+
+#if defined(CHIP_LPC11UXX)
+/**
+ * @brief Set USB PLL divider values
+ * @param msel : PLL feedback divider value. M = msel + 1.
+ * @param psel : PLL post divider value. P = (1<<psel).
+ * @return Nothing
+ * @note See the user manual for how to setup the PLL.
+ */
+STATIC INLINE void Chip_Clock_SetupUSBPLL(uint8_t msel, uint8_t psel)
+{
+ LPC_SYSCTL->USBPLLCTRL = (msel & 0x1F) | ((psel & 0x3) << 5);
+}
+
+/**
+ * @brief Read USB PLL lock status
+ * @return true of the PLL is locked. false if not locked
+ */
+STATIC INLINE bool Chip_Clock_IsUSBPLLLocked(void)
+{
+ return (bool) ((LPC_SYSCTL->USBPLLSTAT & 1) != 0);
+}
+
+/**
+ * @brief Set USB PLL clock source
+ * @param src : Clock source for USB PLL
+ * @return Nothing
+ * @note This function will also toggle the clock source update register
+ * to update the clock source.
+ */
+void Chip_Clock_SetUSBPLLSource(CHIP_SYSCTL_PLLCLKSRC_T src);
+
+#endif /*defined(CHIP_LPC11UXX)*/
+
+/**
+ * @brief Bypass System Oscillator and set oscillator frequency range
+ * @param bypass : Flag to bypass oscillator
+ * @param highfr : Flag to set oscillator range from 15-25 MHz
+ * @return Nothing
+ * @note Sets the PLL input to bypass the oscillator. This would be
+ * used if an external clock that is not an oscillator is attached
+ * to the XTALIN pin.
+ */
+void Chip_Clock_SetPLLBypass(bool bypass, bool highfr);
+
+/**
+ * Watchdog and low frequency oscillator frequencies plus or minus 40%
+ */
+typedef enum CHIP_WDTLFO_OSC {
+ WDTLFO_OSC_ILLEGAL,
+ WDTLFO_OSC_0_60, /*!< 0.6 MHz watchdog/LFO rate */
+ WDTLFO_OSC_1_05, /*!< 1.05 MHz watchdog/LFO rate */
+ WDTLFO_OSC_1_40, /*!< 1.4 MHz watchdog/LFO rate */
+ WDTLFO_OSC_1_75, /*!< 1.75 MHz watchdog/LFO rate */
+ WDTLFO_OSC_2_10, /*!< 2.1 MHz watchdog/LFO rate */
+ WDTLFO_OSC_2_40, /*!< 2.4 MHz watchdog/LFO rate */
+ WDTLFO_OSC_2_70, /*!< 2.7 MHz watchdog/LFO rate */
+ WDTLFO_OSC_3_00, /*!< 3.0 MHz watchdog/LFO rate */
+ WDTLFO_OSC_3_25, /*!< 3.25 MHz watchdog/LFO rate */
+ WDTLFO_OSC_3_50, /*!< 3.5 MHz watchdog/LFO rate */
+ WDTLFO_OSC_3_75, /*!< 3.75 MHz watchdog/LFO rate */
+ WDTLFO_OSC_4_00, /*!< 4.0 MHz watchdog/LFO rate */
+ WDTLFO_OSC_4_20, /*!< 4.2 MHz watchdog/LFO rate */
+ WDTLFO_OSC_4_40, /*!< 4.4 MHz watchdog/LFO rate */
+ WDTLFO_OSC_4_60 /*!< 4.6 MHz watchdog/LFO rate */
+} CHIP_WDTLFO_OSC_T;
+
+/**
+ * @brief Setup Watchdog oscillator rate and divider
+ * @param wdtclk : Selected watchdog clock rate
+ * @param div : Watchdog divider value, even value between 2 and 64
+ * @return Nothing
+ * @note Watchdog rate = selected rate divided by divider rate
+ */
+STATIC INLINE void Chip_Clock_SetWDTOSC(CHIP_WDTLFO_OSC_T wdtclk, uint8_t div)
+{
+ LPC_SYSCTL->WDTOSCCTRL = (((uint32_t) wdtclk) << 5) | ((div >> 1) - 1);
+}
+
+#if defined(CHIP_LPC11AXX)
+/**
+ * @brief Setup low frequency oscillator rate and divider
+ * @param lfoclk : Selected low frequency clock rate
+ * @param div : Low frequency divider value, even value between 2 and 64
+ * @return Nothing
+ * @note Low frequency oscillator rate = selected rate divided by divider rate
+ */
+STATIC INLINE void Chip_Clock_SetLFOSC(CHIP_WDTLFO_OSC_T lfoclk, uint8_t div)
+{
+ LPC_SYSCTL->LFOSCCTRL = (((uint32_t) lfoclk) << 5) | ((div >> 1) - 1);
+}
+
+#endif /*CHIP_LPC11AXX*/
+
+/**
+ * Clock sources for main system clock
+ */
+typedef enum CHIP_SYSCTL_MAINCLKSRC {
+ SYSCTL_MAINCLKSRC_IRC = 0, /*!< Internal oscillator */
+ SYSCTL_MAINCLKSRC_PLLIN, /*!< System PLL input */
+ SYSCTL_MAINCLKSRC_LFOSC, /*!< LF oscillator rate (11Axx only) */
+ SYSCTL_MAINCLKSRC_WDTOSC = SYSCTL_MAINCLKSRC_LFOSC, /*!< Watchdog oscillator rate */
+ SYSCTL_MAINCLKSRC_PLLOUT, /*!< System PLL output */
+} CHIP_SYSCTL_MAINCLKSRC_T;
+
+/**
+ * @brief Set main system clock source
+ * @param src : Clock source for main system
+ * @return Nothing
+ * @note This function will also toggle the clock source update register
+ * to update the clock source.
+ */
+void Chip_Clock_SetMainClockSource(CHIP_SYSCTL_MAINCLKSRC_T src);
+
+/**
+ * @brief Returns the main clock source
+ * @return Which clock is used for the core clock source?
+ */
+STATIC INLINE CHIP_SYSCTL_MAINCLKSRC_T Chip_Clock_GetMainClockSource(void)
+{
+ return (CHIP_SYSCTL_MAINCLKSRC_T) (LPC_SYSCTL->MAINCLKSEL);
+}
+
+/**
+ * @brief Set system clock divider
+ * @param div : divider for system clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The system clock
+ * rate is the main system clock divided by this value.
+ */
+STATIC INLINE void Chip_Clock_SetSysClockDiv(uint32_t div)
+{
+ LPC_SYSCTL->SYSAHBCLKDIV = div;
+}
+
+/**
+ * System and peripheral clocks
+ */
+typedef enum CHIP_SYSCTL_CLOCK {
+ SYSCTL_CLOCK_SYS = 0, /*!< 0: System clock */
+ SYSCTL_CLOCK_ROM, /*!<1: ROM clock */
+ SYSCTL_CLOCK_RAM, /*!< 2: RAM clock */
+ SYSCTL_CLOCK_FLASHREG, /*!< 3: FLASH register interface clock */
+ SYSCTL_CLOCK_FLASHARRAY, /*!< 4: FLASH array access clock */
+#if defined(CHIP_LPC110X)
+ SYSCTL_CLOCK_RESERVED5, /*!< 5: Reserved */
+#else
+ SYSCTL_CLOCK_I2C, /*!< 5: I2C clock, not on LPC110x */
+#endif
+ SYSCTL_CLOCK_GPIO, /*!< 6: GPIO clock */
+ SYSCTL_CLOCK_CT16B0, /*!< 7: 16-bit Counter/timer 0 clock */
+ SYSCTL_CLOCK_CT16B1, /*!< 8: 16-bit Counter/timer 1 clock */
+ SYSCTL_CLOCK_CT32B0, /*!< 9: 32-bit Counter/timer 0 clock */
+ SYSCTL_CLOCK_CT32B1, /*!< 10: 32-bit Counter/timer 1 clock */
+ SYSCTL_CLOCK_SSP0, /*!< 11: SSP0 clock */
+ SYSCTL_CLOCK_UART0, /*!< 12: UART0 clock */
+ SYSCTL_CLOCK_ADC, /*!< 13: ADC clock */
+#if defined(CHIP_LPC11UXX)
+ SYSCTL_CLOCK_USB, /*!< 14: USB clock, LPC11Uxx only */
+#else
+ SYSCTL_CLOCK_RESERVED14, /*!< 14: Reserved */
+#endif
+ SYSCTL_CLOCK_WDT, /*!< 15: Watchdog timer clock */
+ SYSCTL_CLOCK_IOCON, /*!< 16: IOCON block clock */
+#if defined(CHIP_LPC11CXX)
+ SYSCTL_CLOCK_CAN, /*!< 17: CAN clock, LPC11Cxx only */
+#else
+ SYSCTL_CLOCK_RESERVED17, /*!< 17: Reserved */
+#endif
+#if !(defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV))
+ SYSCTL_CLOCK_SSP1, /*!< 18: SSP1 clock, LPC11A/C/E/Uxx//1125 only */
+#if !defined(CHIP_LPC11CXX)
+ SYSCTL_CLOCK_PINT, /*!< 19: GPIO Pin int register interface clock, LPC11A/E/Uxx only */
+#if defined(CHIP_LPC11AXX)
+ SYSCTL_CLOCK_ACOMP, /*!< 20: Analog comparator clock, LPC11Axx only */
+ SYSCTL_CLOCK_DAC, /*!< 21: DAC clock, LPC11Axx only */
+#else
+ SYSCTL_CLOCK_RESERVED20, /*!< 20: Reserved */
+ SYSCTL_CLOCK_RESERVED21, /*!< 21: Reserved */
+#endif
+ SYSCTL_CLOCK_RESERVED22, /*!< 22: Reserved */
+ SYSCTL_CLOCK_P0INT, /*!< 23: GPIO GROUP1 interrupt register clock, LPC11Axx only */
+ SYSCTL_CLOCK_GROUP0INT = SYSCTL_CLOCK_P0INT,/*!< 23: GPIO GROUP0 interrupt register interface clock, LPC11E/Uxx only */
+ SYSCTL_CLOCK_P1INT, /*!< 24: GPIO GROUP1 interrupt register clock, LPC11Axx only */
+ SYSCTL_CLOCK_GROUP1INT = SYSCTL_CLOCK_P1INT,/*!< 24: GPIO GROUP1 interrupt register interface clock, LPC11E/Uxx only */
+ SYSCTL_CLOCK_RESERVED25, /*!< 25: Reserved */
+#if defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+ SYSCTL_CLOCK_RAM1, /*!< 26: SRAM block (0x20000000) clock, LPC11E/Uxx only */
+#else
+ SYSCTL_CLOCK_RESERVED26, /*!< 26: Reserved */
+#endif
+#if defined(CHIP_LPC11UXX)
+ SYSCTL_CLOCK_USBRAM, /*!< 27: USB SRAM block clock, LPC11Uxx only */
+#else
+ SYSCTL_CLOCK_RESERVED27, /*!< 27: Reserved */
+#endif
+#endif /* !defined(CHIP_LPC11CXX) */
+#endif /* !(defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV)) */
+} CHIP_SYSCTL_CLOCK_T;
+
+/**
+ * @brief Enable a system or peripheral clock
+ * @param clk : Clock to enable
+ * @return Nothing
+ */
+STATIC INLINE void Chip_Clock_EnablePeriphClock(CHIP_SYSCTL_CLOCK_T clk)
+{
+ LPC_SYSCTL->SYSAHBCLKCTRL |= (1 << clk);
+}
+
+/**
+ * @brief Disable a system or peripheral clock
+ * @param clk : Clock to disable
+ * @return Nothing
+ */
+STATIC INLINE void Chip_Clock_DisablePeriphClock(CHIP_SYSCTL_CLOCK_T clk)
+{
+ LPC_SYSCTL->SYSAHBCLKCTRL &= ~(1 << clk);
+}
+
+/**
+ * @brief Set SSP0 divider
+ * @param div : divider for SSP0 clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The SSP0 clock
+ * rate is the main system clock divided by this value.
+ */
+STATIC INLINE void Chip_Clock_SetSSP0ClockDiv(uint32_t div)
+{
+ LPC_SYSCTL->SSP0CLKDIV = div;
+}
+
+/**
+ * @brief Return SSP0 divider
+ * @return divider for SSP0 clock
+ * @note A value of 0 means the clock is disabled.
+ */
+STATIC INLINE uint32_t Chip_Clock_GetSSP0ClockDiv(void)
+{
+ return LPC_SYSCTL->SSP0CLKDIV;
+}
+
+/**
+ * @brief Set UART divider clock
+ * @param div : divider for UART clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The UART clock
+ * rate is the main system clock divided by this value.
+ */
+STATIC INLINE void Chip_Clock_SetUARTClockDiv(uint32_t div)
+{
+ LPC_SYSCTL->USARTCLKDIV = div;
+}
+
+/**
+ * @brief Return UART divider
+ * @return divider for UART clock
+ * @note A value of 0 means the clock is disabled.
+ */
+STATIC INLINE uint32_t Chip_Clock_GetUARTClockDiv(void)
+{
+ return LPC_SYSCTL->USARTCLKDIV;
+}
+
+#if defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC1125)
+/**
+ * @brief Set SSP1 divider clock
+ * @param div : divider for SSP1 clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The SSP1 clock
+ * rate is the main system clock divided by this value.
+ */
+STATIC INLINE void Chip_Clock_SetSSP1ClockDiv(uint32_t div)
+{
+ LPC_SYSCTL->SSP1CLKDIV = div;
+}
+
+/**
+ * @brief Return SSP1 divider
+ * @return divider for SSP1 clock
+ * @note A value of 0 means the clock is disabled.
+ */
+STATIC INLINE uint32_t Chip_Clock_GetSSP1ClockDiv(void)
+{
+ return LPC_SYSCTL->SSP1CLKDIV;
+}
+
+#endif /*defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX) || defined(CHIP_LPC11UXX)*/
+
+#if defined(CHIP_LPC11UXX)
+/**
+ * Clock sources for USB
+ */
+typedef enum CHIP_SYSCTL_USBCLKSRC {
+ SYSCTL_USBCLKSRC_PLLOUT = 0, /*!< USB PLL out */
+ SYSCTL_USBCLKSRC_MAINSYSCLK, /*!< Main system clock */
+} CHIP_SYSCTL_USBCLKSRC_T;
+
+/**
+ * @brief Set USB clock source and divider
+ * @param src : Clock source for USB
+ * @param div : divider for USB clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The USB clock
+ * rate is either the main system clock or USB PLL output clock divided
+ * by this value. This function will also toggle the clock source
+ * update register to update the clock source.
+ */
+void Chip_Clock_SetUSBClockSource(CHIP_SYSCTL_USBCLKSRC_T src, uint32_t div);
+
+#endif /*CHIP_LPC11UXX*/
+
+#if defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC1125)
+/**
+ * Clock sources for WDT
+ */
+typedef enum CHIP_SYSCTL_WDTCLKSRC {
+ SYSCTL_WDTCLKSRC_IRC = 0, /*!< Internal oscillator for watchdog clock */
+ SYSCTL_WDTCLKSRC_MAINSYSCLK, /*!< Main system clock for watchdog clock */
+ SYSCTL_WDTCLKSRC_WDTOSC, /*!< Watchdog oscillator for watchdog clock */
+} CHIP_SYSCTL_WDTCLKSRC_T;
+
+/**
+ * @brief Set WDT clock source and divider
+ * @param src : Clock source for WDT
+ * @param div : divider for WDT clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The WDT clock
+ * rate is the clock source divided by the divider. This function will
+ * also toggle the clock source update register to update the clock
+ * source.
+ */
+void Chip_Clock_SetWDTClockSource(CHIP_SYSCTL_WDTCLKSRC_T src, uint32_t div);
+
+#endif
+
+#if !defined(CHIP_LPC110X)
+/**
+ * Clock sources for CLKOUT
+ */
+typedef enum CHIP_SYSCTL_CLKOUTSRC {
+ SYSCTL_CLKOUTSRC_IRC = 0, /*!< Internal oscillator for CLKOUT */
+ SYSCTL_CLKOUTSRC_MAINOSC, /*!< Main oscillator for CLKOUT */
+ SYSCTL_CLKOUTSRC_WDTOSC, /*!< Watchdog oscillator for CLKOUT */
+ SYSCTL_CLKOUTSRC_LFOSC = SYSCTL_CLKOUTSRC_WDTOSC, /*!< LF oscillator rate (LPC11A/Exx only) for CLKOUT */
+ SYSCTL_CLKOUTSRC_MAINSYSCLK, /*!< Main system clock for CLKOUT */
+} CHIP_SYSCTL_CLKOUTSRC_T;
+
+/**
+ * @brief Set CLKOUT clock source and divider
+ * @param src : Clock source for CLKOUT
+ * @param div : divider for CLKOUT clock
+ * @return Nothing
+ * @note Use 0 to disable, or a divider value of 1 to 255. The CLKOUT clock
+ * rate is the clock source divided by the divider. This function will
+ * also toggle the clock source update register to update the clock
+ * source.
+ */
+void Chip_Clock_SetCLKOUTSource(CHIP_SYSCTL_CLKOUTSRC_T src, uint32_t div);
+
+#endif
+
+/**
+ * @brief Returns the main oscillator clock rate
+ * @return main oscillator clock rate
+ */
+STATIC INLINE uint32_t Chip_Clock_GetMainOscRate(void)
+{
+ return OscRateIn;
+}
+
+/**
+ * @brief Returns the internal oscillator (IRC) clock rate
+ * @return internal oscillator (IRC) clock rate
+ */
+STATIC INLINE uint32_t Chip_Clock_GetIntOscRate(void)
+{
+ return SYSCTL_IRC_FREQ;
+}
+
+#if defined(CHIP_LPC11AXX)
+/**
+ * @brief Returns the external clock input rate
+ * @return internal external clock input rate
+ * @note LPC11Axx devices only
+ */
+STATIC INLINE uint32_t Chip_Clock_GetExtClockInRate(void)
+{
+ return ExtRateIn;
+}
+
+#endif
+
+/**
+ * @brief Return estimated watchdog oscillator rate
+ * @return Estimated watchdog oscillator rate
+ * @note This rate is accurate to plus or minus 40%.
+ */
+uint32_t Chip_Clock_GetWDTOSCRate(void);
+
+#if defined(CHIP_LPC11AXX)
+/**
+ * @brief Return estimated low frequency oscillator rate
+ * @return Estimated low frequency oscillator rate
+ * @note This rate is accurate to plus or minus 40%.
+ */
+uint32_t Chip_Clock_GetLFOOSCRate(void);
+
+#endif
+
+/**
+ * @brief Return System PLL input clock rate
+ * @return System PLL input clock rate
+ */
+uint32_t Chip_Clock_GetSystemPLLInClockRate(void);
+
+/**
+ * @brief Return System PLL output clock rate
+ * @return System PLL output clock rate
+ */
+uint32_t Chip_Clock_GetSystemPLLOutClockRate(void);
+
+#if defined(CHIP_LPC11UXX)
+/**
+ * @brief Return USB PLL input clock rate
+ * @return USB PLL input clock rate
+ */
+uint32_t Chip_Clock_GetUSBPLLInClockRate(void);
+
+/**
+ * @brief Return USB PLL output clock rate
+ * @return USB PLL output clock rate
+ */
+uint32_t Chip_Clock_GetUSBPLLOutClockRate(void);
+
+#endif
+
+/**
+ * @brief Return main clock rate
+ * @return main clock rate
+ */
+uint32_t Chip_Clock_GetMainClockRate(void);
+
+/**
+ * @brief Return system clock rate
+ * @return system clock rate
+ */
+uint32_t Chip_Clock_GetSystemClockRate(void);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CLOCK_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/cmsis.h Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,64 @@ +/* + * @brief LPC11xx selective CMSIS inclusion file + * + * Copyright(C) NXP Semiconductors, 2012 + * All rights reserved. + * + * Software that is described herein is for illustrative purposes only + * which provides customers with programming information regarding the + * LPC products. This software is supplied "AS IS" without any warranties of + * any kind, and NXP Semiconductors and its licensor disclaim any and + * all warranties, express or implied, including all implied warranties of + * merchantability, fitness for a particular purpose and non-infringement of + * intellectual property rights. NXP Semiconductors assumes no responsibility + * or liability for the use of the software, conveys no license or rights under any + * patent, copyright, mask work right, or any other intellectual property rights in + * or to any products. NXP Semiconductors reserves the right to make changes + * in the software without notification. NXP Semiconductors also makes no + * representation or warranty that such application will be suitable for the + * specified use without further testing or modification. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation is hereby granted, under NXP Semiconductors' and its + * licensor's relevant copyrights in the software, without fee, provided that it + * is used in conjunction with NXP Semiconductors microcontrollers. This + * copyright, permission, and disclaimer notice must appear in all copies of + * this code. + */ + +#ifndef __CMSIS_H_ +#define __CMSIS_H_ + +#include "lpc_types.h" +#include "sys_config.h" + +/* Select correct CMSIS include file based on CHIP_* definition */ +#if defined(CHIP_LPC110X) +#include "cmsis_110x.h" +typedef LPC110X_IRQn_Type IRQn_Type; +#elif defined(CHIP_LPC1125) +#include "cmsis_1125.h" +typedef LPC1125_IRQn_Type IRQn_Type; +#elif defined(CHIP_LPC11AXX) +#include "cmsis_11axx.h" +typedef LPC11AXX_IRQn_Type IRQn_Type; +#elif defined(CHIP_LPC11CXX) +#include "cmsis_11cxx.h" +typedef LPC11CXX_IRQn_Type IRQn_Type; +#elif defined(CHIP_LPC11EXX) +#include "cmsis_11exx.h" +typedef LPC11EXX_IRQn_Type IRQn_Type; +#elif defined(CHIP_LPC11UXX) +#include "cmsis_11uxx.h" +typedef LPC11UXX_IRQn_Type IRQn_Type; +#elif defined(CHIP_LPC11XXLV) +#include "cmsis_11lvxx.h" +typedef LPC11XXLV_IRQn_Type IRQn_Type; +#else +#error "No CHIP_* definition is defined" +#endif + +/* Cortex-M0 processor and core peripherals */ +#include "core_cm0.h" + +#endif /* __CMSIS_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/cmsis_11cxx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,152 @@
+/*
+ * @brief Basic CMSIS include file for LPC11CXX
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2013
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __CMSIS_11CXX_H_
+#define __CMSIS_11CXX_H_
+
+#include "lpc_types.h"
+#include "sys_config.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup CMSIS_11CXX CHIP: LPC11Cxx CMSIS include file
+ * @ingroup CHIP_11XX_CMSIS_Drivers
+ * Applies to LPC1111, LPC1112, LPC1113, LPC1114, LPC11D14, LPC1115,
+ * LPC11C12, LPC11C13, LPC11C22, and LPC11C34 devices.
+ * @{
+ */
+
+#if defined(__ARMCC_VERSION)
+// Kill warning "#pragma push with no matching #pragma pop"
+ #pragma diag_suppress 2525
+ #pragma push
+ #pragma anon_unions
+#elif defined(__CWCC__)
+ #pragma push
+ #pragma cpp_extensions on
+#elif defined(__GNUC__)
+/* anonymous unions are enabled by default */
+#elif defined(__IAR_SYSTEMS_ICC__)
+// #pragma push // FIXME not usable for IAR
+ #pragma language=extended
+#else
+ #error Not supported compiler type
+#endif
+
+/*
+ * ==========================================================================
+ * ---------- Interrupt Number Definition -----------------------------------
+ * ==========================================================================
+ */
+
+#if !defined(CHIP_LPC11CXX)
+#error Incorrect or missing device variant (CHIP_LPC11AXX)
+#endif
+
+/** @defgroup CMSIS_11CXX_IRQ CHIP_LPC11CXX: LPC11CXX/LPC111X peripheral interrupt numbers
+ * @{
+ */
+
+typedef enum LPC11CXX_IRQn {
+ NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
+ HardFault_IRQn = -13, /*!< 3 Cortex-M0 Hard Fault Interrupt */
+ SVCall_IRQn = -5, /*!< 11 Cortex-M0 SV Call Interrupt */
+ PendSV_IRQn = -2, /*!< 14 Cortex-M0 Pend SV Interrupt */
+ SysTick_IRQn = -1, /*!< 15 Cortex-M0 System Tick Interrupt */
+
+ PIO0_0_IRQn = 0, /*!< GPIO port 0, pin 0 Interrupt */
+ PIO0_1_IRQn = 1, /*!< GPIO port 0, pin 1 Interrupt */
+ PIO0_2_IRQn = 2, /*!< GPIO port 0, pin 2 Interrupt */
+ PIO0_3_IRQn = 3, /*!< GPIO port 0, pin 3 Interrupt */
+ PIO0_4_IRQn = 4, /*!< GPIO port 0, pin 4 Interrupt */
+ PIO0_5_IRQn = 5, /*!< GPIO port 0, pin 5 Interrupt */
+ PIO0_6_IRQn = 6, /*!< GPIO port 0, pin 6 Interrupt */
+ PIO0_7_IRQn = 7, /*!< GPIO port 0, pin 7 Interrupt */
+ PIO0_8_IRQn = 8, /*!< GPIO port 0, pin 8 Interrupt */
+ PIO0_9_IRQn = 9, /*!< GPIO port 0, pin 9 Interrupt */
+ PIO0_10_IRQn = 10, /*!< GPIO port 0, pin 10 Interrupt */
+ PIO0_11_IRQn = 11, /*!< GPIO port 0, pin 11 Interrupt */
+ PIO1_0_IRQn = 12, /*!< GPIO port 1, pin 0 Interrupt */
+ CAN_IRQn = 13, /*!< CAN Interrupt */
+ SSP1_IRQn = 14, /*!< SSP1 Interrupt */
+ I2C0_IRQn = 15, /*!< I2C Interrupt */
+ TIMER_16_0_IRQn = 16, /*!< 16-bit Timer0 Interrupt */
+ TIMER_16_1_IRQn = 17, /*!< 16-bit Timer1 Interrupt */
+ TIMER_32_0_IRQn = 18, /*!< 32-bit Timer0 Interrupt */
+ TIMER_32_1_IRQn = 19, /*!< 32-bit Timer1 Interrupt */
+ SSP0_IRQn = 20, /*!< SSP0 Interrupt */
+ UART0_IRQn = 21, /*!< UART Interrupt */
+ Reserved22_IRQn = 22,
+ Reserved23_IRQn = 23,
+ ADC_IRQn = 24, /*!< A/D Converter Interrupt */
+ WDT_IRQn = 25, /*!< Watchdog timer Interrupt */
+ BOD_IRQn = 26, /*!< Brown Out Detect(BOD) Interrupt */
+ Reserved27_IRQn = 27,
+ EINT3_IRQn = 28, /*!< External Interrupt 3 Interrupt */
+ EINT2_IRQn = 29, /*!< External Interrupt 2 Interrupt */
+ EINT1_IRQn = 30, /*!< External Interrupt 1 Interrupt */
+ EINT0_IRQn = 31, /*!< External Interrupt 0 Interrupt */
+} LPC11CXX_IRQn_Type;
+
+/**
+ * @}
+ */
+
+/*
+ * ==========================================================================
+ * ----------- Processor and Core Peripheral Section ------------------------
+ * ==========================================================================
+ */
+
+/** @defgroup CMSIS_11CXX_COMMON CHIP: LPC11Cxx Cortex CMSIS definitions
+ * @{
+ */
+
+/* Configuration of the Cortex-M0 Processor and Core Peripherals */
+#define __MPU_PRESENT 0 /*!< MPU present or not */
+#define __NVIC_PRIO_BITS 2 /*!< Number of Bits used for Priority Levels */
+#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CMSIS_11CXX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/core_cm0.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,667 @@
+/**************************************************************************//**
+ * @file core_cm0.h
+ * @brief CMSIS Cortex-M0 Core Peripheral Access Layer Header File
+ * @version V3.01
+ * @date 13. March 2012
+ *
+ * @note
+ * Copyright (C) 2009-2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M
+ * processor based microcontrollers. This file can be freely distributed
+ * within development tools that are supporting such ARM based processors.
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+#if defined ( __ICCARM__ )
+ #pragma system_include /* treat file as system include file for MISRA check */
+#endif
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM0_H_GENERIC
+#define __CORE_CM0_H_GENERIC
+
+/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions
+ CMSIS violates the following MISRA-C:2004 rules:
+
+ \li Required Rule 8.5, object/function definition in header file.<br>
+ Function definitions in header files are used to allow 'inlining'.
+
+ \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+ Unions are used for effective representation of core registers.
+
+ \li Advisory Rule 19.7, Function-like macro defined.<br>
+ Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ * CMSIS definitions
+ ******************************************************************************/
+/** \ingroup Cortex_M0
+ @{
+ */
+
+/* CMSIS CM0 definitions */
+#define __CM0_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */
+#define __CM0_CMSIS_VERSION_SUB (0x01) /*!< [15:0] CMSIS HAL sub version */
+#define __CM0_CMSIS_VERSION ((__CM0_CMSIS_VERSION_MAIN << 16) | \
+ __CM0_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */
+
+#define __CORTEX_M (0x00) /*!< Cortex-M Core */
+
+
+#if defined ( __CC_ARM )
+ #define __ASM __asm /*!< asm keyword for ARM Compiler */
+ #define __INLINE __inline /*!< inline keyword for ARM Compiler */
+ #define __STATIC_INLINE static __inline
+
+#elif defined ( __ICCARM__ )
+ #define __ASM __asm /*!< asm keyword for IAR Compiler */
+ #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __GNUC__ )
+ #define __ASM __asm /*!< asm keyword for GNU Compiler */
+ #define __INLINE inline /*!< inline keyword for GNU Compiler */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __TASKING__ )
+ #define __ASM __asm /*!< asm keyword for TASKING Compiler */
+ #define __INLINE inline /*!< inline keyword for TASKING Compiler */
+ #define __STATIC_INLINE static inline
+
+#endif
+
+/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all
+*/
+#define __FPU_USED 0
+
+#if defined ( __CC_ARM )
+ #if defined __TARGET_FPU_VFP
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __ICCARM__ )
+ #if defined __ARMVFP__
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __GNUC__ )
+ #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __TASKING__ )
+ #if defined __FPU_VFP__
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+#endif
+
+#include <stdint.h> /* standard types definitions */
+#include <core_cmInstr.h> /* Core Instruction Access */
+#include <core_cmFunc.h> /* Core Function Access */
+
+#endif /* __CORE_CM0_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM0_H_DEPENDANT
+#define __CORE_CM0_H_DEPENDANT
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+ #ifndef __CM0_REV
+ #define __CM0_REV 0x0000
+ #warning "__CM0_REV not defined in device header file; using default!"
+ #endif
+
+ #ifndef __NVIC_PRIO_BITS
+ #define __NVIC_PRIO_BITS 2
+ #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+ #endif
+
+ #ifndef __Vendor_SysTickConfig
+ #define __Vendor_SysTickConfig 0
+ #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+ #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+ \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+ <strong>IO Type Qualifiers</strong> are used
+ \li to specify the access to peripheral variables.
+ \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+ #define __I volatile /*!< Defines 'read only' permissions */
+#else
+ #define __I volatile const /*!< Defines 'read only' permissions */
+#endif
+#define __O volatile /*!< Defines 'write only' permissions */
+#define __IO volatile /*!< Defines 'read / write' permissions */
+
+/*@} end of group Cortex_M0 */
+
+
+
+/*******************************************************************************
+ * Register Abstraction
+ Core Register contain:
+ - Core Register
+ - Core NVIC Register
+ - Core SCB Register
+ - Core SysTick Register
+ ******************************************************************************/
+/** \defgroup CMSIS_core_register Defines and Type Definitions
+ \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_CORE Status and Control Registers
+ \brief Core Register type definitions.
+ @{
+ */
+
+/** \brief Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+ struct
+ {
+#if (__CORTEX_M != 0x04)
+ uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */
+#else
+ uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */
+ uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
+ uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */
+#endif
+ uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} APSR_Type;
+
+
+/** \brief Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+ uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} IPSR_Type;
+
+
+/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+#if (__CORTEX_M != 0x04)
+ uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */
+#else
+ uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */
+ uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
+ uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */
+#endif
+ uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */
+ uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */
+ uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} xPSR_Type;
+
+
+/** \brief Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */
+ uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */
+ uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */
+ uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} CONTROL_Type;
+
+/*@} end of group CMSIS_CORE */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC)
+ \brief Type definitions for the NVIC Registers
+ @{
+ */
+
+/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+ __IO uint32_t ISER[1]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */
+ uint32_t RESERVED0[31];
+ __IO uint32_t ICER[1]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */
+ uint32_t RSERVED1[31];
+ __IO uint32_t ISPR[1]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */
+ uint32_t RESERVED2[31];
+ __IO uint32_t ICPR[1]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */
+ uint32_t RESERVED3[31];
+ uint32_t RESERVED4[64];
+ __IO uint32_t IP[8]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register */
+} NVIC_Type;
+
+/*@} end of group CMSIS_NVIC */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SCB System Control Block (SCB)
+ \brief Type definitions for the System Control Block Registers
+ @{
+ */
+
+/** \brief Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+ __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */
+ __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */
+ uint32_t RESERVED0;
+ __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */
+ __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */
+ __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */
+ uint32_t RESERVED1;
+ __IO uint32_t SHP[2]; /*!< Offset: 0x01C (R/W) System Handlers Priority Registers. [0] is RESERVED */
+ __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SysTick System Tick Timer (SysTick)
+ \brief Type definitions for the System Timer Registers.
+ @{
+ */
+
+/** \brief Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+ __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */
+ __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */
+ __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */
+ __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug)
+ \brief Cortex-M0 Core Debug Registers (DCB registers, SHCSR, and DFSR)
+ are only accessible over DAP and not via processor. Therefore
+ they are not covered by the Cortex-M0 header file.
+ @{
+ */
+/*@} end of group CMSIS_CoreDebug */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_core_base Core Definitions
+ \brief Definitions for base addresses, unions, and structures.
+ @{
+ */
+
+/* Memory mapping of Cortex-M0 Hardware */
+#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */
+#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */
+#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */
+#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */
+
+#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */
+#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */
+#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */
+
+
+/*@} */
+
+
+
+/*******************************************************************************
+ * Hardware Abstraction Layer
+ Core Function Interface contains:
+ - Core NVIC Functions
+ - Core SysTick Functions
+ - Core Register Access Functions
+ ******************************************************************************/
+/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ########################## NVIC functions #################################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+ \brief Functions that manage interrupts and exceptions via the NVIC.
+ @{
+ */
+
+/* Interrupt Priorities are WORD accessible only under ARMv6M */
+/* The following MACROS handle generation of the register offset and byte masks */
+#define _BIT_SHIFT(IRQn) ( (((uint32_t)(IRQn) ) & 0x03) * 8 )
+#define _SHP_IDX(IRQn) ( ((((uint32_t)(IRQn) & 0x0F)-8) >> 2) )
+#define _IP_IDX(IRQn) ( ((uint32_t)(IRQn) >> 2) )
+
+
+/** \brief Enable External Interrupt
+
+ The function enables a device-specific interrupt in the NVIC interrupt controller.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+ NVIC->ISER[0] = (1 << ((uint32_t)(IRQn) & 0x1F));
+}
+
+
+/** \brief Disable External Interrupt
+
+ The function disables a device-specific interrupt in the NVIC interrupt controller.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+ NVIC->ICER[0] = (1 << ((uint32_t)(IRQn) & 0x1F));
+}
+
+
+/** \brief Get Pending Interrupt
+
+ The function reads the pending register in the NVIC and returns the pending bit
+ for the specified interrupt.
+
+ \param [in] IRQn Interrupt number.
+
+ \return 0 Interrupt status is not pending.
+ \return 1 Interrupt status is pending.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+ return((uint32_t) ((NVIC->ISPR[0] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0));
+}
+
+
+/** \brief Set Pending Interrupt
+
+ The function sets the pending bit of an external interrupt.
+
+ \param [in] IRQn Interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+ NVIC->ISPR[0] = (1 << ((uint32_t)(IRQn) & 0x1F));
+}
+
+
+/** \brief Clear Pending Interrupt
+
+ The function clears the pending bit of an external interrupt.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+ NVIC->ICPR[0] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
+}
+
+
+/** \brief Set Interrupt Priority
+
+ The function sets the priority of an interrupt.
+
+ \note The priority cannot be set for every core interrupt.
+
+ \param [in] IRQn Interrupt number.
+ \param [in] priority Priority to set.
+ */
+__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+ if(IRQn < 0) {
+ SCB->SHP[_SHP_IDX(IRQn)] = (SCB->SHP[_SHP_IDX(IRQn)] & ~(0xFF << _BIT_SHIFT(IRQn))) |
+ (((priority << (8 - __NVIC_PRIO_BITS)) & 0xFF) << _BIT_SHIFT(IRQn)); }
+ else {
+ NVIC->IP[_IP_IDX(IRQn)] = (NVIC->IP[_IP_IDX(IRQn)] & ~(0xFF << _BIT_SHIFT(IRQn))) |
+ (((priority << (8 - __NVIC_PRIO_BITS)) & 0xFF) << _BIT_SHIFT(IRQn)); }
+}
+
+
+/** \brief Get Interrupt Priority
+
+ The function reads the priority of an interrupt. The interrupt
+ number can be positive to specify an external (device specific)
+ interrupt, or negative to specify an internal (core) interrupt.
+
+
+ \param [in] IRQn Interrupt number.
+ \return Interrupt Priority. Value is aligned automatically to the implemented
+ priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+ if(IRQn < 0) {
+ return((uint32_t)((SCB->SHP[_SHP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M0 system interrupts */
+ else {
+ return((uint32_t)((NVIC->IP[ _IP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */
+}
+
+
+/** \brief System Reset
+
+ The function initiates a system reset request to reset the MCU.
+ */
+__STATIC_INLINE void NVIC_SystemReset(void)
+{
+ __DSB(); /* Ensure all outstanding memory accesses included
+ buffered write are completed before reset */
+ SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+ SCB_AIRCR_SYSRESETREQ_Msk);
+ __DSB(); /* Ensure completion of memory access */
+ while(1); /* wait until reset */
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+
+/* ################################## SysTick function ############################################ */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+ \brief Functions that configure the System.
+ @{
+ */
+
+#if (__Vendor_SysTickConfig == 0)
+
+/** \brief System Tick Configuration
+
+ The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
+ Counter is in free running mode to generate periodic interrupts.
+
+ \param [in] ticks Number of ticks between two interrupts.
+
+ \return 0 Function succeeded.
+ \return 1 Function failed.
+
+ \note When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+ function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+ must contain a vendor-specific implementation of this function.
+
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+ if (ticks > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */
+
+ SysTick->LOAD = (ticks & SysTick_LOAD_RELOAD_Msk) - 1; /* set reload register */
+ NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */
+ SysTick->VAL = 0; /* Load the SysTick Counter Value */
+ SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
+ SysTick_CTRL_TICKINT_Msk |
+ SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
+ return (0); /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+
+#endif /* __CORE_CM0_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */
+
+#ifdef __cplusplus
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/core_cmFunc.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,616 @@
+/**************************************************************************//**
+ * @file core_cmFunc.h
+ * @brief CMSIS Cortex-M Core Function Access Header File
+ * @version V3.01
+ * @date 06. March 2012
+ *
+ * @note
+ * Copyright (C) 2009-2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M
+ * processor based microcontrollers. This file can be freely distributed
+ * within development tools that are supporting such ARM based processors.
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef __CORE_CMFUNC_H
+#define __CORE_CMFUNC_H
+
+
+/* ########################### Core Function Access ########################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+ @{
+ */
+
+#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+ #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* intrinsic void __enable_irq(); */
+/* intrinsic void __disable_irq(); */
+
+/** \brief Get Control Register
+
+ This function returns the content of the Control Register.
+
+ \return Control Register value
+ */
+__STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+ register uint32_t __regControl __ASM("control");
+ return(__regControl);
+}
+
+
+/** \brief Set Control Register
+
+ This function writes the given value to the Control Register.
+
+ \param [in] control Control Register value to set
+ */
+__STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+ register uint32_t __regControl __ASM("control");
+ __regControl = control;
+}
+
+
+/** \brief Get IPSR Register
+
+ This function returns the content of the IPSR Register.
+
+ \return IPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_IPSR(void)
+{
+ register uint32_t __regIPSR __ASM("ipsr");
+ return(__regIPSR);
+}
+
+
+/** \brief Get APSR Register
+
+ This function returns the content of the APSR Register.
+
+ \return APSR Register value
+ */
+__STATIC_INLINE uint32_t __get_APSR(void)
+{
+ register uint32_t __regAPSR __ASM("apsr");
+ return(__regAPSR);
+}
+
+
+/** \brief Get xPSR Register
+
+ This function returns the content of the xPSR Register.
+
+ \return xPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_xPSR(void)
+{
+ register uint32_t __regXPSR __ASM("xpsr");
+ return(__regXPSR);
+}
+
+
+/** \brief Get Process Stack Pointer
+
+ This function returns the current value of the Process Stack Pointer (PSP).
+
+ \return PSP Register value
+ */
+__STATIC_INLINE uint32_t __get_PSP(void)
+{
+ register uint32_t __regProcessStackPointer __ASM("psp");
+ return(__regProcessStackPointer);
+}
+
+
+/** \brief Set Process Stack Pointer
+
+ This function assigns the given value to the Process Stack Pointer (PSP).
+
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ register uint32_t __regProcessStackPointer __ASM("psp");
+ __regProcessStackPointer = topOfProcStack;
+}
+
+
+/** \brief Get Main Stack Pointer
+
+ This function returns the current value of the Main Stack Pointer (MSP).
+
+ \return MSP Register value
+ */
+__STATIC_INLINE uint32_t __get_MSP(void)
+{
+ register uint32_t __regMainStackPointer __ASM("msp");
+ return(__regMainStackPointer);
+}
+
+
+/** \brief Set Main Stack Pointer
+
+ This function assigns the given value to the Main Stack Pointer (MSP).
+
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ register uint32_t __regMainStackPointer __ASM("msp");
+ __regMainStackPointer = topOfMainStack;
+}
+
+
+/** \brief Get Priority Mask
+
+ This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+ \return Priority Mask value
+ */
+__STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+ register uint32_t __regPriMask __ASM("primask");
+ return(__regPriMask);
+}
+
+
+/** \brief Set Priority Mask
+
+ This function assigns the given value to the Priority Mask Register.
+
+ \param [in] priMask Priority Mask
+ */
+__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+ register uint32_t __regPriMask __ASM("primask");
+ __regPriMask = (priMask);
+}
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Enable FIQ
+
+ This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq __enable_fiq
+
+
+/** \brief Disable FIQ
+
+ This function disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq __disable_fiq
+
+
+/** \brief Get Base Priority
+
+ This function returns the current value of the Base Priority register.
+
+ \return Base Priority register value
+ */
+__STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+ register uint32_t __regBasePri __ASM("basepri");
+ return(__regBasePri);
+}
+
+
+/** \brief Set Base Priority
+
+ This function assigns the given value to the Base Priority register.
+
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
+{
+ register uint32_t __regBasePri __ASM("basepri");
+ __regBasePri = (basePri & 0xff);
+}
+
+
+/** \brief Get Fault Mask
+
+ This function returns the current value of the Fault Mask register.
+
+ \return Fault Mask register value
+ */
+__STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+ register uint32_t __regFaultMask __ASM("faultmask");
+ return(__regFaultMask);
+}
+
+
+/** \brief Set Fault Mask
+
+ This function assigns the given value to the Fault Mask register.
+
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ register uint32_t __regFaultMask __ASM("faultmask");
+ __regFaultMask = (faultMask & (uint32_t)1);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if (__CORTEX_M == 0x04)
+
+/** \brief Get FPSCR
+
+ This function returns the current value of the Floating Point Status/Control register.
+
+ \return Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ register uint32_t __regfpscr __ASM("fpscr");
+ return(__regfpscr);
+#else
+ return(0);
+#endif
+}
+
+
+/** \brief Set FPSCR
+
+ This function assigns the given value to the Floating Point Status/Control register.
+
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ register uint32_t __regfpscr __ASM("fpscr");
+ __regfpscr = (fpscr);
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/** \brief Enable IRQ Interrupts
+
+ This function enables IRQ interrupts by clearing the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
+{
+ __ASM volatile ("cpsie i");
+}
+
+
+/** \brief Disable IRQ Interrupts
+
+ This function disables IRQ interrupts by setting the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
+{
+ __ASM volatile ("cpsid i");
+}
+
+
+/** \brief Get Control Register
+
+ This function returns the content of the Control Register.
+
+ \return Control Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Control Register
+
+ This function writes the given value to the Control Register.
+
+ \param [in] control Control Register value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+ __ASM volatile ("MSR control, %0" : : "r" (control) );
+}
+
+
+/** \brief Get IPSR Register
+
+ This function returns the content of the IPSR Register.
+
+ \return IPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Get APSR Register
+
+ This function returns the content of the APSR Register.
+
+ \return APSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Get xPSR Register
+
+ This function returns the content of the xPSR Register.
+
+ \return xPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Get Process Stack Pointer
+
+ This function returns the current value of the Process Stack Pointer (PSP).
+
+ \return PSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
+{
+ register uint32_t result;
+
+ __ASM volatile ("MRS %0, psp\n" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Process Stack Pointer
+
+ This function assigns the given value to the Process Stack Pointer (PSP).
+
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) );
+}
+
+
+/** \brief Get Main Stack Pointer
+
+ This function returns the current value of the Main Stack Pointer (MSP).
+
+ \return MSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
+{
+ register uint32_t result;
+
+ __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Main Stack Pointer
+
+ This function assigns the given value to the Main Stack Pointer (MSP).
+
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) );
+}
+
+
+/** \brief Get Priority Mask
+
+ This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+ \return Priority Mask value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Priority Mask
+
+ This function assigns the given value to the Priority Mask Register.
+
+ \param [in] priMask Priority Mask
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask, %0" : : "r" (priMask) );
+}
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Enable FIQ
+
+ This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
+{
+ __ASM volatile ("cpsie f");
+}
+
+
+/** \brief Disable FIQ
+
+ This function disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
+{
+ __ASM volatile ("cpsid f");
+}
+
+
+/** \brief Get Base Priority
+
+ This function returns the current value of the Base Priority register.
+
+ \return Base Priority register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Base Priority
+
+ This function assigns the given value to the Base Priority register.
+
+ \param [in] basePri Base Priority value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
+{
+ __ASM volatile ("MSR basepri, %0" : : "r" (value) );
+}
+
+
+/** \brief Get Fault Mask
+
+ This function returns the current value of the Fault Mask register.
+
+ \return Fault Mask register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Fault Mask
+
+ This function assigns the given value to the Fault Mask register.
+
+ \param [in] faultMask Fault Mask value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) );
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if (__CORTEX_M == 0x04)
+
+/** \brief Get FPSCR
+
+ This function returns the current value of the Floating Point Status/Control register.
+
+ \return Floating Point Status/Control register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ uint32_t result;
+
+ __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
+ return(result);
+#else
+ return(0);
+#endif
+}
+
+
+/** \brief Set FPSCR
+
+ This function assigns the given value to the Floating Point Status/Control register.
+
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) );
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all instrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+#endif /* __CORE_CMFUNC_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/core_cmInstr.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,618 @@
+/**************************************************************************//**
+ * @file core_cmInstr.h
+ * @brief CMSIS Cortex-M Core Instruction Access Header File
+ * @version V3.01
+ * @date 06. March 2012
+ *
+ * @note
+ * Copyright (C) 2009-2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M
+ * processor based microcontrollers. This file can be freely distributed
+ * within development tools that are supporting such ARM based processors.
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef __CORE_CMINSTR_H
+#define __CORE_CMINSTR_H
+
+
+/* ########################## Core Instruction Access ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+ Access to dedicated instructions
+ @{
+*/
+
+#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+ #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+
+/** \brief No Operation
+
+ No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP __nop
+
+
+/** \brief Wait For Interrupt
+
+ Wait For Interrupt is a hint instruction that suspends execution
+ until one of a number of events occurs.
+ */
+#define __WFI __wfi
+
+
+/** \brief Wait For Event
+
+ Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+#define __WFE __wfe
+
+
+/** \brief Send Event
+
+ Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV __sev
+
+
+/** \brief Instruction Synchronization Barrier
+
+ Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or
+ memory, after the instruction has been completed.
+ */
+#define __ISB() __isb(0xF)
+
+
+/** \brief Data Synchronization Barrier
+
+ This function acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB() __dsb(0xF)
+
+
+/** \brief Data Memory Barrier
+
+ This function ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+#define __DMB() __dmb(0xF)
+
+
+/** \brief Reverse byte order (32 bit)
+
+ This function reverses the byte order in integer value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV __rev
+
+
+/** \brief Reverse byte order (16 bit)
+
+ This function reverses the byte order in two unsigned short values.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+ rev16 r0, r0
+ bx lr
+}
+
+
+/** \brief Reverse byte order in signed short value
+
+ This function reverses the byte order in a signed short value with sign extension to integer.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)
+{
+ revsh r0, r0
+ bx lr
+}
+
+
+/** \brief Rotate Right in unsigned value (32 bit)
+
+ This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+ \param [in] value Value to rotate
+ \param [in] value Number of Bits to rotate
+ \return Rotated value
+ */
+#define __ROR __ror
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Reverse bit order of value
+
+ This function reverses the bit order of the given value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __RBIT __rbit
+
+
+/** \brief LDR Exclusive (8 bit)
+
+ This function performs a exclusive LDR command for 8 bit value.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr))
+
+
+/** \brief LDR Exclusive (16 bit)
+
+ This function performs a exclusive LDR command for 16 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDREXH(ptr) ((uint16_t) __ldrex(ptr))
+
+
+/** \brief LDR Exclusive (32 bit)
+
+ This function performs a exclusive LDR command for 32 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr))
+
+
+/** \brief STR Exclusive (8 bit)
+
+ This function performs a exclusive STR command for 8 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXB(value, ptr) __strex(value, ptr)
+
+
+/** \brief STR Exclusive (16 bit)
+
+ This function performs a exclusive STR command for 16 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXH(value, ptr) __strex(value, ptr)
+
+
+/** \brief STR Exclusive (32 bit)
+
+ This function performs a exclusive STR command for 32 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXW(value, ptr) __strex(value, ptr)
+
+
+/** \brief Remove the exclusive lock
+
+ This function removes the exclusive lock which is created by LDREX.
+
+ */
+#define __CLREX __clrex
+
+
+/** \brief Signed Saturate
+
+ This function saturates a signed value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT __ssat
+
+
+/** \brief Unsigned Saturate
+
+ This function saturates an unsigned value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT __usat
+
+
+/** \brief Count leading zeros
+
+ This function counts the number of leading zeros of a data value.
+
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+#define __CLZ __clz
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/** \brief No Operation
+
+ No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
+{
+ __ASM volatile ("nop");
+}
+
+
+/** \brief Wait For Interrupt
+
+ Wait For Interrupt is a hint instruction that suspends execution
+ until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
+{
+ __ASM volatile ("wfi");
+}
+
+
+/** \brief Wait For Event
+
+ Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
+{
+ __ASM volatile ("wfe");
+}
+
+
+/** \brief Send Event
+
+ Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
+{
+ __ASM volatile ("sev");
+}
+
+
+/** \brief Instruction Synchronization Barrier
+
+ Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or
+ memory, after the instruction has been completed.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
+{
+ __ASM volatile ("isb");
+}
+
+
+/** \brief Data Synchronization Barrier
+
+ This function acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
+{
+ __ASM volatile ("dsb");
+}
+
+
+/** \brief Data Memory Barrier
+
+ This function ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
+{
+ __ASM volatile ("dmb");
+}
+
+
+/** \brief Reverse byte order (32 bit)
+
+ This function reverses the byte order in integer value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rev %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief Reverse byte order (16 bit)
+
+ This function reverses the byte order in two unsigned short values.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rev16 %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief Reverse byte order in signed short value
+
+ This function reverses the byte order in a signed short value with sign extension to integer.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("revsh %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief Rotate Right in unsigned value (32 bit)
+
+ This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+ \param [in] value Value to rotate
+ \param [in] value Number of Bits to rotate
+ \return Rotated value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+
+ __ASM volatile ("ror %0, %0, %1" : "+r" (op1) : "r" (op2) );
+ return(op1);
+}
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Reverse bit order of value
+
+ This function reverses the bit order of the given value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief LDR Exclusive (8 bit)
+
+ This function performs a exclusive LDR command for 8 bit value.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)
+{
+ uint8_t result;
+
+ __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) );
+ return(result);
+}
+
+
+/** \brief LDR Exclusive (16 bit)
+
+ This function performs a exclusive LDR command for 16 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)
+{
+ uint16_t result;
+
+ __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) );
+ return(result);
+}
+
+
+/** \brief LDR Exclusive (32 bit)
+
+ This function performs a exclusive LDR command for 32 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrex %0, [%1]" : "=r" (result) : "r" (addr) );
+ return(result);
+}
+
+
+/** \brief STR Exclusive (8 bit)
+
+ This function performs a exclusive STR command for 8 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strexb %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
+ return(result);
+}
+
+
+/** \brief STR Exclusive (16 bit)
+
+ This function performs a exclusive STR command for 16 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strexh %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
+ return(result);
+}
+
+
+/** \brief STR Exclusive (32 bit)
+
+ This function performs a exclusive STR command for 32 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strex %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
+ return(result);
+}
+
+
+/** \brief Remove the exclusive lock
+
+ This function removes the exclusive lock which is created by LDREX.
+
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
+{
+ __ASM volatile ("clrex");
+}
+
+
+/** \brief Signed Saturate
+
+ This function saturates a signed value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+
+/** \brief Unsigned Saturate
+
+ This function saturates an unsigned value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+
+/** \brief Count leading zeros
+
+ This function counts the number of leading zeros of a data value.
+
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
+{
+ uint8_t result;
+
+ __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all intrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+#endif /* __CORE_CMINSTR_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/error.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,134 @@
+/*
+ * @brief Error code returned by Boot ROM drivers/library functions
+ * @ingroup Common
+ *
+ * This file contains unified error codes to be used across driver,
+ * middleware, applications, hal and demo software.
+ *
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __LPC_ERROR_H__
+#define __LPC_ERROR_H__
+
+/** Error code returned by Boot ROM drivers/library functions
+*
+* Error codes are a 32-bit value with :
+* - The 16 MSB contains the peripheral code number
+* - The 16 LSB contains an error code number associated to that peripheral
+*
+*/
+typedef enum
+{
+ /**\b 0x00000000*/ LPC_OK=0, /**< enum value returned on Success */
+ /**\b 0xFFFFFFFF*/ ERR_FAILED = -1, /**< enum value returned on general failure */
+ /**\b 0xFFFFFFFE*/ ERR_TIME_OUT = -2, /**< enum value returned on general timeout */
+ /**\b 0xFFFFFFFD*/ ERR_BUSY = -3, /**< enum value returned when resource is busy */
+
+ /* ISP related errors */
+ ERR_ISP_BASE = 0x00000000,
+ /*0x00000001*/ ERR_ISP_INVALID_COMMAND = ERR_ISP_BASE + 1,
+ /*0x00000002*/ ERR_ISP_SRC_ADDR_ERROR, /* Source address not on word boundary */
+ /*0x00000003*/ ERR_ISP_DST_ADDR_ERROR, /* Destination address not on word or 256 byte boundary */
+ /*0x00000004*/ ERR_ISP_SRC_ADDR_NOT_MAPPED,
+ /*0x00000005*/ ERR_ISP_DST_ADDR_NOT_MAPPED,
+ /*0x00000006*/ ERR_ISP_COUNT_ERROR, /* Byte count is not multiple of 4 or is not a permitted value */
+ /*0x00000007*/ ERR_ISP_INVALID_SECTOR,
+ /*0x00000008*/ ERR_ISP_SECTOR_NOT_BLANK,
+ /*0x00000009*/ ERR_ISP_SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION,
+ /*0x0000000A*/ ERR_ISP_COMPARE_ERROR,
+ /*0x0000000B*/ ERR_ISP_BUSY, /* Flash programming hardware interface is busy */
+ /*0x0000000C*/ ERR_ISP_PARAM_ERROR, /* Insufficient number of parameters */
+ /*0x0000000D*/ ERR_ISP_ADDR_ERROR, /* Address not on word boundary */
+ /*0x0000000E*/ ERR_ISP_ADDR_NOT_MAPPED,
+ /*0x0000000F*/ ERR_ISP_CMD_LOCKED, /* Command is locked */
+ /*0x00000010*/ ERR_ISP_INVALID_CODE, /* Unlock code is invalid */
+ /*0x00000011*/ ERR_ISP_INVALID_BAUD_RATE,
+ /*0x00000012*/ ERR_ISP_INVALID_STOP_BIT,
+ /*0x00000013*/ ERR_ISP_CODE_READ_PROTECTION_ENABLED,
+
+ /* ROM API related errors */
+ ERR_API_BASE = 0x00010000,
+ /**\b 0x00010001*/ ERR_API_INVALID_PARAMS = ERR_API_BASE + 1, /**< Invalid parameters*/
+ /**\b 0x00010002*/ ERR_API_INVALID_PARAM1, /**< PARAM1 is invalid */
+ /**\b 0x00010003*/ ERR_API_INVALID_PARAM2, /**< PARAM2 is invalid */
+ /**\b 0x00010004*/ ERR_API_INVALID_PARAM3, /**< PARAM3 is invalid */
+ /**\b 0x00010005*/ ERR_API_MOD_INIT, /**< API is called before module init */
+
+ /* SPIFI API related errors */
+ ERR_SPIFI_BASE = 0x00020000,
+ /*0x00020001*/ ERR_SPIFI_DEVICE_ERROR =ERR_SPIFI_BASE+1,
+ /*0x00020002*/ ERR_SPIFI_INTERNAL_ERROR,
+ /*0x00020003*/ ERR_SPIFI_TIMEOUT,
+ /*0x00020004*/ ERR_SPIFI_OPERAND_ERROR,
+ /*0x00020005*/ ERR_SPIFI_STATUS_PROBLEM,
+ /*0x00020006*/ ERR_SPIFI_UNKNOWN_EXT,
+ /*0x00020007*/ ERR_SPIFI_UNKNOWN_ID,
+ /*0x00020008*/ ERR_SPIFI_UNKNOWN_TYPE,
+ /*0x00020009*/ ERR_SPIFI_UNKNOWN_MFG,
+
+ /* Security API related errors */
+ ERR_SEC_BASE = 0x00030000,
+ /*0x00030001*/ ERR_SEC_AES_WRONG_CMD=ERR_SEC_BASE+1,
+ /*0x00030002*/ ERR_SEC_AES_NOT_SUPPORTED,
+ /*0x00030003*/ ERR_SEC_AES_KEY_ALREADY_PROGRAMMED,
+
+
+ /* USB device stack related errors */
+ ERR_USBD_BASE = 0x00040000,
+ /**\b 0x00040001*/ ERR_USBD_INVALID_REQ = ERR_USBD_BASE + 1, /**< invalid request */
+ /**\b 0x00040002*/ ERR_USBD_UNHANDLED, /**< Callback did not process the event */
+ /**\b 0x00040003*/ ERR_USBD_STALL, /**< Stall the endpoint on which the call back is called */
+ /**\b 0x00040004*/ ERR_USBD_SEND_ZLP, /**< Send ZLP packet on the endpoint on which the call back is called */
+ /**\b 0x00040005*/ ERR_USBD_SEND_DATA, /**< Send data packet on the endpoint on which the call back is called */
+ /**\b 0x00040006*/ ERR_USBD_BAD_DESC, /**< Bad descriptor*/
+ /**\b 0x00040007*/ ERR_USBD_BAD_CFG_DESC,/**< Bad config descriptor*/
+ /**\b 0x00040008*/ ERR_USBD_BAD_INTF_DESC,/**< Bad interface descriptor*/
+ /**\b 0x00040009*/ ERR_USBD_BAD_EP_DESC,/**< Bad endpoint descriptor*/
+ /**\b 0x0004000a*/ ERR_USBD_BAD_MEM_BUF, /**< Bad alignment of buffer passed. */
+ /**\b 0x0004000b*/ ERR_USBD_TOO_MANY_CLASS_HDLR, /**< Too many class handlers. */
+
+ /* CGU related errors */
+ ERR_CGU_BASE = 0x00050000,
+ /*0x00050001*/ ERR_CGU_NOT_IMPL=ERR_CGU_BASE+1,
+ /*0x00050002*/ ERR_CGU_INVALID_PARAM,
+ /*0x00050003*/ ERR_CGU_INVALID_SLICE,
+ /*0x00050004*/ ERR_CGU_OUTPUT_GEN,
+ /*0x00050005*/ ERR_CGU_DIV_SRC,
+ /*0x00050006*/ ERR_CGU_DIV_VAL,
+ /*0x00050007*/ ERR_CGU_SRC
+
+} ErrorCode_t;
+
+
+
+//#define offsetof(s,m) (int)&(((s *)0)->m)
+#define COMPILE_TIME_ASSERT(pred) switch(0){case 0:case pred:;}
+
+#endif /* __LPC_ERROR_H__ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/fmc_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,101 @@
+/*
+ * @brief FLASH Memory Controller (FMC) registers and control functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __FMC_11XX_H_
+#define __FMC_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup FMC_11XX CHIP: LPC11xx FLASH Memory Controller driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief FLASH Memory Controller Unit register block structure
+ */
+typedef struct {/*!< FMC Structure */
+ __I uint32_t RESERVED1[4];
+ __IO uint32_t FLASHTIM;
+ __I uint32_t RESERVED2[3];
+ __IO uint32_t FMSSTART;
+ __IO uint32_t FMSSTOP;
+ __I uint32_t RESERVED3;
+ __I uint32_t FMSW[4];
+ __I uint32_t RESERVED4[25];
+#if defined(CHIP_LPC1125)
+ __I uint32_t RESERVED5[977];
+#else
+ __IO uint32_t EEMSSTART;
+ __IO uint32_t EEMSSTOP;
+ __I uint32_t EEMSSIG;
+ __I uint32_t RESERVED5[974];
+#endif
+ __I uint32_t FMSTAT;
+ __I uint32_t RESERVED6;
+ __O uint32_t FMSTATCLR;
+} LPC_FMC_T;
+
+/**
+ * @brief FLASH Access time definitions
+ */
+typedef enum {
+ FLASHTIM_20MHZ_CPU = 0, /*!< Flash accesses use 1 CPU clocks. Use for up to 20 MHz CPU clock*/
+ FLASHTIM_40MHZ_CPU = 1, /*!< Flash accesses use 2 CPU clocks. Use for up to 40 MHz CPU clock*/
+ FLASHTIM_50MHZ_CPU = 2, /*!< Flash accesses use 3 CPU clocks. Use for up to 50 MHz CPU clock*/
+} FMC_FLASHTIM_T;
+
+/**
+ * @brief Set FLASH access time in clocks
+ * @param clks : Clock cycles for FLASH access (minus 1)
+ * @return Nothing
+ * @note For CPU speed up to 20MHz, use a value of 0. For up to 40MHz, use
+ * a value of 1. For up to 50MHz, use a value of 2.
+ */
+STATIC INLINE void Chip_FMC_SetFLASHAccess(FMC_FLASHTIM_T clks)
+{
+ uint32_t tmp = LPC_FMC->FLASHTIM & (~(0x3));
+
+ /* Don't alter upper bits */
+ LPC_FMC->FLASHTIM = tmp | clks;
+}
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __FMC_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/gpio_11xx_2.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,642 @@
+/*
+ * @brief LPC11xx GPIO driver for CHIP_LPC11CXX, CHIP_LPC110X, CHIP_LPC11XXLV,
+ * and CHIP_LPC1125 families only.
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2013
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __GPIO_11XX_2_H_
+#define __GPIO_11XX_2_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup GPIO_11XX_2 CHIP: LPC11xx GPIO driver for CHIP_LPC11CXX, CHIP_LPC110X, and CHIP_LPC11XXLV families
+ * @ingroup CHIP_11XX_Drivers
+ * For device familes identified with CHIP definitions CHIP_LPC11CXX,
+ * CHIP_LPC110X, and CHIP_LPC11XXLV only.
+ * @{
+ */
+
+#if defined(CHIP_LPC11CXX) || defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC1125)
+
+/**
+ * @brief GPIO port register block structure
+ */
+typedef struct { /*!< GPIO_PORT Structure */
+ __IO uint32_t DATA[4096]; /*!< Offset: 0x0000 to 0x3FFC Data address masking register (R/W) */
+ uint32_t RESERVED1[4096];
+ __IO uint32_t DIR; /*!< Offset: 0x8000 Data direction register (R/W) */
+ __IO uint32_t IS; /*!< Offset: 0x8004 Interrupt sense register (R/W) */
+ __IO uint32_t IBE; /*!< Offset: 0x8008 Interrupt both edges register (R/W) */
+ __IO uint32_t IEV; /*!< Offset: 0x800C Interrupt event register (R/W) */
+ __IO uint32_t IE; /*!< Offset: 0x8010 Interrupt mask register (R/W) */
+ __I uint32_t RIS; /*!< Offset: 0x8014 Raw interrupt status register (R/ ) */
+ __I uint32_t MIS; /*!< Offset: 0x8018 Masked interrupt status register (R/ ) */
+ __O uint32_t IC; /*!< Offset: 0x801C Interrupt clear register (W) */
+ uint32_t RESERVED2[8184]; /* Padding added for aligning contiguous GPIO blocks */
+} LPC_GPIO_T;
+
+/**
+ * @brief Initialize GPIO block
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @return Nothing
+ */
+void Chip_GPIO_Init(LPC_GPIO_T *pGPIO);
+
+/**
+ * @brief De-Initialize GPIO block
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @return Nothing
+ */
+void Chip_GPIO_DeInit(LPC_GPIO_T *pGPIO);
+
+/**
+ * @brief Set a GPIO port/bit state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : GPIO port to set
+ * @param bit : GPIO bit to set
+ * @param setting : true for high, false for low
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_WritePortBit(LPC_GPIO_T *pGPIO, uint32_t port, uint8_t bit, bool setting)
+{
+ pGPIO[port].DATA[1 << bit] = setting << bit;
+}
+
+/**
+ * @brief Set a GPIO pin state via the GPIO byte register
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : GPIO pin to set
+ * @param setting : true for high, false for low
+ * @return Nothing
+ * @note This function replaces Chip_GPIO_WritePortBit()
+ */
+STATIC INLINE void Chip_GPIO_SetPinState(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin, bool setting)
+{
+ pGPIO[port].DATA[1 << pin] = setting << pin;
+}
+
+/**
+ * @brief Read a GPIO state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : GPIO port to read
+ * @param bit : GPIO bit to read
+ * @return true of the GPIO is high, false if low
+ * @note It is recommended to use the Chip_GPIO_GetPinState() function instead.
+ */
+STATIC INLINE bool Chip_GPIO_ReadPortBit(LPC_GPIO_T *pGPIO, uint32_t port, uint8_t bit)
+{
+ return (bool) ((pGPIO[port].DATA[1 << bit] >> bit) & 1);
+}
+
+/**
+ * @brief Get a GPIO pin state via the GPIO byte register
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : GPIO pin to get state for
+ * @return true if the GPIO is high, false if low
+ * @note This function replaces Chip_GPIO_ReadPortBit()
+ */
+STATIC INLINE bool Chip_GPIO_GetPinState(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ return (pGPIO[port].DATA[1 << pin]) != 0;
+}
+
+/**
+ * @brief Seta GPIO direction
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : GPIO port to set
+ * @param bit : GPIO bit to set
+ * @param setting : true for output, false for input
+ * @return Nothing
+ * @note It is recommended to use the Chip_GPIO_SetPinDIROutput(),
+ * Chip_GPIO_SetPinDIRInput() or Chip_GPIO_SetPinDIR() functions instead
+ * of this function.
+ */
+void Chip_GPIO_WriteDirBit(LPC_GPIO_T *pGPIO, uint32_t port, uint8_t bit, bool setting);
+
+/**
+ * @brief Set GPIO direction for a single GPIO pin to an output
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : GPIO pin to set direction on as output
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetPinDIROutput(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ pGPIO[port].DIR |= (1UL << pin);
+}
+
+/**
+ * @brief Set GPIO direction for a single GPIO pin to an input
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : GPIO pin to set direction on as input
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetPinDIRInput(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ pGPIO[port].DIR &= ~(1UL << pin);
+}
+
+/**
+ * @brief Set GPIO direction for a single GPIO pin
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : GPIO pin to set direction for
+ * @param output : true for output, false for input
+ * @return Nothing
+ */
+void Chip_GPIO_SetPinDIR(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin, bool output);
+
+/**
+ * @brief Read a GPIO direction (out or in)
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : GPIO port to read
+ * @param bit : GPIO bit to read
+ * @return true of the GPIO is an output, false if input
+ * @note It is recommended to use the Chip_GPIO_GetPinDIR() function instead.
+ */
+STATIC INLINE bool Chip_GPIO_ReadDirBit(LPC_GPIO_T *pGPIO, uint32_t port, uint8_t bit)
+{
+ return (bool) (((pGPIO[port].DIR) >> bit) & 1);
+}
+
+/**
+ * @brief Get GPIO direction for a single GPIO pin
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : GPIO pin to get direction for
+ * @return true if the GPIO is an output, false if input
+ */
+STATIC INLINE bool Chip_GPIO_GetPinDIR(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ return (bool) (pGPIO[port].DIR >> pin) & 1;
+}
+
+/**
+ * @brief Set Direction for a GPIO port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port Number
+ * @param bit : GPIO bit to set
+ * @param out : Direction value, 0 = input, !0 = output
+ * @return None
+ * @note Bits set to '0' are not altered. It is recommended to use the
+ * Chip_GPIO_SetPortDIR() function instead.
+ */
+void Chip_GPIO_SetDir(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t bit, uint8_t out);
+
+/**
+ * @brief Set GPIO direction for a all selected GPIO pins to an output
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinMask : GPIO pin mask to set direction on as output (bits 0..n for pins 0..n)
+ * @return Nothing
+ * @note Sets multiple GPIO pins to the output direction, each bit's position that is
+ * high sets the corresponding pin number for that bit to an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPortDIROutput(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinMask)
+{
+ pGPIO[port].DIR |= pinMask;
+}
+
+/**
+ * @brief Set GPIO direction for a all selected GPIO pins to an input
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinMask : GPIO pin mask to set direction on as input (bits 0..b for pins 0..n)
+ * @return Nothing
+ * @note Sets multiple GPIO pins to the input direction, each bit's position that is
+ * high sets the corresponding pin number for that bit to an input.
+ */
+STATIC INLINE void Chip_GPIO_SetPortDIRInput(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinMask)
+{
+ pGPIO[port].DIR &= ~pinMask;
+}
+
+/**
+ * @brief Set GPIO direction for a all selected GPIO pins to an input or output
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinMask : GPIO pin mask to set direction on (bits 0..b for pins 0..n)
+ * @param outSet : Direction value, false = set as inputs, true = set as outputs
+ * @return Nothing
+ * @note Sets multiple GPIO pins to the input direction, each bit's position that is
+ * high sets the corresponding pin number for that bit to an input.
+ */
+void Chip_GPIO_SetPortDIR(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinMask, bool outSet);
+
+/**
+ * @brief Get GPIO direction for a all GPIO pins
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return a bitfield containing the input and output states for each pin
+ * @note For pins 0..n, a high state in a bit corresponds to an output state for the
+ * same pin, while a low state corresponds to an input state.
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetPortDIR(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].DIR;
+}
+
+/**
+ * @brief Set all GPIO raw pin states (regardless of masking)
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param value : Value to set all GPIO pin states (0..n) to
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetPortValue(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t value)
+{
+ pGPIO[port].DATA[0xFFF] = value;
+}
+
+/**
+ * @brief Get all GPIO raw pin states (regardless of masking)
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return Current (raw) state of all GPIO pins
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetPortValue(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].DATA[0xFFF];
+}
+
+/**
+ * @brief Set a GPIO port/bit to the high state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param bit : Bit(s) in the port to set high
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output. It is recommended to use the
+ * Chip_GPIO_SetPortOutHigh() function instead.
+ */
+STATIC INLINE void Chip_GPIO_SetValue(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t bit)
+{
+ pGPIO[port].DATA[bit] = bit;
+}
+
+/**
+ * @brief Set selected GPIO output pins to the high state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pins : pins (0..n) to set high
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPortOutHigh(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pins)
+{
+ pGPIO[port].DATA[pins] = 0xFFF;
+}
+
+/**
+ * @brief Set an individual GPIO output pin to the high state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : pin number (0..n) to set high
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPinOutHigh(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ pGPIO[port].DATA[1 << pin] = (1 << pin);
+}
+
+/**
+ * @brief Set a GPIO port/bit to the low state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param bit : Bit(s) in the port to set low
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_ClearValue(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t bit)
+{
+ pGPIO[port].DATA[bit] = ~bit;
+}
+
+/**
+ * @brief Set selected GPIO output pins to the low state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pins : pins (0..n) to set low
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPortOutLow(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pins)
+{
+ pGPIO[port].DATA[pins] = 0;
+}
+
+/**
+ * @brief Set an individual GPIO output pin to the low state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : pin number (0..n) to set low
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPinOutLow(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ pGPIO[port].DATA[1 << pin] = 0;
+}
+
+/**
+ * @brief Toggle selected GPIO output pins to the opposite state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pins : pins (0..n) to toggle
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPortToggle(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pins)
+{
+ pGPIO[port].DATA[pins] ^= 0xFFF;
+}
+
+/**
+ * @brief Toggle an individual GPIO output pin to the opposite state
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : pin number (0..n) to toggle
+ * @return None
+ * @note Any bit set as a '0' will not have it's state changed. This only
+ * applies to ports configured as an output.
+ */
+STATIC INLINE void Chip_GPIO_SetPinToggle(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin)
+{
+ pGPIO[port].DATA[1 << pin] ^= (1 << pin);
+}
+
+/**
+ * @brief Read current bit states for the selected port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number to read
+ * @return Current value of GPIO port
+ * @note The current states of the bits for the port are read, regardless of
+ * whether the GPIO port bits are input or output.
+ */
+STATIC INLINE uint32_t Chip_GPIO_ReadValue(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].DATA[4095];
+}
+
+/**
+ * @brief Configure the pins as edge sensitive for interrupts
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to set to edge mode (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetPinModeEdge(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IS &= ~pinmask;
+}
+
+/**
+ * @brief Configure the pins as level sensitive for interrupts
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to set to level mode (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetPinModeLevel(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IS |= pinmask;
+}
+
+/**
+ * @brief Returns current GPIO edge or high level interrupt configuration for all pins for a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return A bifield containing the edge/level interrupt configuration for each
+ * pin for the selected port. Bit 0 = pin 0, 1 = pin 1.
+ * For each bit, a 0 means the edge interrupt is configured, while a 1 means a level
+ * interrupt is configured. Mask with this return value to determine the
+ * edge/level configuration for each pin in a port.
+ */
+STATIC INLINE uint32_t Chip_GPIO_IsLevelEnabled(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].IS;
+}
+
+/**
+ * @brief Sets GPIO interrupt configuration for both edges for selected pins
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to set to dual edge mode (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetEdgeModeBoth(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IBE |= pinmask;
+}
+
+/**
+ * @brief Sets GPIO interrupt configuration for a single edge for selected pins
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to set to single edge mode (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_SetEdgeModeSingle(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IBE &= ~pinmask;
+}
+
+/**
+ * @brief Returns current GPIO interrupt dual or single edge configuration for all pins for a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return A bifield containing the single/dual interrupt configuration for each
+ * pin for the selected port. Bit 0 = pin 0, 1 = pin 1.
+ * For each bit, a 0 means the interrupt triggers on a single edge (use the
+ * Chip_GPIO_SetEdgeModeHigh() and Chip_GPIO_SetEdgeModeLow() functions to configure
+ * selected edge), while a 1 means the interrupt triggers on both edges. Mask
+ * with this return value to determine the edge/level configuration for each pin in
+ * a port.
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetEdgeModeDir(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].IBE;
+}
+
+/**
+ * @brief Sets GPIO interrupt configuration when in single edge or level mode to high edge trigger or high level
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to set to high mode (ORed value of bits 0..11)
+ * @return Nothing
+ * @note Use this function to select high level or high edge interrupt mode
+ * for the selected pins on the selected port when not in dual edge mode.
+ */
+STATIC INLINE void Chip_GPIO_SetModeHigh(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IEV |= pinmask;
+}
+
+/**
+ * @brief Sets GPIO interrupt configuration when in single edge or level mode to low edge trigger or low level
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to set to low mode (ORed value of bits 0..11)
+ * @return Nothing
+ * @note Use this function to select low level or low edge interrupt mode
+ * for the selected pins on the selected port when not in dual edge mode.
+ */
+STATIC INLINE void Chip_GPIO_SetModeLow(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IEV &= ~pinmask;
+}
+
+/**
+ * @brief Returns current GPIO interrupt edge direction or level mode
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return A bifield containing the low or high direction of the interrupt
+ * configuration for each pin for the selected port. Bit 0 = pin 0, 1 = pin 1.
+ * For each bit, a 0 means the interrupt triggers on a low level or edge, while a
+ * 1 means the interrupt triggers on a high level or edge. Mask with this
+ * return value to determine the high/low configuration for each pin in a port.
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetModeHighLow(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].IEV;
+}
+
+/**
+ * @brief Enables interrupts for selected pins on a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to enable interrupts for (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_EnableInt(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IE |= pinmask;
+}
+
+/**
+ * @brief Disables interrupts for selected pins on a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to disable interrupts for (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_DisableInt(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IE &= ~pinmask;
+}
+
+/**
+ * @brief Returns current enable pin interrupts for a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return A bifield containing the enabled pin interrupts (0..11)
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetEnabledInts(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].IE;
+}
+
+/**
+ * @brief Returns raw interrupt pending status for pin interrupts for a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return A bifield containing the raw pending interrupt states for each pin (0..11) on the port
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetRawInts(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].RIS;
+}
+
+/**
+ * @brief Returns masked interrupt pending status for pin interrupts for a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @return A bifield containing the masked pending interrupt states for each pin (0..11) on the port
+ */
+STATIC INLINE uint32_t Chip_GPIO_GetMaskedInts(LPC_GPIO_T *pGPIO, uint8_t port)
+{
+ return pGPIO[port].MIS;
+}
+
+/**
+ * @brief Clears pending interrupts for selected pins for a port
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pinmask : Pins to clear interrupts for (ORed value of bits 0..11)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_GPIO_ClearInts(LPC_GPIO_T *pGPIO, uint8_t port, uint32_t pinmask)
+{
+ pGPIO[port].IC = pinmask;
+}
+
+/**
+ * @brief GPIO interrupt mode definitions
+ */
+typedef enum {
+ GPIO_INT_ACTIVE_LOW_LEVEL = 0x0, /*!< Selects interrupt on pin to be triggered on LOW level */
+ GPIO_INT_ACTIVE_HIGH_LEVEL = 0x1, /*!< Selects interrupt on pin to be triggered on HIGH level */
+ GPIO_INT_FALLING_EDGE = 0x2, /*!< Selects interrupt on pin to be triggered on FALLING level */
+ GPIO_INT_RISING_EDGE = 0x3, /*!< Selects interrupt on pin to be triggered on RISING level */
+ GPIO_INT_BOTH_EDGES = 0x6 /*!< Selects interrupt on pin to be triggered on both edges */
+} GPIO_INT_MODE_T;
+
+/**
+ * @brief Composite function for setting up a full interrupt configuration for a single pin
+ * @param pGPIO : The base of GPIO peripheral on the chip
+ * @param port : Port number
+ * @param pin : Pin number (0..11)
+ * @param modeFlags : GPIO interrupt mode selection
+ * @return Nothing
+ */
+void Chip_GPIO_SetupPinInt(LPC_GPIO_T *pGPIO, uint8_t port, uint8_t pin, GPIO_INT_MODE_T mode);
+
+#endif /* defined(CHIP_LPC11CXX) || defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC1125) */
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __GPIO_11XX_2_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/gpiogroup_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,212 @@
+/*
+ * @brief LPC11xx GPIO group driver for CHIP_LPC11AXX, CHIP_LPC11EXX, and
+ * CHIP_LPC11UXX families only.
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2013
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __GPIOGROUP_11XX_H_
+#define __GPIOGROUP_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup GPIOGP_11XX CHIP: LPC11xx GPIO group driver for CHIP_LPC11(A/E/U)XX families
+ * @ingroup CHIP_11XX_Drivers
+ * For device familes identified with CHIP definitions CHIP_LPC11AXX,
+ * CHIP_LPC11EXX, and CHIP_LPC11UXX only.
+ * @{
+ */
+
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+
+/**
+ * @brief GPIO grouped interrupt register block structure
+ */
+typedef struct { /*!< GPIO_GROUP_INTn Structure */
+ __IO uint32_t CTRL; /*!< GPIO grouped interrupt control register */
+ __I uint32_t RESERVED0[7];
+ __IO uint32_t PORT_POL[8]; /*!< GPIO grouped interrupt port polarity register */
+ __IO uint32_t PORT_ENA[8]; /*!< GPIO grouped interrupt port m enable register */
+ uint32_t RESERVED1[4072];
+} LPC_GPIOGROUPINT_T;
+
+/**
+ * LPC11xx GPIO group bit definitions
+ */
+#define GPIOGR_INT (1 << 0) /*!< GPIO interrupt pending/clear bit */
+#define GPIOGR_COMB (1 << 1) /*!< GPIO interrupt OR(0)/AND(1) mode bit */
+#define GPIOGR_TRIG (1 << 2) /*!< GPIO interrupt edge(0)/level(1) mode bit */
+
+/**
+ * @brief Clear interrupt pending status for the selected group
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_ClearIntStatus(LPC_GPIOGROUPINT_T *pGPIOGPINT, uint8_t group)
+{
+ uint32_t temp;
+
+ temp = pGPIOGPINT[group].CTRL;
+ pGPIOGPINT[group].CTRL = temp | GPIOGR_INT;
+}
+
+/**
+ * @brief Returns current GPIO group inetrrupt pending status
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @return true if the group interrupt is pending, otherwise false.
+ */
+STATIC INLINE bool Chip_GPIOGP_GetIntStatus(LPC_GPIOGROUPINT_T *pGPIOGPINT, uint8_t group)
+{
+ return (bool) ((pGPIOGPINT[group].CTRL & GPIOGR_INT) != 0);
+}
+
+/**
+ * @brief Selected GPIO group functionality for trigger on any pin in group (OR mode)
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_SelectOrMode(LPC_GPIOGROUPINT_T *pGPIOGPINT, uint8_t group)
+{
+ pGPIOGPINT[group].CTRL &= ~GPIOGR_COMB;
+}
+
+/**
+ * @brief Selected GPIO group functionality for trigger on all matching pins in group (AND mode)
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_SelectAndMode(LPC_GPIOGROUPINT_T *pGPIOGPINT, uint8_t group)
+{
+ pGPIOGPINT[group].CTRL |= GPIOGR_COMB;
+}
+
+/**
+ * @brief Selected GPIO group functionality edge trigger mode
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_SelectEdgeMode(LPC_GPIOGROUPINT_T *pGPIOGPINT, uint8_t group)
+{
+ pGPIOGPINT[group].CTRL &= ~GPIOGR_TRIG;
+}
+
+/**
+ * @brief Selected GPIO group functionality level trigger mode
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_SelectLevelMode(LPC_GPIOGROUPINT_T *pGPIOGPINT, uint8_t group)
+{
+ pGPIOGPINT[group].CTRL |= GPIOGR_TRIG;
+}
+
+/**
+ * @brief Set selected pins for the group and port to low level trigger
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @param port : GPIO port number
+ * @param pinMask : Or'ed value of pins to select for low level (bit 0 = pin 0, 1 = pin1, etc.)
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_SelectLowLevel(LPC_GPIOGROUPINT_T *pGPIOGPINT,
+ uint8_t group,
+ uint8_t port,
+ uint32_t pinMask)
+{
+ pGPIOGPINT[group].PORT_POL[port] &= ~pinMask;
+}
+
+/**
+ * @brief Set selected pins for the group and port to high level trigger
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @param port : GPIO port number
+ * @param pinMask : Or'ed value of pins to select for high level (bit 0 = pin 0, 1 = pin1, etc.)
+ * @return None
+ */
+STATIC INLINE void Chip_GPIOGP_SelectHighLevel(LPC_GPIOGROUPINT_T *pGPIOGPINT,
+ uint8_t group,
+ uint8_t port,
+ uint32_t pinMask)
+{
+ pGPIOGPINT[group].PORT_POL[port] = pinMask;
+}
+
+/**
+ * @brief Disabled selected pins for the group interrupt
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @param port : GPIO port number
+ * @param pinMask : Or'ed value of pins to disable interrupt for (bit 0 = pin 0, 1 = pin1, etc.)
+ * @return None
+ * @note Disabled pins do not contrinute to the group interrupt.
+ */
+STATIC INLINE void Chip_GPIOGP_DisableGroupPins(LPC_GPIOGROUPINT_T *pGPIOGPINT,
+ uint8_t group,
+ uint8_t port,
+ uint32_t pinMask)
+{
+ pGPIOGPINT[group].PORT_ENA[port] &= ~pinMask;
+}
+
+/**
+ * @brief Enable selected pins for the group interrupt
+ * @param pGPIOGPINT : Pointer to GPIO group register block
+ * @param group : GPIO group number
+ * @param port : GPIO port number
+ * @param pinMask : Or'ed value of pins to enable interrupt for (bit 0 = pin 0, 1 = pin1, etc.)
+ * @return None
+ * @note Enables pins contrinute to the group interrupt.
+ */
+STATIC INLINE void Chip_GPIOGP_EnableGroupPins(LPC_GPIOGROUPINT_T *pGPIOGPINT,
+ uint8_t group,
+ uint8_t port,
+ uint32_t pinMask)
+{
+ pGPIOGPINT[group].PORT_ENA[port] = pinMask;
+}
+
+#endif /* defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) */
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __GPIOGROUP_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/i2c_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,543 @@
+/*
+ * @brief LPC11xx I2C driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __I2C_11XX_H_
+#define __I2C_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup I2C_11XX CHIP: LPC11xx I2C driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+#if !defined(CHIP_LPC110X)
+
+/**
+ * @brief I2C register block structure
+ */
+typedef struct { /* I2C0 Structure */
+ __IO uint32_t CONSET; /*!< I2C Control Set Register. When a one is written to a bit of this register, the corresponding bit in the I2C control register is set. Writing a zero has no effect on the corresponding bit in the I2C control register. */
+ __I uint32_t STAT; /*!< I2C Status Register. During I2C operation, this register provides detailed status codes that allow software to determine the next action needed. */
+ __IO uint32_t DAT; /*!< I2C Data Register. During master or slave transmit mode, data to be transmitted is written to this register. During master or slave receive mode, data that has been received may be read from this register. */
+ __IO uint32_t ADR0; /*!< I2C Slave Address Register 0. Contains the 7-bit slave address for operation of the I2C interface in slave mode, and is not used in master mode. The least significant bit determines whether a slave responds to the General Call address. */
+ __IO uint32_t SCLH; /*!< SCH Duty Cycle Register High Half Word. Determines the high time of the I2C clock. */
+ __IO uint32_t SCLL; /*!< SCL Duty Cycle Register Low Half Word. Determines the low time of the I2C clock. SCLL and SCLH together determine the clock frequency generated by an I2C master and certain times used in slave mode. */
+ __O uint32_t CONCLR; /*!< I2C Control Clear Register. When a one is written to a bit of this register, the corresponding bit in the I2C control register is cleared. Writing a zero has no effect on the corresponding bit in the I2C control register. */
+ __IO uint32_t MMCTRL; /*!< Monitor mode control register. */
+ __IO uint32_t ADR1; /*!< I2C Slave Address Register. Contains the 7-bit slave address for operation of the I2C interface in slave mode, and is not used in master mode. The least significant bit determines whether a slave responds to the General Call address. */
+ __IO uint32_t ADR2; /*!< I2C Slave Address Register. Contains the 7-bit slave address for operation of the I2C interface in slave mode, and is not used in master mode. The least significant bit determines whether a slave responds to the General Call address. */
+ __IO uint32_t ADR3; /*!< I2C Slave Address Register. Contains the 7-bit slave address for operation of the I2C interface in slave mode, and is not used in master mode. The least significant bit determines whether a slave responds to the General Call address. */
+ __I uint32_t DATA_BUFFER; /*!< Data buffer register. The contents of the 8 MSBs of the DAT shift register will be transferred to the DATA_BUFFER automatically after every nine bits (8 bits of data plus ACK or NACK) has been received on the bus. */
+ __IO uint32_t MASK[4]; /*!< I2C Slave address mask register */
+} LPC_I2C_T;
+
+/**
+ * @brief Return values for SLAVE handler
+ * @note
+ * Chip drivers will usally be designed to match their events with this value
+ */
+#define RET_SLAVE_TX 6 /**< Return value, when 1 byte TX'd successfully */
+#define RET_SLAVE_RX 5 /**< Return value, when 1 byte RX'd successfully */
+#define RET_SLAVE_IDLE 2 /**< Return value, when slave enter idle mode */
+#define RET_SLAVE_BUSY 0 /**< Return value, when slave is busy */
+
+/**
+ * @brief I2C state handle return values
+ */
+#define I2C_STA_STO_RECV 0x20
+
+/*
+ * @brief I2C Control Set register description
+ */
+#define I2C_I2CONSET_AA ((0x04))/*!< Assert acknowledge flag */
+#define I2C_I2CONSET_SI ((0x08))/*!< I2C interrupt flag */
+#define I2C_I2CONSET_STO ((0x10))/*!< STOP flag */
+#define I2C_I2CONSET_STA ((0x20))/*!< START flag */
+#define I2C_I2CONSET_I2EN ((0x40))/*!< I2C interface enable */
+
+/*
+ * @brief I2C Control Clear register description
+ */
+#define I2C_I2CONCLR_AAC ((1 << 2)) /*!< Assert acknowledge Clear bit */
+#define I2C_I2CONCLR_SIC ((1 << 3)) /*!< I2C interrupt Clear bit */
+#define I2C_I2CONCLR_STOC ((1 << 4)) /*!< I2C STOP Clear bit */
+#define I2C_I2CONCLR_STAC ((1 << 5)) /*!< START flag Clear bit */
+#define I2C_I2CONCLR_I2ENC ((1 << 6)) /*!< I2C interface Disable bit */
+
+/*
+ * @brief I2C Common Control register description
+ */
+#define I2C_CON_AA (1UL << 2) /*!< Assert acknowledge bit */
+#define I2C_CON_SI (1UL << 3) /*!< I2C interrupt bit */
+#define I2C_CON_STO (1UL << 4) /*!< I2C STOP bit */
+#define I2C_CON_STA (1UL << 5) /*!< START flag bit */
+#define I2C_CON_I2EN (1UL << 6) /*!< I2C interface bit */
+
+/*
+ * @brief I2C Status Code definition (I2C Status register)
+ */
+#define I2C_STAT_CODE_BITMASK ((0xF8))/*!< Return Code mask in I2C status register */
+#define I2C_STAT_CODE_ERROR ((0xFF))/*!< Return Code error mask in I2C status register */
+
+/*
+ * @brief I2C return status code definitions
+ */
+#define I2C_I2STAT_NO_INF ((0xF8))/*!< No relevant information */
+#define I2C_I2STAT_BUS_ERROR ((0x00))/*!< Bus Error */
+
+/*
+ * @brief I2C Master transmit mode
+ */
+#define I2C_I2STAT_M_TX_START ((0x08))/*!< A start condition has been transmitted */
+#define I2C_I2STAT_M_TX_RESTART ((0x10))/*!< A repeat start condition has been transmitted */
+#define I2C_I2STAT_M_TX_SLAW_ACK ((0x18))/*!< SLA+W has been transmitted, ACK has been received */
+#define I2C_I2STAT_M_TX_SLAW_NACK ((0x20))/*!< SLA+W has been transmitted, NACK has been received */
+#define I2C_I2STAT_M_TX_DAT_ACK ((0x28))/*!< Data has been transmitted, ACK has been received */
+#define I2C_I2STAT_M_TX_DAT_NACK ((0x30))/*!< Data has been transmitted, NACK has been received */
+#define I2C_I2STAT_M_TX_ARB_LOST ((0x38))/*!< Arbitration lost in SLA+R/W or Data bytes */
+
+/*
+ * @brief I2C Master receive mode
+ */
+#define I2C_I2STAT_M_RX_START ((0x08))/*!< A start condition has been transmitted */
+#define I2C_I2STAT_M_RX_RESTART ((0x10))/*!< A repeat start condition has been transmitted */
+#define I2C_I2STAT_M_RX_ARB_LOST ((0x38))/*!< Arbitration lost */
+#define I2C_I2STAT_M_RX_SLAR_ACK ((0x40))/*!< SLA+R has been transmitted, ACK has been received */
+#define I2C_I2STAT_M_RX_SLAR_NACK ((0x48))/*!< SLA+R has been transmitted, NACK has been received */
+#define I2C_I2STAT_M_RX_DAT_ACK ((0x50))/*!< Data has been received, ACK has been returned */
+#define I2C_I2STAT_M_RX_DAT_NACK ((0x58))/*!< Data has been received, NACK has been returned */
+
+/*
+ * @brief I2C Slave receive mode
+ */
+#define I2C_I2STAT_S_RX_SLAW_ACK ((0x60))/*!< Own slave address has been received, ACK has been returned */
+#define I2C_I2STAT_S_RX_ARB_LOST_M_SLA ((0x68))/*!< Arbitration lost in SLA+R/W as master */
+// #define I2C_I2STAT_S_RX_SLAW_ACK ((0x68)) /*!< Own SLA+W has been received, ACK returned */
+#define I2C_I2STAT_S_RX_GENCALL_ACK ((0x70))/*!< General call address has been received, ACK has been returned */
+#define I2C_I2STAT_S_RX_ARB_LOST_M_GENCALL ((0x78))/*!< Arbitration lost in SLA+R/W (GENERAL CALL) as master */
+// #define I2C_I2STAT_S_RX_GENCALL_ACK ((0x78)) /*!< General call address has been received, ACK has been returned */
+#define I2C_I2STAT_S_RX_PRE_SLA_DAT_ACK ((0x80))/*!< Previously addressed with own SLA; Data has been received, ACK has been returned */
+#define I2C_I2STAT_S_RX_PRE_SLA_DAT_NACK ((0x88))/*!< Previously addressed with own SLA;Data has been received and NOT ACK has been returned */
+#define I2C_I2STAT_S_RX_PRE_GENCALL_DAT_ACK ((0x90))/*!< Previously addressed with General Call; Data has been received and ACK has been returned */
+#define I2C_I2STAT_S_RX_PRE_GENCALL_DAT_NACK ((0x98))/*!< Previously addressed with General Call; Data has been received and NOT ACK has been returned */
+#define I2C_I2STAT_S_RX_STA_STO_SLVREC_SLVTRX ((0xA0))/*!< A STOP condition or repeated START condition has been received while still addressed as SLV/REC (Slave Receive) or
+ SLV/TRX (Slave Transmit) */
+
+/*
+ * @brief I2C Slave transmit mode
+ */
+#define I2C_I2STAT_S_TX_SLAR_ACK ((0xA8))/*!< Own SLA+R has been received, ACK has been returned */
+#define I2C_I2STAT_S_TX_ARB_LOST_M_SLA ((0xB0))/*!< Arbitration lost in SLA+R/W as master */
+// #define I2C_I2STAT_S_TX_SLAR_ACK ((0xB0)) /*!< Own SLA+R has been received, ACK has been returned */
+#define I2C_I2STAT_S_TX_DAT_ACK ((0xB8))/*!< Data has been transmitted, ACK has been received */
+#define I2C_I2STAT_S_TX_DAT_NACK ((0xC0))/*!< Data has been transmitted, NACK has been received */
+#define I2C_I2STAT_S_TX_LAST_DAT_ACK ((0xC8))/*!< Last data byte in I2DAT has been transmitted (AA = 0); ACK has been received */
+#define I2C_SLAVE_TIME_OUT 0x10000000UL/*!< Time out in case of using I2C slave mode */
+
+/*
+ * @brief I2C Data register definition
+ */
+#define I2C_I2DAT_BITMASK ((0xFF))/*!< Mask for I2DAT register */
+#define I2C_I2DAT_IDLE_CHAR (0xFF) /*!< Idle data value will be send out in slave mode in case of the actual expecting data requested from the master is greater than
+ its sending data length that can be supported */
+
+/*
+ * @brief I2C Monitor mode control register description
+ */
+#define I2C_I2MMCTRL_MM_ENA ((1 << 0)) /**< Monitor mode enable */
+#define I2C_I2MMCTRL_ENA_SCL ((1 << 1)) /**< SCL output enable */
+#define I2C_I2MMCTRL_MATCH_ALL ((1 << 2)) /**< Select interrupt register match */
+#define I2C_I2MMCTRL_BITMASK ((0x07)) /**< Mask for I2MMCTRL register */
+
+/*
+ * @brief I2C Data buffer register description
+ */
+#define I2DATA_BUFFER_BITMASK ((0xFF))/*!< I2C Data buffer register bit mask */
+
+/*
+ * @brief I2C Slave Address registers definition
+ */
+#define I2C_I2ADR_GC ((1 << 0)) /*!< General Call enable bit */
+#define I2C_I2ADR_BITMASK ((0xFF))/*!< I2C Slave Address registers bit mask */
+
+/*
+ * @brief I2C Mask Register definition
+ */
+#define I2C_I2MASK_MASK(n) ((n & 0xFE))/*!< I2C Mask Register mask field */
+
+/*
+ * @brief I2C SCL HIGH duty cycle Register definition
+ */
+#define I2C_I2SCLH_BITMASK ((0xFFFF)) /*!< I2C SCL HIGH duty cycle Register bit mask */
+
+/*
+ * @brief I2C SCL LOW duty cycle Register definition
+ */
+#define I2C_I2SCLL_BITMASK ((0xFFFF)) /*!< I2C SCL LOW duty cycle Register bit mask */
+
+/*
+ * @brief I2C status values
+ */
+#define I2C_SETUP_STATUS_ARBF (1 << 8) /**< Arbitration false */
+#define I2C_SETUP_STATUS_NOACKF (1 << 9) /**< No ACK returned */
+#define I2C_SETUP_STATUS_DONE (1 << 10) /**< Status DONE */
+
+/*
+ * @brief I2C state handle return values
+ */
+#define I2C_OK 0x00
+#define I2C_BYTE_SENT 0x01
+#define I2C_BYTE_RECV 0x02
+#define I2C_LAST_BYTE_RECV 0x04
+#define I2C_SEND_END 0x08
+#define I2C_RECV_END 0x10
+#define I2C_STA_STO_RECV 0x20
+
+#define I2C_ERR (0x10000000)
+#define I2C_NAK_RECV (0x10000000 | 0x01)
+
+#define I2C_CheckError(ErrorCode) (ErrorCode & 0x10000000)
+
+/*
+ * @brief I2C monitor control configuration defines
+ */
+#define I2C_MONITOR_CFG_SCL_OUTPUT I2C_I2MMCTRL_ENA_SCL /**< SCL output enable */
+#define I2C_MONITOR_CFG_MATCHALL I2C_I2MMCTRL_MATCH_ALL /**< Select interrupt register match */
+
+/**
+ * @brief I2C Slave Identifiers
+ */
+typedef enum {
+ I2C_SLAVE_GENERAL, /**< Slave ID for general calls */
+ I2C_SLAVE_0, /**< Slave ID fo Slave Address 0 */
+ I2C_SLAVE_1, /**< Slave ID fo Slave Address 1 */
+ I2C_SLAVE_2, /**< Slave ID fo Slave Address 2 */
+ I2C_SLAVE_3, /**< Slave ID fo Slave Address 3 */
+ I2C_SLAVE_NUM_INTERFACE /**< Number of slave interfaces */
+} I2C_SLAVE_ID;
+
+/**
+ * @brief I2C transfer status
+ */
+typedef enum {
+ I2C_STATUS_DONE, /**< Transfer done successfully */
+ I2C_STATUS_NAK, /**< NAK received during transfer */
+ I2C_STATUS_ARBLOST, /**< Aribitration lost during transfer */
+ I2C_STATUS_BUSERR, /**< Bus error in I2C transfer */
+ I2C_STATUS_BUSY, /**< I2C is busy doing transfer */
+} I2C_STATUS_T;
+
+/**
+ * @brief Master transfer data structure definitions
+ */
+typedef struct {
+ uint8_t slaveAddr; /**< 7-bit I2C Slave address */
+ const uint8_t *txBuff; /**< Pointer to array of bytes to be transmitted */
+ int txSz; /**< Number of bytes in transmit array,
+ if 0 only receive transfer will be carried on */
+ uint8_t *rxBuff; /**< Pointer memory where bytes received from I2C be stored */
+ int rxSz; /**< Number of bytes to received,
+ if 0 only transmission we be carried on */
+ I2C_STATUS_T status; /**< Status of the current I2C transfer */
+} I2C_XFER_T;
+
+/**
+ * @brief I2C interface IDs
+ * @note
+ * All Chip functions will take this as the first parameter,
+ * I2C_NUM_INTERFACE must never be used for calling any Chip
+ * functions, it is only used to find the number of interfaces
+ * available in the Chip.
+ */
+typedef enum I2C_ID {
+ I2C0, /**< ID I2C0 */
+ I2C_NUM_INTERFACE /**< Number of I2C interfaces in the chip */
+} I2C_ID_T;
+
+/**
+ * @brief I2C master events
+ */
+typedef enum {
+ I2C_EVENT_WAIT = 1, /**< I2C Wait event */
+ I2C_EVENT_DONE, /**< Done event that wakes up Wait event */
+ I2C_EVENT_LOCK, /**< Re-entrency lock event for I2C transfer */
+ I2C_EVENT_UNLOCK, /**< Re-entrency unlock event for I2C transfer */
+ I2C_EVENT_SLAVE_RX, /**< Slave receive event */
+ I2C_EVENT_SLAVE_TX, /**< Slave transmit event */
+} I2C_EVENT_T;
+
+/**
+ * @brief Event handler function type
+ */
+typedef void (*I2C_EVENTHANDLER_T)(I2C_ID_T, I2C_EVENT_T);
+
+/**
+ * @brief Initializes the LPC_I2C peripheral with specified parameter.
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return Nothing
+ */
+void Chip_I2C_Init(I2C_ID_T id);
+
+/**
+ * @brief De-initializes the I2C peripheral registers to their default reset values
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return Nothing
+ */
+void Chip_I2C_DeInit(I2C_ID_T id);
+
+/**
+ * @brief Set up clock rate for LPC_I2C peripheral.
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param clockrate : Target clock rate value to initialized I2C peripheral (Hz)
+ * @return Nothing
+ * @note
+ * Parameter @a clockrate for I2C0 should be from 1000 up to 1000000
+ * (1 KHz to 1 MHz), as I2C0 support Fast Mode Plus. If the @a clockrate
+ * is more than 400 KHz (Fast Plus Mode) Board_I2C_EnableFastPlus()
+ * must be called prior to calling this function.
+ */
+void Chip_I2C_SetClockRate(I2C_ID_T id, uint32_t clockrate);
+
+/**
+ * @brief Get current clock rate for LPC_I2C peripheral.
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return The current I2C peripheral clock rate
+ */
+uint32_t Chip_I2C_GetClockRate(I2C_ID_T id);
+
+/**
+ * @brief Transmit and Receive data in master mode
+ * @param id : I2C peripheral selected (I2C0, I2C1 etc)
+ * @param xfer : Pointer to a I2C_XFER_T structure see notes below
+ * @return
+ * Any of #I2C_STATUS_T values, xfer->txSz will have number of bytes
+ * not sent due to error, xfer->rxSz will have the number of bytes yet
+ * to be received.
+ * @note
+ * The parameter @a xfer should have its member @a slaveAddr initialized
+ * to the 7-Bit slave address to which the master will do the xfer, Bit0
+ * to bit6 should have the address and Bit8 is ignored. During the transfer
+ * no code (like event handler) must change the content of the memory
+ * pointed to by @a xfer. The member of @a xfer, @a txBuff and @a txSz be
+ * initialized to the memory from which the I2C must pick the data to be
+ * transfered to slave and the number of bytes to send respectively, similarly
+ * @a rxBuff and @a rxSz must have pointer to memroy where data received
+ * from slave be stored and the number of data to get from slave respectilvely.
+ */
+int Chip_I2C_MasterTransfer(I2C_ID_T id, I2C_XFER_T *xfer);
+
+/**
+ * @brief Transmit data to I2C slave using I2C Master mode
+ * @param id : I2C peripheral ID (I2C0, I2C1 .. etc)
+ * @param slaveAddr : Slave address to which the data be written
+ * @param buff : Pointer to buffer having the array of data
+ * @param len : Number of bytes to be transfered from @a buff
+ * @return Number of bytes successfully transfered
+ */
+int Chip_I2C_MasterSend(I2C_ID_T id, uint8_t slaveAddr, const uint8_t *buff, uint8_t len);
+
+/**
+ * @brief Transfer a command to slave and receive data from slave after a repeated start
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param slaveAddr : Slave address of the I2C device
+ * @param cmd : Command (Address/Register) to be written
+ * @param buff : Pointer to memory that will hold the data received
+ * @param len : Number of bytes to receive
+ * @return Number of bytes successfully received
+ */
+int Chip_I2C_MasterCmdRead(I2C_ID_T id, uint8_t slaveAddr, uint8_t cmd, uint8_t *buff, int len);
+
+/**
+ * @brief Get pointer to current function handling the events
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return Pointer to function handing events of I2C
+ */
+I2C_EVENTHANDLER_T Chip_I2C_GetMasterEventHandler(I2C_ID_T id);
+
+/**
+ * @brief Set function that must handle I2C events
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param event : Pointer to function that will handle the event (Should not be NULL)
+ * @return 1 when successful, 0 when a transfer is on going with its own event handler
+ */
+int Chip_I2C_SetMasterEventHandler(I2C_ID_T id, I2C_EVENTHANDLER_T event);
+
+/**
+ * @brief Set function that must handle I2C events
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param slaveAddr : Slave address from which data be read
+ * @param buff : Pointer to memory where data read be stored
+ * @param len : Number of bytes to read from slave
+ * @return Number of bytes read successfully
+ */
+int Chip_I2C_MasterRead(I2C_ID_T id, uint8_t slaveAddr, uint8_t *buff, int len);
+
+/**
+ * @brief Default event handler for polling operation
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param event : Event ID of the event that called the function
+ * @return Nothing
+ */
+void Chip_I2C_EventHandlerPolling(I2C_ID_T id, I2C_EVENT_T event);
+
+/**
+ * @brief Default event handler for interrupt base operation
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param event : Event ID of the event that called the function
+ * @return Nothing
+ */
+void Chip_I2C_EventHandler(I2C_ID_T id, I2C_EVENT_T event);
+
+/**
+ * @brief I2C Master transfer state change handler
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return Nothing
+ * @note Usually called from the appropriate Interrupt handler
+ */
+void Chip_I2C_MasterStateHandler(I2C_ID_T id);
+
+/**
+ * @brief Disable I2C peripheral's operation
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return Nothing
+ */
+void Chip_I2C_Disable(I2C_ID_T id);
+
+/**
+ * @brief Checks if master xfer in progress
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return 1 if master xfer in progress 0 otherwise
+ * @note
+ * This API is generally used in interrupt handler
+ * of the application to decide whether to call
+ * master state handler or to call slave state handler
+ */
+int Chip_I2C_IsMasterActive(I2C_ID_T id);
+
+/**
+ * @brief Setup a slave I2C device
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @param sid : I2C Slave peripheral ID (I2C_SLAVE_0, I2C_SLAVE_1 etc)
+ * @param xfer : Pointer to transfer structure (see note below for more info)
+ * @param event : Event handler for slave transfers
+ * @param addrMask : Address mask to use along with slave address (see notes below for more info)
+ * @return Nothing
+ * @note
+ * Parameter @a xfer should point to a valid I2C_XFER_T structure object
+ * and must have @a slaveAddr initialized with 7bit Slave address (From Bit1 to Bit7),
+ * Bit0 when set enables general call handling, @a slaveAddr along with @a addrMask will
+ * be used to match the slave address. @a rxBuff and @a txBuff must point to valid buffers
+ * where slave can receive or send the data from, size of which will be provided by
+ * @a rxSz and @a txSz respectively. Function pointed to by @a event will be called
+ * for the following events #I2C_EVENT_SLAVE_RX (One byte of data received successfully
+ * from the master and stored inside memory pointed by xfer->rxBuff, incremented
+ * the pointer and decremented the @a xfer->rxSz), #I2C_EVENT_SLAVE_TX (One byte of
+ * data from xfer->txBuff was sent to master successfully, incremented the pointer
+ * and decremented xfer->txSz), #I2C_EVENT_DONE (Master is done doing its transfers
+ * with the slave).<br>
+ * <br>Bit-0 of the parameter @a addrMask is reserved and should always be 0. Any bit (BIT1
+ * to BIT7) set in @a addrMask will make the corresponding bit in *xfer->slaveAddr* as
+ * don't care. Thit is, if *xfer->slaveAddr* is (0x10 << 1) and @a addrMask is (0x03 << 1) then
+ * 0x10, 0x11, 0x12, 0x13 will all be considered as valid slave addresses for the registered
+ * slave. Upon receving any event *xfer->slaveAddr* (BIT1 to BIT7) will hold the actual
+ * address which was received from master.<br>
+ * <br><b>General Call Handling</b><br>
+ * Slave can receive data from master using general call address (0x00). General call
+ * handling must be setup as given below
+ * - Call Chip_I2C_SlaveSetup() with argument @a sid as I2C_SLAVE_GENERAL
+ * - xfer->slaveAddr ignored, argument @a addrMask ignored
+ * - function provided by @a event will registered to be called when slave received data using addr 0x00
+ * - xfer->rxBuff and xfer->rxSz should be valid in argument @a xfer
+ * - To handle General Call only (No other slaves are configured)
+ * - Call Chip_I2C_SlaveSetup() with sid as I2C_SLAVE_X (X=0,1,2,3)
+ * - setup @a xfer with slaveAddr member set to 0, @a event is ignored hence can be NULL
+ * - provide @a addrMask (typically 0, if not you better be knowing what you are doing)
+ * - To handler General Call when other slave is active
+ * - Call Chip_I2C_SlaveSetup() with sid as I2C_SLAVE_X (X=0,1,2,3)
+ * - setup @a xfer with slaveAddr member set to 7-Bit Slave address [from Bit1 to 7]
+ * - Set Bit0 of @a xfer->slaveAddr as 1
+ * - Provide appropriate @a addrMask
+ * - Argument @a event must point to function, that handles events from actual slaveAddress and not the GC
+ * @warning
+ * If the slave has only one byte in its txBuff, once that byte is transfered to master the event handler
+ * will be called for event #I2C_EVENT_DONE. If the master attempts to read more bytes in the same transfer
+ * then the slave hardware will send 0xFF to master till the end of transfer, event handler will not be
+ * called to notify this. For more info see section below<br>
+ * <br><b> Last data handling in slave </b><br>
+ * If the user wants to implement a slave which will read a byte from a specific location over and over
+ * again whenever master reads the slave. If the user initializes the xfer->txBuff as the location to read
+ * the byte from and xfer->txSz as 1, then say, if master reads one byte; slave will send the byte read from
+ * xfer->txBuff and will call the event handler with #I2C_EVENT_DONE. If the master attempts to read another
+ * byte instead of sending the byte read from xfer->txBuff the slave hardware will send 0xFF and no event will
+ * occur. To handle this issue, slave should set xfer->txSz to 2, in which case when master reads the byte
+ * event handler will be called with #I2C_EVENT_SLAVE_TX, in which the slave implementation can reset the buffer
+ * and size back to original location (i.e, xfer->txBuff--, xfer->txSz++), if the master reads another byte
+ * in the same transfer, byte read from xfer->txBuff will be sent and #I2C_EVENT_SLAVE_TX will be called again, and
+ * the process repeats.
+ */
+void Chip_I2C_SlaveSetup(I2C_ID_T id,
+ I2C_SLAVE_ID sid,
+ I2C_XFER_T *xfer,
+ I2C_EVENTHANDLER_T event,
+ uint8_t addrMask);
+
+/**
+ * @brief I2C Slave event handler
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return Nothing
+ */
+void Chip_I2C_SlaveStateHandler(I2C_ID_T id);
+
+/**
+ * @brief I2C peripheral state change checking
+ * @param id : I2C peripheral ID (I2C0, I2C1 ... etc)
+ * @return 1 if I2C peripheral @a id has changed its state,
+ * 0 if there is no state change
+ * @note
+ * This function must be used by the application when
+ * the polling has to be done based on state change.
+ */
+int Chip_I2C_IsStateChanged(I2C_ID_T id);
+
+#endif /* !defined(CHIP_LPC110X) */
+
+/**
+ * @}
+ */
+
+ #ifdef __cplusplus
+}
+#endif
+
+#endif /* __I2C_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/iocon_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,288 @@
+/*
+ * @brief IOCON registers and control functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __IOCON_11XX_H_
+#define __IOCON_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup IOCON_11XX CHIP: LPC11xx IO Control driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief IO Configuration Unit register block structure
+ */
+#if defined(CHIP_LPC11UXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX)
+typedef struct { /*!< LPC11AXX/LPC11UXX/LPC11EXX IOCON Structure */
+ __IO uint32_t PIO0[24];
+ __IO uint32_t PIO1[32];
+} LPC_IOCON_T;
+
+#else
+/**
+ * @brief LPC11XX I/O Configuration register offset
+ */
+typedef enum CHIP_IOCON_PIO {
+ IOCON_PIO0_0 = (0x00C >> 2),
+ IOCON_PIO0_1 = (0x010 >> 2),
+ IOCON_PIO0_2 = (0x01C >> 2),
+ IOCON_PIO0_3 = (0x02C >> 2),
+ IOCON_PIO0_4 = (0x030 >> 2),
+ IOCON_PIO0_5 = (0x034 >> 2),
+ IOCON_PIO0_6 = (0x04C >> 2),
+ IOCON_PIO0_7 = (0x050 >> 2),
+ IOCON_PIO0_8 = (0x060 >> 2),
+ IOCON_PIO0_9 = (0x064 >> 2),
+ IOCON_PIO0_10 = (0x070 >> 2),
+ IOCON_PIO0_11 = (0x074 >> 2),
+
+ IOCON_PIO1_0 = (0x078 >> 2),
+ IOCON_PIO1_1 = (0x07C >> 2),
+ IOCON_PIO1_2 = (0x080 >> 2),
+ IOCON_PIO1_3 = (0x090 >> 2),
+ IOCON_PIO1_4 = (0x094 >> 2),
+ IOCON_PIO1_5 = (0x0A0 >> 2),
+ IOCON_PIO1_6 = (0x0A4 >> 2),
+ IOCON_PIO1_7 = (0x0A8 >> 2),
+ IOCON_PIO1_8 = (0x014 >> 2),
+ IOCON_PIO1_9 = (0x038 >> 2),
+ IOCON_PIO1_10 = (0x06C >> 2),
+ IOCON_PIO1_11 = (0x098 >> 2),
+
+ IOCON_PIO2_0 = (0x008 >> 2),
+ IOCON_PIO2_1 = (0x028 >> 2),
+ IOCON_PIO2_2 = (0x05C >> 2),
+ IOCON_PIO2_3 = (0x08C >> 2),
+ IOCON_PIO2_4 = (0x040 >> 2),
+ IOCON_PIO2_5 = (0x044 >> 2),
+ IOCON_PIO2_6 = (0x000 >> 2),
+ IOCON_PIO2_7 = (0x020 >> 2),
+ IOCON_PIO2_8 = (0x024 >> 2),
+ IOCON_PIO2_9 = (0x054 >> 2),
+ IOCON_PIO2_10 = (0x058 >> 2),
+#if !defined(CHIP_LPC1125)
+ IOCON_PIO2_11 = (0x070 >> 2),
+#endif
+
+ IOCON_PIO3_0 = (0x084 >> 2),
+#if !defined(CHIP_LPC1125)
+ IOCON_PIO3_1 = (0x088 >> 2),
+#endif
+ IOCON_PIO3_2 = (0x09C >> 2),
+ IOCON_PIO3_3 = (0x0AC >> 2),
+ IOCON_PIO3_4 = (0x03C >> 2),
+ IOCON_PIO3_5 = (0x048 >> 2),
+} CHIP_IOCON_PIO_T;
+
+/**
+ * @brief LPC11XX Pin location select
+ */
+typedef enum CHIP_IOCON_PIN_LOC {
+ IOCON_SCKLOC_PIO0_10 = (0xB0), /*!< Selects SCK0 function in pin location PIO0_10 */
+#if !defined(CHIP_LPC1125)
+ IOCON_SCKLOC_PIO2_11 = (0xB0 | 1), /*!< Selects SCK0 function in pin location PIO2_11 */
+#endif
+ IOCON_SCKLOC_PIO0_6 = (0xB0 | 2), /*!< Selects SCK0 function in pin location PIO0_6 */
+
+ IOCON_DSRLOC_PIO2_1 = (0xB4), /*!< Selects DSR function in pin location PIO2_1 */
+#if !defined(CHIP_LPC1125)
+ IOCON_DSRLOC_PIO3_1 = (0xB4 | 1), /*!< Selects DSR function in pin location PIO3_1 */
+#endif
+
+ IOCON_DCDLOC_PIO2_2 = (0xB8), /*!< Selects DCD function in pin location PIO2_2 */
+ IOCON_DCDLOC_PIO3_2 = (0xB8 | 1), /*!< Selects DCD function in pin location PIO3_2 */
+
+ IOCON_RILOC_PIO2_3 = (0xBC), /*!< Selects RI function in pin location PIO2_3 */
+ IOCON_RILOC_PIO3_3 = (0xBC | 1), /*!< Selects Ri function in pin location PIO3_3 */
+
+#if defined(CHIP_LPC1125)
+ IOCON_SSEL1_LOC_PIO2_2 = (0x18), /*!< Selects SSEL1 function in pin location PIO2_2 */
+ IOCON_SSEL1_LOC_PIO2_4 = (0x18 | 1), /*!< Selects SSEL1 function in pin location PIO2_4 */
+
+ IOCON_CT16B0_CAP0_LOC_PIO0_2 = (0xC0), /*!< Selects SSEL1 CTB16B0_CAP0 function in pin location PIO0_2 */
+ IOCON_CT16B0_CAP0_LOC_PIO3_3 = (0xC0 | 1), /*!< Selects SSEL1 CTB16B0_CAP0 function in pin location PIO3_3 */
+
+ IOCON_SCK1_LOC_PIO2_1 = (0xC4), /*!< Selects SCK1 function in pin location PIO2_1 */
+ IOCON_SCK1_LOC_PIO3_2 = (0xC4 | 1), /*!< Selects SCK1 function in pin location PIO3_2 */
+
+ IOCON_MISO1_LOC_PIO2_2 = (0xC8), /*!< Selects MISO1 function in pin location PIO2_2 */
+ IOCON_MISO1_LOC_PIO1_10 = (0xC8 | 1), /*!< Selects MISO1 function in pin location PIO1_10 */
+
+ IOCON_MOSI1_LOC_PIO2_3 = (0xCC), /*!< Selects MOSI1 function in pin location PIO2_3 */
+ IOCON_MOSI1_LOC_PIO1_9 = (0xCC), /*!< Selects MOSI1 function in pin location PIO1_9 */
+
+ IOCON_CT326B0_CAP0_LOC_PIO1_5 = (0xD0), /*!< Selects CT32B0_CAP0 function in pin location PIO1_5 */
+ IOCON_CT326B0_CAP0_LOC_PIO2_9 = (0xD0 | 1), /*!< Selects CT32B0_CAP0 function in pin location PIO2_9 */
+
+ IOCON_U0_RXD_LOC_PIO1_6 = (0xD4), /*!< Selects U0 RXD function in pin location PIO1_6 */
+ IOCON_U0_RXD_LOC_PIO2_7 = (0xD4 | 1), /*!< Selects U0 RXD function in pin location PIO2_7 */
+ IOCON_U0_RXD_LOC_PIO3_4 = (0xD4 | 3), /*!< Selects U0 RXD function in pin location PIO3_4 */
+#endif
+
+} CHIP_IOCON_PIN_LOC_T;
+
+typedef struct { /*!< LPC11XX/LPC11XXLV/LPC11UXX IOCON Structure */
+ __IO uint32_t REG[48];
+} LPC_IOCON_T;
+#endif
+
+/**
+ * IOCON function and mode selection definitions
+ * See the User Manual for specific modes and functions supported by the
+ * various LPC11xx devices. Functionality can vary per device.
+ */
+#define IOCON_FUNC0 0x0 /*!< Selects pin function 0 */
+#define IOCON_FUNC1 0x1 /*!< Selects pin function 1 */
+#define IOCON_FUNC2 0x2 /*!< Selects pin function 2 */
+#define IOCON_FUNC3 0x3 /*!< Selects pin function 3 */
+#define IOCON_FUNC4 0x4 /*!< Selects pin function 4 */
+#define IOCON_FUNC5 0x5 /*!< Selects pin function 5 */
+#define IOCON_FUNC6 0x6 /*!< Selects pin function 6 */
+#define IOCON_FUNC7 0x7 /*!< Selects pin function 7 */
+#define IOCON_MODE_INACT (0x0 << 3) /*!< No addition pin function */
+#define IOCON_MODE_PULLDOWN (0x1 << 3) /*!< Selects pull-down function */
+#define IOCON_MODE_PULLUP (0x2 << 3) /*!< Selects pull-up function */
+#define IOCON_MODE_REPEATER (0x3 << 3) /*!< Selects pin repeater function */
+#define IOCON_HYS_EN (0x1 << 5) /*!< Enables hysteresis */
+#define IOCON_INV_EN (0x1 << 6) /*!< Enables invert function on input */
+#define IOCON_ADMODE_EN (0x0 << 7) /*!< Enables analog input function (analog pins only) */
+#define IOCON_DIGMODE_EN (0x1 << 7) /*!< Enables digital function (analog pins only) */
+#define IOCON_SFI2C_EN (0x0 << 8) /*!< I2C standard mode/fast-mode */
+#define IOCON_STDI2C_EN (0x1 << 8) /*!< I2C standard I/O functionality */
+#define IOCON_FASTI2C_EN (0x2 << 8) /*!< I2C Fast-mode Plus */
+#define IOCON_FILT_DIS (0x1 << 8) /*!< Disables noise pulses filtering (10nS glitch filter) */
+#define IOCON_OPENDRAIN_EN (0x1 << 10) /*!< Enables open-drain function */
+
+/**
+ * IOCON function and mode selection definitions (old)
+ * For backwards compatibility.
+ */
+#define MD_PLN (0x0 << 3) /*!< Disable pull-down and pull-up resistor at resistor at pad */
+#define MD_PDN (0x1 << 3) /*!< Enable pull-down resistor at pad */
+#define MD_PUP (0x2 << 3) /*!< Enable pull-up resistor at pad */
+#define MD_BUK (0x3 << 3) /*!< Enable pull-down and pull-up resistor at resistor at pad (repeater mode) */
+#define MD_HYS (0x1 << 5) /*!< Enable hysteresis */
+#define MD_INV (0x1 << 6) /*!< Invert enable */
+#define MD_ADMODE (0x0 << 7) /*!< Select analog mode */
+#define MD_DIGMODE (0x1 << 7) /*!< Select digitial mode */
+#define MD_DISFIL (0x0 << 8) /*!< Disable 10nS input glitch filter */
+#define MD_ENFIL (0x1 << 8) /*!< Enable 10nS input glitch filter */
+#define MD_SFI2C (0x0 << 8) /*!< I2C standard mode/fast-mode */
+#define MD_STDI2C (0x1 << 8) /*!< I2C standard I/O functionality */
+#define MD_FASTI2C (0x2 << 8) /*!< I2C Fast-mode Plus */
+#define MD_OPENDRAIN (0x1 << 10) /*!< Open drain mode bit */
+#define FUNC0 0x0
+#define FUNC1 0x1
+#define FUNC2 0x2
+#define FUNC3 0x3
+#define FUNC4 0x4
+#define FUNC5 0x5
+#define FUNC6 0x6
+#define FUNC7 0x7
+
+#if defined(CHIP_LPC11UXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11AXX)
+/**
+ * @brief Sets I/O Control pin mux
+ * @param pIOCON : The base of IOCON peripheral on the chip
+ * @param port : GPIO port to mux
+ * @param pin : GPIO pin to mux
+ * @param modefunc : OR'ed values or type IOCON_*
+ * @return Nothing
+ */
+void Chip_IOCON_PinMuxSet(LPC_IOCON_T *pIOCON, uint8_t port, uint8_t pin, uint32_t modefunc);
+
+/**
+ * @brief I/O Control pin mux
+ * @param pIOCON : The base of IOCON peripheral on the chip
+ * @param port : GPIO port to mux
+ * @param pin : GPIO pin to mux
+ * @param mode : OR'ed values or type IOCON_*
+ * @param func : Pin function, value of type IOCON_FUNC?
+ * @return Nothing
+ */
+STATIC INLINE void Chip_IOCON_PinMux(LPC_IOCON_T *pIOCON, uint8_t port, uint8_t pin, uint16_t mode, uint8_t func)
+{
+ Chip_IOCON_PinMuxSet(pIOCON, port, pin, (uint32_t) (mode | func));
+}
+
+#else
+
+/**
+ * @brief Sets I/O Control pin mux
+ * @param pIOCON : The base of IOCON peripheral on the chip
+ * @param pin : GPIO pin to mux
+ * @param modefunc : OR'ed values or type IOCON_*
+ * @return Nothing
+ */
+STATIC INLINE void Chip_IOCON_PinMuxSet(LPC_IOCON_T *pIOCON, CHIP_IOCON_PIO_T pin, uint32_t modefunc)
+{
+ pIOCON->REG[pin] = modefunc;
+}
+
+/**
+ * @brief I/O Control pin mux
+ * @param pIOCON : The base of IOCON peripheral on the chip
+ * @param pin : GPIO pin to mux
+ * @param mode : OR'ed values or type IOCON_*
+ * @param func : Pin function, value of type IOCON_FUNC?
+ * @return Nothing
+ */
+STATIC INLINE void Chip_IOCON_PinMux(LPC_IOCON_T *pIOCON, CHIP_IOCON_PIO_T pin, uint16_t mode, uint8_t func)
+{
+ Chip_IOCON_PinMuxSet(pIOCON, pin, (uint32_t) (mode | func));
+}
+
+/**
+ * @brief Select pin location
+ * @param pIOCON : The base of IOCON peripheral on the chip
+ * @param sel : location selection
+ * @return Nothing
+ */
+STATIC INLINE void Chip_IOCON_PinLocSel(LPC_IOCON_T *pIOCON, CHIP_IOCON_PIN_LOC_T sel)
+{
+ pIOCON->REG[sel >> 2] = sel & 0x03;
+}
+
+#endif /* defined(CHIP_LPC11UXX) || defined (CHIP_LPC11EXX) || defined (CHIP_LPC11AXX) */
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __IOCON_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/lpc_types.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,216 @@
+/*
+ * @brief Common types used in LPC functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __LPC_TYPES_H_
+#define __LPC_TYPES_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/** @defgroup LPC_Types CHIP: LPC Common Types
+ * @ingroup CHIP_Common
+ * @{
+ */
+
+/** @defgroup LPC_Types_Public_Types LPC Public Types
+ * @{
+ */
+
+/**
+ * @brief Boolean Type definition
+ */
+typedef enum {FALSE = 0, TRUE = !FALSE} Bool;
+
+/**
+ * @brief Boolean Type definition
+ */
+#if !defined(__cplusplus)
+// typedef enum {false = 0, true = !false} bool;
+#endif
+
+/**
+ * @brief Flag Status and Interrupt Flag Status type definition
+ */
+typedef enum {RESET = 0, SET = !RESET} FlagStatus, IntStatus, SetState;
+#define PARAM_SETSTATE(State) ((State == RESET) || (State == SET))
+
+/**
+ * @brief Functional State Definition
+ */
+typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
+#define PARAM_FUNCTIONALSTATE(State) ((State == DISABLE) || (State == ENABLE))
+
+/**
+ * @ Status type definition
+ */
+typedef enum {ERROR = 0, SUCCESS = !ERROR} Status;
+
+/**
+ * Read/Write transfer type mode (Block or non-block)
+ */
+typedef enum {
+ NONE_BLOCKING = 0, /**< None Blocking type */
+ BLOCKING, /**< Blocking type */
+} TRANSFER_BLOCK_T;
+
+/** Pointer to Function returning Void (any number of parameters) */
+typedef void (*PFV)();
+
+/** Pointer to Function returning int32_t (any number of parameters) */
+typedef int32_t (*PFI)();
+
+/**
+ * @}
+ */
+
+/** @defgroup LPC_Types_Public_Macros LPC Public Macros
+ * @{
+ */
+
+/* _BIT(n) sets the bit at position "n"
+ * _BIT(n) is intended to be used in "OR" and "AND" expressions:
+ * e.g., "(_BIT(3) | _BIT(7))".
+ */
+#undef _BIT
+/* Set bit macro */
+#define _BIT(n) (1 << (n))
+
+/* _SBF(f,v) sets the bit field starting at position "f" to value "v".
+ * _SBF(f,v) is intended to be used in "OR" and "AND" expressions:
+ * e.g., "((_SBF(5,7) | _SBF(12,0xF)) & 0xFFFF)"
+ */
+#undef _SBF
+/* Set bit field macro */
+#define _SBF(f, v) ((v) << (f))
+
+/* _BITMASK constructs a symbol with 'field_width' least significant
+ * bits set.
+ * e.g., _BITMASK(5) constructs '0x1F', _BITMASK(16) == 0xFFFF
+ * The symbol is intended to be used to limit the bit field width
+ * thusly:
+ * <a_register> = (any_expression) & _BITMASK(x), where 0 < x <= 32.
+ * If "any_expression" results in a value that is larger than can be
+ * contained in 'x' bits, the bits above 'x - 1' are masked off. When
+ * used with the _SBF example above, the example would be written:
+ * a_reg = ((_SBF(5,7) | _SBF(12,0xF)) & _BITMASK(16))
+ * This ensures that the value written to a_reg is no wider than
+ * 16 bits, and makes the code easier to read and understand.
+ */
+#undef _BITMASK
+/* Bitmask creation macro */
+#define _BITMASK(field_width) ( _BIT(field_width) - 1)
+
+/* NULL pointer */
+#ifndef NULL
+#define NULL ((void *) 0)
+#endif
+
+/* Number of elements in an array */
+#define NELEMENTS(array) (sizeof(array) / sizeof(array[0]))
+
+/* Static data/function define */
+#define STATIC static
+/* External data/function define */
+#define EXTERN extern
+
+#if !defined(MAX)
+#define MAX(a, b) (((a) > (b)) ? (a) : (b))
+#endif
+#if !defined(MIN)
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#endif
+
+/**
+ * @}
+ */
+
+/* Old Type Definition compatibility */
+/** @addtogroup LPC_Types_Public_Types
+ * @{
+ */
+
+/** LPC type for character type */
+typedef char CHAR;
+
+/** LPC type for 8 bit unsigned value */
+typedef uint8_t UNS_8;
+
+/** LPC type for 8 bit signed value */
+typedef int8_t INT_8;
+
+/** LPC type for 16 bit unsigned value */
+typedef uint16_t UNS_16;
+
+/** LPC type for 16 bit signed value */
+typedef int16_t INT_16;
+
+/** LPC type for 32 bit unsigned value */
+typedef uint32_t UNS_32;
+
+/** LPC type for 32 bit signed value */
+typedef int32_t INT_32;
+
+/** LPC type for 64 bit signed value */
+typedef int64_t INT_64;
+
+/** LPC type for 64 bit unsigned value */
+typedef uint64_t UNS_64;
+
+#ifdef __CODE_RED
+#define BOOL_32 bool
+#define BOOL_16 bool
+#define BOOL_8 bool
+#else
+/** 32 bit boolean type */
+typedef bool BOOL_32;
+
+/** 16 bit boolean type */
+typedef bool BOOL_16;
+
+/** 8 bit boolean type */
+typedef bool BOOL_8;
+#endif
+
+#ifdef __CC_ARM
+#define INLINE __inline
+#else
+#define INLINE inline
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+#endif /* __LPC_TYPES_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/pinint_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,257 @@
+/*
+ * @brief LPC11xx Pin Interrupt and Pattern Match Registers and driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __PININT_11XX_H_
+#define __PININT_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup PININT_11XX CHIP: LPC11xx Pin Interrupt and Pattern Match driver
+ * @ingroup CHIP_11XX_Drivers
+ * For device familes identified with CHIP definitions CHIP_LPC11AXX,
+ * CHIP_LPC11EXX, and CHIP_LPC11UXX only.
+ * @{
+ */
+
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+
+/**
+ * @brief LPC11xx Pin Interrupt and Pattern Match register block structure
+ */
+typedef struct { /*!< PIN_INT Structure */
+ __IO uint32_t ISEL; /*!< Pin Interrupt Mode register */
+ __IO uint32_t IENR; /*!< Pin Interrupt Enable (Rising) register */
+ __IO uint32_t SIENR; /*!< Set Pin Interrupt Enable (Rising) register */
+ __IO uint32_t CIENR; /*!< Clear Pin Interrupt Enable (Rising) register */
+ __IO uint32_t IENF; /*!< Pin Interrupt Enable Falling Edge / Active Level register */
+ __IO uint32_t SIENF; /*!< Set Pin Interrupt Enable Falling Edge / Active Level register */
+ __IO uint32_t CIENF; /*!< Clear Pin Interrupt Enable Falling Edge / Active Level address */
+ __IO uint32_t RISE; /*!< Pin Interrupt Rising Edge register */
+ __IO uint32_t FALL; /*!< Pin Interrupt Falling Edge register */
+ __IO uint32_t IST; /*!< Pin Interrupt Status register */
+} LPC_PIN_INT_T;
+
+/**
+ * LPC11xx Pin Interrupt channel values
+ */
+#define PININTCH0 (1 << 0)
+#define PININTCH1 (1 << 1)
+#define PININTCH2 (1 << 2)
+#define PININTCH3 (1 << 3)
+#define PININTCH4 (1 << 4)
+#define PININTCH5 (1 << 5)
+#define PININTCH6 (1 << 6)
+#define PININTCH7 (1 << 7)
+#define PININTCH(ch) (1 << (ch))
+
+/**
+ * @brief Initialize Pin interrupt block
+ * @param pPININT : The base address of Pin interrupt block
+ * @return Nothing
+ * @note This function should be used after the Chip_GPIO_Init() function.
+ */
+STATIC INLINE void Chip_PININT_Init(LPC_PIN_INT_T *pPININT) {}
+
+/**
+ * @brief De-Initialize Pin interrupt block
+ * @param pPININT : The base address of Pin interrupt block
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_DeInit(LPC_PIN_INT_T *pPININT) {}
+
+/**
+ * @brief Configure the pins as edge sensitive in Pin interrupt block
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_SetPinModeEdge(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->ISEL &= ~pins;
+}
+
+/**
+ * @brief Configure the pins as level sensitive in Pin interrupt block
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_SetPinModeLevel(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->ISEL |= pins;
+}
+
+/**
+ * @brief Return current PININT rising edge or high level interrupt enable state
+ * @param pPININT : The base address of Pin interrupt block
+ * @return A bifield containing the high edge/level interrupt enables for each
+ * interrupt. Bit 0 = PININT0, 1 = PININT1, etc.
+ * For each bit, a 0 means the high edge/level interrupt is disabled, while a 1
+ * means it's enabled.
+ */
+STATIC INLINE uint32_t Chip_PININT_GetHighEnabled(LPC_PIN_INT_T *pPININT)
+{
+ return pPININT->IENR;
+}
+
+/**
+ * @brief Enable high edge/level PININT interrupts for pins
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins to enable (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_EnableIntHigh(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->SIENR = pins;
+}
+
+/**
+ * @brief Disable high edge/level PININT interrupts for pins
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins to disable (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_DisableIntHigh(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->CIENR = pins;
+}
+
+/**
+ * @brief Return current PININT falling edge or low level interrupt enable state
+ * @param pPININT : The base address of Pin interrupt block
+ * @return A bifield containing the low edge/level interrupt enables for each
+ * interrupt. Bit 0 = PININT0, 1 = PININT1, etc.
+ * For each bit, a 0 means the low edge/level interrupt is disabled, while a 1
+ * means it's enabled.
+ */
+STATIC INLINE uint32_t Chip_PININT_GetLowEnabled(LPC_PIN_INT_T *pPININT)
+{
+ return pPININT->IENF;
+}
+
+/**
+ * @brief Enable low edge/level PININT interrupts for pins
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins to enable (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_EnableIntLow(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->SIENF = pins;
+}
+
+/**
+ * @brief Disable low edge/level PININT interrupts for pins
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins to disable (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_DisableIntLow(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->CIENF = pins;
+}
+
+/**
+ * @brief Return pin states that have a detected latched high edge (RISE) state
+ * @param pPININT : The base address of Pin interrupt block
+ * @return PININT states (bit n = high) with a latched rise state detected
+ */
+STATIC INLINE uint32_t Chip_PININT_GetRiseStates(LPC_PIN_INT_T *pPININT)
+{
+ return pPININT->RISE;
+}
+
+/**
+ * @brief Clears pin states that had a latched high edge (RISE) state
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins with latched states to clear
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_ClearRiseStates(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->RISE = pins;
+}
+
+/**
+ * @brief Return pin states that have a detected latched falling edge (FALL) state
+ * @param pPININT : The base address of Pin interrupt block
+ * @return PININT states (bit n = high) with a latched rise state detected
+ */
+STATIC INLINE uint32_t Chip_PININT_GetFallStates(LPC_PIN_INT_T *pPININT)
+{
+ return pPININT->FALL;
+}
+
+/**
+ * @brief Clears pin states that had a latched falling edge (FALL) state
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pins with latched states to clear
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_ClearFallStates(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->FALL = pins;
+}
+
+/**
+ * @brief Get interrupt status from Pin interrupt block
+ * @param pPININT : The base address of Pin interrupt block
+ * @return Interrupt status (bit n for PININTn = high means interrupt ie pending)
+ */
+STATIC INLINE uint32_t Chip_PININT_GetIntStatus(LPC_PIN_INT_T *pPININT)
+{
+ return pPININT->IST;
+}
+
+/**
+ * @brief Clear interrupt status in Pin interrupt block
+ * @param pPININT : The base address of Pin interrupt block
+ * @param pins : Pin interrupts to clear (ORed value of PININTCH*)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_PININT_ClearIntStatus(LPC_PIN_INT_T *pPININT, uint32_t pins)
+{
+ pPININT->IST = pins;
+}
+
+#endif /* defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) */
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __PININT_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/pmu_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,201 @@
+/*
+ * @brief LPC11xx PMU chip driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __PMU_11XX_H_
+#define __PMU_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup PMU_11XX CHIP: LPC11xx Power Management Unit block driver
+ * @ingroup CHIP_11XX_Drivers
+ * This driver only applies to devices in the CHIP_LPC11AXX, CHIP_LPC11CXX,
+ * CHIP_LPC11EXX, CHIP_LPC11UXX, and CHIP_LPC1125 families. Note different
+ * families may have slightly different PMU support.
+ * @{
+ */
+
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC1125)
+#if defined(CHIP_LPC1125)
+#error "LPC1125 support for the PMU driver is not ready"
+#endif
+
+/**
+ * @brief LPC11xx Power Management Unit register block structure
+ */
+typedef struct {
+ __IO uint32_t PCON; /*!< Offset: 0x000 Power control Register (R/W) */
+ __IO uint32_t GPREG[4]; /*!< Offset: 0x004 General purpose Registers 0..3 (R/W) */
+} LPC_PMU_T;
+
+/**
+ * @brief LPC11xx low power mode type definitions
+ */
+typedef enum CHIP_PMU_MCUPOWER {
+ PMU_MCU_SLEEP = 0, /*!< Sleep mode */
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+ PMU_MCU_DEEP_SLEEP, /*!< Deep Sleep mode */
+ PMU_MCU_POWER_DOWN, /*!< Power down mode */
+ PMU_MCU_DEEP_PWRDOWN /*!< Deep power down mode */
+#elif defined(CHIP_LPC11CXX)
+ PMU_MCU_DEEP_PWRDOWN = 3 /*!< Deep power down mode */
+#endif
+} CHIP_PMU_MCUPOWER_T;
+
+/**
+ * PMU PCON register bit fields & masks
+ */
+#define PMU_PCON_PM_SLEEP (0x0) /*!< ARM WFI enter sleep mode */
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+#define PMU_PCON_PM_DEEPSLEEP (0x1) /*!< ARM WFI enter Deep-sleep mode */
+#define PMU_PCON_PM_POWERDOWN (0x2) /*!< ARM WFI enter Power-down mode */
+#define PMU_PCON_PM_DEEPPOWERDOWN (0x3) /*!< ARM WFI enter Deep Power-down mode */
+#elif defined(CHIP_LPC11CXX)
+#define PMU_PCON_PM_DEEPPOWERDOWN (0x2)
+#endif
+#define PMU_PCON_SLEEPFLAG (1 << 8) /*!< Sleep mode flag */
+#define PMU_PCON_DPDFLAG (1 << 11) /*!< Deep power-down flag */
+
+/**
+ * @brief Write a value to a GPREG register
+ * @param pPMU : Pointer to PMU register block
+ * @param regIndex : Register index to write to, must be 0..3
+ * @param value : Value to write
+ * @return None
+ */
+STATIC INLINE void Chip_PMU_WriteGPREG(LPC_PMU_T *pPMU, uint8_t regIndex, uint32_t value)
+{
+ pPMU->GPREG[regIndex] = value;
+}
+
+/**
+ * @brief Read a value to a GPREG register
+ * @param pPMU : Pointer to PMU register block
+ * @param regIndex : Register index to read from, must be 0..3
+ * @return Value read from the GPREG register
+ */
+STATIC INLINE uint32_t Chip_PMU_ReadGPREG(LPC_PMU_T *pPMU, uint8_t regIndex)
+{
+ return pPMU->GPREG[regIndex];
+}
+
+/**
+ * @brief Enter MCU Sleep mode
+ * @param pPMU : Pointer to PMU register block
+ * @return None
+ * @note The sleep mode affects the ARM Cortex-M0+ core only. Peripherals
+ * and memories are active.
+ */
+void Chip_PMU_SleepState(LPC_PMU_T *pPMU);
+
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+/**
+ * @brief Enter MCU Deep Sleep mode
+ * @param pPMU : Pointer to PMU register block
+ * @return None
+ * @note In Deep-sleep mode, the peripherals receive no internal clocks.
+ * The flash is in stand-by mode. The SRAM memory and all peripheral registers
+ * as well as the processor maintain their internal states. The WWDT, WKT,
+ * and BOD can remain active to wake up the system on an interrupt.
+ */
+void Chip_PMU_DeepSleepState(LPC_PMU_T *pPMU);
+
+/**
+ * @brief Enter MCU Power down mode
+ * @param pPMU : Pointer to PMU register block
+ * @return None
+ * @note In Power-down mode, the peripherals receive no internal clocks.
+ * The internal SRAM memory and all peripheral registers as well as the
+ * processor maintain their internal states. The flash memory is powered
+ * down. The WWDT, WKT, and BOD can remain active to wake up the system
+ * on an interrupt.
+ */
+void Chip_PMU_PowerDownState(LPC_PMU_T *pPMU);
+#endif
+
+/**
+ * @brief Enter MCU Deep Power down mode
+ * @param pPMU : Pointer to PMU register block
+ * @return None
+ * @note For maximal power savings, the entire system is shut down
+ * except for the general purpose registers in the PMU and the self
+ * wake-up timer. Only the general purpose registers in the PMU maintain
+ * their internal states. The part can wake up on a pulse on the WAKEUP
+ * pin or when the self wake-up timer times out. On wake-up, the part
+ * reboots.
+ */
+void Chip_PMU_DeepPowerDownState(LPC_PMU_T *pPMU);
+
+/**
+ * @brief Place the MCU in a low power state
+ * @param pPMU : Pointer to PMU register block
+ * @param SleepMode : Sleep mode
+ * @return None
+ */
+void Chip_PMU_Sleep(LPC_PMU_T *pPMU, CHIP_PMU_MCUPOWER_T SleepMode);
+
+/**
+ * @brief Returns sleep/power-down flags
+ * @param pPMU : Pointer to PMU register block
+ * @return Or'ed values of PMU_PCON_SLEEPFLAG and PMU_PCON_DPDFLAG
+ * @note These indicate that the PMU is setup for entry into a low
+ * power state on the next WFI() instruction.
+ */
+STATIC INLINE uint32_t Chip_PMU_GetSleepFlags(LPC_PMU_T *pPMU)
+{
+ return (pPMU->PCON & (PMU_PCON_SLEEPFLAG | PMU_PCON_DPDFLAG));
+}
+
+/**
+ * @brief Clears sleep/power-down flags
+ * @param pPMU : Pointer to PMU register block
+ * @param flags : Or'ed value of PMU_PCON_SLEEPFLAG and PMU_PCON_DPDFLAG
+ * @return Nothing
+ * @note Use this function to clear a low power state prior to calling
+ * WFI().
+ */
+STATIC INLINE void Chip_PMU_ClearSleepFlags(LPC_PMU_T *pPMU, uint32_t flags)
+{
+ pPMU->PCON &= ~flags;
+}
+
+#endif /* defined(CHIP_LPC11AXX) || defined(CHIP_LPC11CXX) || ... */
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __PMU_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/ring_buffer.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,188 @@
+/*
+ * @brief Common ring buffer support functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __RING_BUFFER_H_
+#define __RING_BUFFER_H_
+
+#include "lpc_types.h"
+
+/** @defgroup Ring_Buffer CHIP: Simple ring buffer implementation
+ * @ingroup CHIP_Common
+ * @{
+ */
+
+/**
+ * @brief Ring buffer structure
+ */
+typedef struct {
+ void *data;
+ int count;
+ int itemSz;
+ uint32_t head;
+ uint32_t tail;
+} RINGBUFF_T;
+
+/**
+ * @def RB_VHEAD(rb)
+ * volatile typecasted head index
+ */
+#define RB_VHEAD(rb) (*(volatile uint32_t *) &(rb)->head)
+
+/**
+ * @def RB_VTAIL(rb)
+ * volatile typecasted tail index
+ */
+#define RB_VTAIL(rb) (*(volatile uint32_t *) &(rb)->tail)
+
+/**
+ * @brief Initialize ring buffer
+ * @param RingBuff : Pointer to ring buffer to initialize
+ * @param buffer : Pointer to buffer to associate with RingBuff
+ * @param itemSize : Size of each buffer item size
+ * @param count : Size of ring buffer
+ * @note Memory pointed by @a buffer must have correct alignment of
+ * @a itemSize, and @a count must be a power of 2 and must at
+ * least be 2 or greater.
+ * @return Nothing
+ */
+int RingBuffer_Init(RINGBUFF_T *RingBuff, void *buffer, int itemSize, int count);
+
+/**
+ * @brief Resets the ring buffer to empty
+ * @param RingBuff : Pointer to ring buffer
+ * @return Nothing
+ */
+STATIC INLINE void RingBuffer_Flush(RINGBUFF_T *RingBuff)
+{
+ RingBuff->head = RingBuff->tail = 0;
+}
+
+/**
+ * @brief Return size the ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @return Size of the ring buffer in bytes
+ */
+STATIC INLINE int RingBuffer_GetSize(RINGBUFF_T *RingBuff)
+{
+ return RingBuff->count;
+}
+
+/**
+ * @brief Return number of items in the ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @return Number of items in the ring buffer
+ */
+STATIC INLINE int RingBuffer_GetCount(RINGBUFF_T *RingBuff)
+{
+ return RB_VHEAD(RingBuff) - RB_VTAIL(RingBuff);
+}
+
+/**
+ * @brief Return number of free items in the ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @return Number of free items in the ring buffer
+ */
+STATIC INLINE int RingBuffer_GetFree(RINGBUFF_T *RingBuff)
+{
+ return RingBuff->count - RingBuffer_GetCount(RingBuff);
+}
+
+/**
+ * @brief Return number of items in the ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @return 1 if the ring buffer is full, otherwise 0
+ */
+STATIC INLINE int RingBuffer_IsFull(RINGBUFF_T *RingBuff)
+{
+ return (RingBuffer_GetCount(RingBuff) >= RingBuff->count);
+}
+
+/**
+ * @brief Return empty status of ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @return 1 if the ring buffer is empty, otherwise 0
+ */
+STATIC INLINE int RingBuffer_IsEmpty(RINGBUFF_T *RingBuff)
+{
+ return RB_VHEAD(RingBuff) == RB_VTAIL(RingBuff);
+}
+
+/**
+ * @brief Insert a single item into ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @param data : pointer to item
+ * @return 1 when successfully inserted,
+ * 0 on error (Buffer not initialized using
+ * RingBuffer_Init() or attempted to insert
+ * when buffer is full)
+ */
+int RingBuffer_Insert(RINGBUFF_T *RingBuff, const void *data);
+
+/**
+ * @brief Insert an array of items into ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @param data : Pointer to first element of the item array
+ * @param num : Number of items in the array
+ * @return number of items successfully inserted,
+ * 0 on error (Buffer not initialized using
+ * RingBuffer_Init() or attempted to insert
+ * when buffer is full)
+ */
+int RingBuffer_InsertMult(RINGBUFF_T *RingBuff, const void *data, int num);
+
+/**
+ * @brief Pop an item from the ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @param data : Pointer to memory where popped item be stored
+ * @return 1 when item popped successfuly onto @a data,
+ * 0 When error (Buffer not initialized using
+ * RingBuffer_Init() or attempted to pop item when
+ * the buffer is empty)
+ */
+int RingBuffer_Pop(RINGBUFF_T *RingBuff, void *data);
+
+/**
+ * @brief Pop an array of items from the ring buffer
+ * @param RingBuff : Pointer to ring buffer
+ * @param data : Pointer to memory where popped items be stored
+ * @param num : Max number of items array @a data can hold
+ * @return Number of items popped onto @a data,
+ * 0 on error (Buffer not initialized using RingBuffer_Init()
+ * or attempted to pop when the buffer is empty)
+ */
+int RingBuffer_PopMult(RINGBUFF_T *RingBuff, void *data, int num);
+
+
+/**
+ * @}
+ */
+
+#endif /* __RING_BUFFER_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/romapi_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,78 @@
+/*
+ * @brief LPC11xx ROM API declarations and functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __ROMAPI_11XX_H_
+#define __ROMAPI_11XX_H_
+
+#include "error.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup ROMAPI_11XX CHIP: LPC11XX ROM API declarations and functions
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief LPC11XX High level ROM API structure
+ */
+typedef struct {
+ const uint32_t usbdApiBase; /*!< USBD API function table base address */
+ const uint32_t reserved0; /*!< Reserved */
+ const uint32_t candApiBase; /*!< CAN API function table base address */
+ const uint32_t pwrApiBase; /*!< Power API function table base address */
+ const uint32_t reserved1; /*!< Reserved */
+ const uint32_t reserved2; /*!< Reserved */
+ const uint32_t reserved3; /*!< Reserved */
+ const uint32_t reserved4; /*!< Reserved */
+} LPC_ROM_API_T;
+
+/**
+ * @brief LPC11XX IAP_ENTRY API function type
+ */
+typedef void (*IAP_ENTRY_T)(unsigned int[], unsigned int[]);
+
+static INLINE void iap_entry(unsigned int cmd_param[], unsigned int status_result[])
+{
+ ((IAP_ENTRY_T) IAP_ENTRY_LOCATION)(cmd_param, status_result);
+}
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ROMAPI_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/ssp_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,571 @@
+/*
+ * @brief LPC11xx SSP Registers and control functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __SSP_11XX_H_
+#define __SSP_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup SSP_11XX CHIP: LPC11xx SSP register block and driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief SSP register block structure
+ */
+typedef struct { /*!< SSPn Structure */
+ __IO uint32_t CR0; /*!< Control Register 0. Selects the serial clock rate, bus type, and data size. */
+ __IO uint32_t CR1; /*!< Control Register 1. Selects master/slave and other modes. */
+ __IO uint32_t DR; /*!< Data Register. Writes fill the transmit FIFO, and reads empty the receive FIFO. */
+ __I uint32_t SR; /*!< Status Register */
+ __IO uint32_t CPSR; /*!< Clock Prescale Register */
+ __IO uint32_t IMSC; /*!< Interrupt Mask Set and Clear Register */
+ __I uint32_t RIS; /*!< Raw Interrupt Status Register */
+ __I uint32_t MIS; /*!< Masked Interrupt Status Register */
+ __O uint32_t ICR; /*!< SSPICR Interrupt Clear Register */
+} LPC_SSP_T;
+
+/**
+ * Macro defines for CR0 register
+ */
+
+/** SSP data size select, must be 4 bits to 16 bits */
+#define SSP_CR0_DSS(n) ((uint32_t) ((n) & 0xF))
+/** SSP control 0 Motorola SPI mode */
+#define SSP_CR0_FRF_SPI ((uint32_t) (0 << 4))
+/** SSP control 0 TI synchronous serial mode */
+#define SSP_CR0_FRF_TI ((uint32_t) (1 << 4))
+/** SSP control 0 National Micro-wire mode */
+#define SSP_CR0_FRF_MICROWIRE ((uint32_t) (2 << 4))
+/** SPI clock polarity bit (used in SPI mode only), (1) = maintains the
+ bus clock high between frames, (0) = low */
+#define SSP_CR0_CPOL_LO ((uint32_t) (0))
+#define SSP_CR0_CPOL_HI ((uint32_t) (1 << 6))
+/** SPI clock out phase bit (used in SPI mode only), (1) = captures data
+ on the second clock transition of the frame, (0) = first */
+#define SSP_CR0_CPHA_FIRST ((uint32_t) (0))
+#define SSP_CR0_CPHA_SECOND ((uint32_t) (1 << 7))
+/** SSP serial clock rate value load macro, divider rate is
+ PERIPH_CLK / (cpsr * (SCR + 1)) */
+#define SSP_CR0_SCR(n) ((uint32_t) ((n & 0xFF) << 8))
+/** SSP CR0 bit mask */
+#define SSP_CR0_BITMASK ((uint32_t) (0xFFFF))
+/** SSP CR0 bit mask */
+#define SSP_CR0_BITMASK ((uint32_t) (0xFFFF))
+/** SSP serial clock rate value load macro, divider rate is
+ PERIPH_CLK / (cpsr * (SCR + 1)) */
+#define SSP_CR0_SCR(n) ((uint32_t) ((n & 0xFF) << 8))
+
+/**
+ * Macro defines for CR1 register
+ */
+
+/** SSP control 1 loopback mode enable bit */
+#define SSP_CR1_LBM_EN ((uint32_t) (1 << 0))
+/** SSP control 1 enable bit */
+#define SSP_CR1_SSP_EN ((uint32_t) (1 << 1))
+/** SSP control 1 slave enable */
+#define SSP_CR1_SLAVE_EN ((uint32_t) (1 << 2))
+#define SSP_CR1_MASTER_EN ((uint32_t) (0))
+/** SSP control 1 slave out disable bit, disables transmit line in slave
+ mode */
+#define SSP_CR1_SO_DISABLE ((uint32_t) (1 << 3))
+/** SSP CR1 bit mask */
+#define SSP_CR1_BITMASK ((uint32_t) (0x0F))
+
+/** SSP CPSR bit mask */
+#define SSP_CPSR_BITMASK ((uint32_t) (0xFF))
+/**
+ * Macro defines for DR register
+ */
+
+/** SSP data bit mask */
+#define SSP_DR_BITMASK(n) ((n) & 0xFFFF)
+
+/**
+ * Macro defines for SR register
+ */
+
+/** SSP SR bit mask */
+#define SSP_SR_BITMASK ((uint32_t) (0x1F))
+
+/** ICR bit mask */
+#define SSP_ICR_BITMASK ((uint32_t) (0x03))
+
+/**
+ * @brief SSP Type of Status
+ */
+typedef enum _SSP_STATUS {
+ SSP_STAT_TFE = ((uint32_t)(1 << 0)),/**< TX FIFO Empty */
+ SSP_STAT_TNF = ((uint32_t)(1 << 1)),/**< TX FIFO not full */
+ SSP_STAT_RNE = ((uint32_t)(1 << 2)),/**< RX FIFO not empty */
+ SSP_STAT_RFF = ((uint32_t)(1 << 3)),/**< RX FIFO full */
+ SSP_STAT_BSY = ((uint32_t)(1 << 4)),/**< SSP Busy */
+} SSP_STATUS_T;
+
+/**
+ * @brief SSP Type of Interrupt Mask
+ */
+typedef enum _SSP_INTMASK {
+ SSP_RORIM = ((uint32_t)(1 << 0)), /**< Overun */
+ SSP_RTIM = ((uint32_t)(1 << 1)),/**< TimeOut */
+ SSP_RXIM = ((uint32_t)(1 << 2)),/**< Rx FIFO is at least half full */
+ SSP_TXIM = ((uint32_t)(1 << 3)),/**< Tx FIFO is at least half empty */
+ SSP_INT_MASK_BITMASK = ((uint32_t)(0xF)),
+} SSP_INTMASK_T;
+
+/**
+ * @brief SSP Type of Mask Interrupt Status
+ */
+typedef enum _SSP_MASKINTSTATUS {
+ SSP_RORMIS = ((uint32_t)(1 << 0)), /**< Overun */
+ SSP_RTMIS = ((uint32_t)(1 << 1)), /**< TimeOut */
+ SSP_RXMIS = ((uint32_t)(1 << 2)), /**< Rx FIFO is at least half full */
+ SSP_TXMIS = ((uint32_t)(1 << 3)), /**< Tx FIFO is at least half empty */
+ SSP_MASK_INT_STAT_BITMASK = ((uint32_t)(0xF)),
+} SSP_MASKINTSTATUS_T;
+
+/**
+ * @brief SSP Type of Raw Interrupt Status
+ */
+typedef enum _SSP_RAWINTSTATUS {
+ SSP_RORRIS = ((uint32_t)(1 << 0)), /**< Overun */
+ SSP_RTRIS = ((uint32_t)(1 << 1)), /**< TimeOut */
+ SSP_RXRIS = ((uint32_t)(1 << 2)), /**< Rx FIFO is at least half full */
+ SSP_TXRIS = ((uint32_t)(1 << 3)), /**< Tx FIFO is at least half empty */
+ SSP_RAW_INT_STAT_BITMASK = ((uint32_t)(0xF)),
+} SSP_RAWINTSTATUS_T;
+
+typedef enum _SSP_INTCLEAR {
+ SSP_RORIC = 0x0,
+ SSP_RTIC = 0x1,
+ SSP_INT_CLEAR_BITMASK = 0x3,
+} SSP_INTCLEAR_T;
+
+/*
+ * @brief SSP clock format
+ */
+typedef enum CHIP_SSP_CLOCK_FORMAT {
+ SSP_CLOCK_CPHA0_CPOL0 = (0 << 6), /**< CPHA = 0, CPOL = 0 */
+ SSP_CLOCK_CPHA0_CPOL1 = (1u << 6), /**< CPHA = 0, CPOL = 1 */
+ SSP_CLOCK_CPHA1_CPOL0 = (2u << 6), /**< CPHA = 1, CPOL = 0 */
+ SSP_CLOCK_CPHA1_CPOL1 = (3u << 6), /**< CPHA = 1, CPOL = 1 */
+ SSP_CLOCK_MODE0 = SSP_CLOCK_CPHA0_CPOL0,/**< alias */
+ SSP_CLOCK_MODE1 = SSP_CLOCK_CPHA1_CPOL0,/**< alias */
+ SSP_CLOCK_MODE2 = SSP_CLOCK_CPHA0_CPOL1,/**< alias */
+ SSP_CLOCK_MODE3 = SSP_CLOCK_CPHA1_CPOL1,/**< alias */
+} CHIP_SSP_CLOCK_MODE_T;
+
+/*
+ * @brief SSP frame format
+ */
+typedef enum CHIP_SSP_FRAME_FORMAT {
+ SSP_FRAMEFORMAT_SPI = (0 << 4), /**< Frame format: SPI */
+ CHIP_SSP_FRAME_FORMAT_TI = (1u << 4), /**< Frame format: TI SSI */
+ SSP_FRAMEFORMAT_MICROWIRE = (2u << 4), /**< Frame format: Microwire */
+} CHIP_SSP_FRAME_FORMAT_T;
+
+/*
+ * @brief Number of bits per frame
+ */
+typedef enum CHIP_SSP_BITS {
+ SSP_BITS_4 = (3u << 0), /*!< 4 bits/frame */
+ SSP_BITS_5 = (4u << 0), /*!< 5 bits/frame */
+ SSP_BITS_6 = (5u << 0), /*!< 6 bits/frame */
+ SSP_BITS_7 = (6u << 0), /*!< 7 bits/frame */
+ SSP_BITS_8 = (7u << 0), /*!< 8 bits/frame */
+ SSP_BITS_9 = (8u << 0), /*!< 9 bits/frame */
+ SSP_BITS_10 = (9u << 0), /*!< 10 bits/frame */
+ SSP_BITS_11 = (10u << 0), /*!< 11 bits/frame */
+ SSP_BITS_12 = (11u << 0), /*!< 12 bits/frame */
+ SSP_BITS_13 = (12u << 0), /*!< 13 bits/frame */
+ SSP_BITS_14 = (13u << 0), /*!< 14 bits/frame */
+ SSP_BITS_15 = (14u << 0), /*!< 15 bits/frame */
+ SSP_BITS_16 = (15u << 0), /*!< 16 bits/frame */
+} CHIP_SSP_BITS_T;
+
+/*
+ * @brief SSP config format
+ */
+typedef struct SSP_ConfigFormat {
+ CHIP_SSP_BITS_T bits; /*!< Format config: bits/frame */
+ CHIP_SSP_CLOCK_MODE_T clockMode; /*!< Format config: clock phase/polarity */
+ CHIP_SSP_FRAME_FORMAT_T frameFormat; /*!< Format config: SPI/TI/Microwire */
+} SSP_ConfigFormat;
+
+/**
+ * @brief Enable SSP operation
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SSP_Enable(LPC_SSP_T *pSSP)
+{
+ pSSP->CR1 |= SSP_CR1_SSP_EN;
+}
+
+/**
+ * @brief Disable SSP operation
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SSP_Disable(LPC_SSP_T *pSSP)
+{
+ pSSP->CR1 &= (~SSP_CR1_SSP_EN) & SSP_CR1_BITMASK;
+}
+
+/**
+ * @brief Enable loopback mode
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ * @note Serial input is taken from the serial output (MOSI or MISO) rather
+ * than the serial input pin
+ */
+STATIC INLINE void Chip_SSP_EnableLoopBack(LPC_SSP_T *pSSP)
+{
+ pSSP->CR1 |= SSP_CR1_LBM_EN;
+}
+
+/**
+ * @brief Disable loopback mode
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ * @note Serial input is taken from the serial output (MOSI or MISO) rather
+ * than the serial input pin
+ */
+STATIC INLINE void Chip_SSP_DisableLoopBack(LPC_SSP_T *pSSP)
+{
+ pSSP->CR1 &= (~SSP_CR1_LBM_EN) & SSP_CR1_BITMASK;
+}
+
+/**
+ * @brief Get the current status of SSP controller
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param Stat : Type of status, should be :
+ * - SSP_STAT_TFE
+ * - SSP_STAT_TNF
+ * - SSP_STAT_RNE
+ * - SSP_STAT_RFF
+ * - SSP_STAT_BSY
+ * @return SSP controller status, SET or RESET
+ */
+STATIC INLINE FlagStatus Chip_SSP_GetStatus(LPC_SSP_T *pSSP, SSP_STATUS_T Stat)
+{
+ return (pSSP->SR & Stat) ? SET : RESET;
+}
+
+/**
+ * @brief Get the masked interrupt status
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return SSP Masked Interrupt Status Register value
+ * @note The return value contains a 1 for each interrupt condition that is asserted and enabled (masked)
+ */
+STATIC INLINE uint32_t Chip_SSP_GetIntStatus(LPC_SSP_T *pSSP)
+{
+ return pSSP->MIS;
+}
+
+/**
+ * @brief Get the raw interrupt status
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param RawInt : Interrupt condition to be get status, shoud be :
+ * - SSP_RORRIS
+ * - SSP_RTRIS
+ * - SSP_RXRIS
+ * - SSP_TXRIS
+ * @return Raw interrupt status corresponding to interrupt condition , SET or RESET
+ * @note Get the status of each interrupt condition ,regardless of whether or not the interrupt is enabled
+ */
+STATIC INLINE IntStatus Chip_SSP_GetRawIntStatus(LPC_SSP_T *pSSP, SSP_RAWINTSTATUS_T RawInt)
+{
+ return (pSSP->RIS & RawInt) ? SET : RESET;
+}
+
+/**
+ * @brief Get the number of bits transferred in each frame
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return the number of bits transferred in each frame minus one
+ * @note The return value is 0x03 -> 0xF corresponding to 4bit -> 16bit transfer
+ */
+STATIC INLINE uint8_t Chip_SSP_GetDataSize(LPC_SSP_T *pSSP)
+{
+ return SSP_CR0_DSS(pSSP->CR0);
+}
+
+/**
+ * @brief Clear the corresponding interrupt condition(s) in the SSP controller
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param IntClear: Type of cleared interrupt, should be :
+ * - SSP_RORIC
+ * - SSP_RTIC
+ * @return Nothing
+ * @note Software can clear one or more interrupt condition(s) in the SSP controller
+ */
+STATIC INLINE void Chip_SSP_ClearIntPending(LPC_SSP_T *pSSP, SSP_INTCLEAR_T IntClear)
+{
+ pSSP->ICR = IntClear;
+}
+
+/**
+ * @brief Enable interrupt for the SSP
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SSP_Int_Enable(LPC_SSP_T *pSSP)
+{
+ pSSP->IMSC |= SSP_TXIM;
+}
+
+/**
+ * @brief Disable interrupt for the SSP
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SSP_Int_Disable(LPC_SSP_T *pSSP)
+{
+ pSSP->IMSC &= (~SSP_TXIM);
+}
+
+/**
+ * @brief Get received SSP data
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return SSP 16-bit data received
+ */
+STATIC INLINE uint16_t Chip_SSP_ReceiveFrame(LPC_SSP_T *pSSP)
+{
+ return (uint16_t) (SSP_DR_BITMASK(pSSP->DR));
+}
+
+/**
+ * @brief Send SSP 16-bit data
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param tx_data : SSP 16-bit data to be transmited
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SSP_SendFrame(LPC_SSP_T *pSSP, uint16_t tx_data)
+{
+ pSSP->DR = SSP_DR_BITMASK(tx_data);
+}
+
+/**
+ * @brief Set up output clocks per bit for SSP bus
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param clk_rate fs: The number of prescaler-output clocks per bit on the bus, minus one
+ * @param prescale : The factor by which the Prescaler divides the SSP peripheral clock PCLK
+ * @return Nothing
+ * @note The bit frequency is PCLK / (prescale x[clk_rate+1])
+ */
+void Chip_SSP_SetClockRate(LPC_SSP_T *pSSP, uint32_t clk_rate, uint32_t prescale);
+
+/**
+ * @brief Set up the SSP frame format
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param bits : The number of bits transferred in each frame, should be SSP_BITS_4 to SSP_BITS_16
+ * @param frameFormat : Frame format, should be :
+ * - SSP_FRAMEFORMAT_SPI
+ * - SSP_FRAME_FORMAT_TI
+ * - SSP_FRAMEFORMAT_MICROWIRE
+ * @param clockMode : Select Clock polarity and Clock phase, should be :
+ * - SSP_CLOCK_CPHA0_CPOL0
+ * - SSP_CLOCK_CPHA0_CPOL1
+ * - SSP_CLOCK_CPHA1_CPOL0
+ * - SSP_CLOCK_CPHA1_CPOL1
+ * @return Nothing
+ * @note Note: The clockFormat is only used in SPI mode
+ */
+STATIC INLINE void Chip_SSP_SetFormat(LPC_SSP_T *pSSP, uint32_t bits, uint32_t frameFormat, uint32_t clockMode)
+{
+ pSSP->CR0 = (pSSP->CR0 & ~0xFF) | bits | frameFormat | clockMode;
+}
+
+/**
+ * @brief Set the SSP working as master or slave mode
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @param mode : Operating mode, should be
+ * - SSP_MODE_MASTER
+ * - SSP_MODE_SLAVE
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SSP_Set_Mode(LPC_SSP_T *pSSP, uint32_t mode)
+{
+ pSSP->CR1 = (pSSP->CR1 & ~(1 << 2)) | mode;
+}
+
+/*
+ * @brief SSP mode
+ */
+typedef enum CHIP_SSP_MODE {
+ SSP_MODE_MASTER = (0 << 2), /**< Master mode */
+ SSP_MODE_SLAVE = (1u << 2), /**< Slave mode */
+} CHIP_SSP_MODE_T;
+
+/*
+ * @brief SPI address
+ */
+typedef struct {
+ uint8_t port; /*!< Port Number */
+ uint8_t pin; /*!< Pin number */
+} SPI_Address_t;
+
+/*
+ * @brief SSP data setup structure
+ */
+typedef struct {
+ void *tx_data; /*!< Pointer to transmit data */
+ uint32_t tx_cnt; /*!< Transmit counter */
+ void *rx_data; /*!< Pointer to transmit data */
+ uint32_t rx_cnt; /*!< Receive counter */
+ uint32_t length; /*!< Length of transfer data */
+} Chip_SSP_DATA_SETUP_T;
+
+/** SSP configuration parameter defines */
+/** Clock phase control bit */
+#define SSP_CPHA_FIRST SSP_CR0_CPHA_FIRST
+#define SSP_CPHA_SECOND SSP_CR0_CPHA_SECOND
+
+/** Clock polarity control bit */
+/* There's no bug here!!!
+ * - If bit[6] in SSPnCR0 is 0: SSP controller maintains the bus clock low between frames.
+ * That means the active clock is in HI state.
+ * - If bit[6] in SSPnCR0 is 1 (SSP_CR0_CPOL_HI): SSP controller maintains the bus clock
+ * high between frames. That means the active clock is in LO state.
+ */
+#define SSP_CPOL_HI SSP_CR0_CPOL_LO
+#define SSP_CPOL_LO SSP_CR0_CPOL_HI
+
+/** SSP master mode enable */
+#define SSP_SLAVE_MODE SSP_CR1_SLAVE_EN
+#define SSP_MASTER_MODE SSP_CR1_MASTER_EN
+
+/**
+ * @brief Clean all data in RX FIFO of SSP
+ * @param pSSP : The base SSP peripheral on the chip
+ * @return Nothing
+ */
+void Chip_SSP_Int_FlushData(LPC_SSP_T *pSSP);
+
+/**
+ * @brief SSP Interrupt Read/Write with 8-bit frame width
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param xf_setup : Pointer to a SSP_DATA_SETUP_T structure that contains specified
+ * information about transmit/receive data configuration
+ * @return SUCCESS or ERROR
+ */
+Status Chip_SSP_Int_RWFrames8Bits(LPC_SSP_T *pSSP, Chip_SSP_DATA_SETUP_T *xf_setup);
+
+/**
+ * @brief SSP Interrupt Read/Write with 16-bit frame width
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param xf_setup : Pointer to a SSP_DATA_SETUP_T structure that contains specified
+ * information about transmit/receive data configuration
+ * @return SUCCESS or ERROR
+ */
+Status Chip_SSP_Int_RWFrames16Bits(LPC_SSP_T *pSSP, Chip_SSP_DATA_SETUP_T *xf_setup);
+
+/**
+ * @brief SSP Polling Read/Write in blocking mode
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param xf_setup : Pointer to a SSP_DATA_SETUP_T structure that contains specified
+ * information about transmit/receive data configuration
+ * @return Actual data length has been transferred
+ * @note
+ * This function can be used in both master and slave mode. It starts with writing phase and after that,
+ * a reading phase is generated to read any data available in RX_FIFO. All needed information is prepared
+ * through xf_setup param.
+ */
+uint32_t Chip_SSP_RWFrames_Blocking(LPC_SSP_T *pSSP, Chip_SSP_DATA_SETUP_T *xf_setup);
+
+/**
+ * @brief SSP Polling Write in blocking mode
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param buffer : Buffer address
+ * @param buffer_len : Buffer length
+ * @return Actual data length has been transferred
+ * @note
+ * This function can be used in both master and slave mode. First, a writing operation will send
+ * the needed data. After that, a dummy reading operation is generated to clear data buffer
+ */
+uint32_t Chip_SSP_WriteFrames_Blocking(LPC_SSP_T *pSSP, uint8_t *buffer, uint32_t buffer_len);
+
+/**
+ * @brief SSP Polling Read in blocking mode
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param buffer : Buffer address
+ * @param buffer_len : The length of buffer
+ * @return Actual data length has been transferred
+ * @note
+ * This function can be used in both master and slave mode. First, a dummy writing operation is generated
+ * to clear data buffer. After that, a reading operation will receive the needed data
+ */
+uint32_t Chip_SSP_ReadFrames_Blocking(LPC_SSP_T *pSSP, uint8_t *buffer, uint32_t buffer_len);
+
+/**
+ * @brief Initialize the SSP
+ * @param pSSP : The base SSP peripheral on the chip
+ * @return Nothing
+ */
+void Chip_SSP_Init(LPC_SSP_T *pSSP);
+
+/**
+ * @brief Deinitialise the SSP
+ * @param pSSP : The base of SSP peripheral on the chip
+ * @return Nothing
+ * @note The SSP controller is disabled
+ */
+void Chip_SSP_DeInit(LPC_SSP_T *pSSP);
+
+/**
+ * @brief Set the SSP operating modes, master or slave
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param master : 1 to set master, 0 to set slave
+ * @return Nothing
+ */
+void Chip_SSP_SetMaster(LPC_SSP_T *pSSP, bool master);
+
+/**
+ * @brief Set the clock frequency for SSP interface
+ * @param pSSP : The base SSP peripheral on the chip
+ * @param bitRate : The SSP bit rate
+ * @return Nothing
+ */
+void Chip_SSP_SetBitRate(LPC_SSP_T *pSSP, uint32_t bitRate);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __SSP_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/sys_config.h Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,33 @@ +/* + * @note + * Copyright(C) NXP Semiconductors, 2012 + * All rights reserved. + * + * @par + * Software that is described herein is for illustrative purposes only + * which provides customers with programming information regarding the + * LPC products. This software is supplied "AS IS" without any warranties of + * any kind, and NXP Semiconductors and its licensor disclaim any and + * all warranties, express or implied, including all implied warranties of + * merchantability, fitness for a particular purpose and non-infringement of + * intellectual property rights. NXP Semiconductors assumes no responsibility + * or liability for the use of the software, conveys no license or rights under any + * patent, copyright, mask work right, or any other intellectual property rights in + * or to any products. NXP Semiconductors reserves the right to make changes + * in the software without notification. NXP Semiconductors also makes no + * representation or warranty that such application will be suitable for the + * specified use without further testing or modification. + * + * @par + * Permission to use, copy, modify, and distribute this software and its + * documentation is hereby granted, under NXP Semiconductors' and its + * licensor's relevant copyrights in the software, without fee, provided that it + * is used in conjunction with NXP Semiconductors microcontrollers. This + * copyright, permission, and disclaimer notice must appear in all copies of + * this code. + */ + +#ifndef __SYS_CONFIG_H_ +#define __SYS_CONFIG_H_ + +#endif /* __SYS_CONFIG_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/sysctl_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,687 @@
+/*
+ * @brief LPC11XX System Control registers and control functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __SYSCTL_11XX_H_
+#define __SYSCTL_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup SYSCTL_11XX CHIP: LPC11xx System Control block driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief LPC11XX System Control block structure
+ */
+typedef struct { /*!< SYSCTL Structure */
+ __IO uint32_t SYSMEMREMAP; /*!< System Memory remap register */
+ __IO uint32_t PRESETCTRL; /*!< Peripheral reset Control register */
+ __IO uint32_t SYSPLLCTRL; /*!< System PLL control register */
+ __I uint32_t SYSPLLSTAT; /*!< System PLL status register */
+ __IO uint32_t USBPLLCTRL; /*!< USB PLL control register, LPC11UXX only*/
+ __I uint32_t USBPLLSTAT; /*!< USB PLL status register, LPC11UXX only */
+ __I uint32_t RESERVED1[2];
+ __IO uint32_t SYSOSCCTRL; /*!< System Oscillator control register */
+ __IO uint32_t WDTOSCCTRL; /*!< Watchdog Oscillator control register */
+ __IO uint32_t IRCCTRL; /*!< IRC control register, not on LPC11UXX and LPC11EXX */
+ __IO uint32_t LFOSCCTRL; /*!< LF oscillator control, LPC11AXX only */
+ __IO uint32_t SYSRSTSTAT; /*!< System Reset Status register */
+ __I uint32_t RESERVED2[3];
+ __IO uint32_t SYSPLLCLKSEL; /*!< System PLL clock source select register */
+ __IO uint32_t SYSPLLCLKUEN; /*!< System PLL clock source update enable register*/
+ __IO uint32_t USBPLLCLKSEL; /*!< USB PLL clock source select register, LPC11UXX only */
+ __IO uint32_t USBPLLCLKUEN; /*!< USB PLL clock source update enable register, LPC11UXX only */
+ __I uint32_t RESERVED3[8];
+ __IO uint32_t MAINCLKSEL; /*!< Main clock source select register */
+ __IO uint32_t MAINCLKUEN; /*!< Main clock source update enable register */
+ __IO uint32_t SYSAHBCLKDIV; /*!< System Clock divider register */
+ __I uint32_t RESERVED4;
+ __IO uint32_t SYSAHBCLKCTRL; /*!< System clock control register */
+ __I uint32_t RESERVED5[4];
+ __IO uint32_t SSP0CLKDIV; /*!< SSP0 clock divider register */
+ __IO uint32_t USARTCLKDIV; /*!< UART clock divider register */
+ __IO uint32_t SSP1CLKDIV; /*!< SSP1 clock divider register, not on CHIP_LPC110X, CHIP_LPC11XXLV */
+ __I uint32_t RESERVED6[8];
+ __IO uint32_t USBCLKSEL; /*!< USB clock source select register, LPC11UXX only */
+ __IO uint32_t USBCLKUEN; /*!< USB clock source update enable register, LPC11UXX only */
+ __IO uint32_t USBCLKDIV; /*!< USB clock source divider register, LPC11UXX only */
+ __I uint32_t RESERVED7;
+ __IO uint32_t WDTCLKSEL; /*!< WDT clock source select register, some parts only */
+ __IO uint32_t WDTCLKUEN; /*!< WDT clock source update enable register, some parts only */
+ __IO uint32_t WDTCLKDIV; /*!< WDT clock divider register, some parts only */
+ __I uint32_t RESERVED8;
+ __IO uint32_t CLKOUTSEL; /*!< Clock out source select register, not on LPC1102/04 */
+ __IO uint32_t CLKOUTUEN; /*!< Clock out source update enable register, not on LPC1102/04 */
+ __IO uint32_t CLKOUTDIV; /*!< Clock out divider register, not on LPC1102/04 */
+ __I uint32_t RESERVED9[5];
+ __I uint32_t PIOPORCAP[2];/*!< POR captured PIO status registers, index 1 on LPC1102/04 */
+ __I uint32_t RESERVED10[18];
+ __IO uint32_t BODCTRL; /*!< Brown Out Detect register */
+ __IO uint32_t SYSTCKCAL; /*!< System tick counter calibration register */
+ __I uint32_t RESERVED11[6];
+ __IO uint32_t IRQLATENCY; /*!< IRQ delay register, on LPC11UXX and LPC11EXX only */
+ __IO uint32_t NMISRC; /*!< NMI source control register,some parts only */
+ __IO uint32_t PINTSEL[8]; /*!< GPIO pin interrupt select register 0-7, not on CHIP_LPC110X, CHIP_LPC11XXLV, CHIP_LPC11CXX */
+ __IO uint32_t USBCLKCTRL; /*!< USB clock control register, LPC11UXX only */
+ __I uint32_t USBCLKST; /*!< USB clock status register, LPC11UXX only */
+ __I uint32_t RESERVED12[24];
+ __IO uint32_t STARTAPRP0; /*!< Start loigc 0 interrupt wake up enable register 0, on CHIP_LPC110X, CHIP_LPC11XXLV, CHIP_LPC11CXX */
+ __IO uint32_t STARTERP0; /*!< Start loigc signal enable register 0, not on LPC11AXX */
+ __IO uint32_t STARTRSRP0CLR; /*!< Start loigc reset register 0, on CHIP_LPC110X, CHIP_LPC11XXLV, CHIP_LPC11CXX */
+ __IO uint32_t STARTSRP0; /*!< Start loigc status register 0, on CHIP_LPC110X, CHIP_LPC11XXLV, CHIP_LPC11CXX */
+ __I uint32_t RESERVED13;
+ __IO uint32_t STARTERP1; /*!< Start logic 1 interrupt wake up enable register 1, on LPC11UXX and LPC11EXX only */
+ __I uint32_t RESERVED14[6];
+ __IO uint32_t PDSLEEPCFG; /*!< Power down states in deep sleep mode register, not on LPC11AXX */
+ __IO uint32_t PDWAKECFG; /*!< Power down states in wake up from deep sleep register, not on LPC11AXX */
+ __IO uint32_t PDRUNCFG; /*!< Power configuration register*/
+ __I uint32_t RESERVED15[110];
+ __I uint32_t DEVICEID; /*!< Device ID register */
+} LPC_SYSCTL_T;
+
+/**
+ * System memory remap modes used to remap interrupt vectors
+ */
+typedef enum CHIP_SYSCTL_BOOT_MODE_REMAP {
+ REMAP_BOOT_LOADER_MODE, /*!< Interrupt vectors are re-mapped to Boot ROM */
+ REMAP_USER_RAM_MODE, /*!< Interrupt vectors are re-mapped to Static RAM */
+ REMAP_USER_FLASH_MODE /*!< Interrupt vectors are not re-mapped and reside in Flash */
+} CHIP_SYSCTL_BOOT_MODE_REMAP_T;
+
+/**
+ * @brief Re-map interrupt vectors
+ * @param remap : system memory map value
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_Map(CHIP_SYSCTL_BOOT_MODE_REMAP_T remap)
+{
+ LPC_SYSCTL->SYSMEMREMAP = (uint32_t) remap;
+}
+
+/**
+ * Peripheral reset identifiers, not available on all devices
+ */
+typedef enum {
+ RESET_SSP0, /*!< SSP0 reset control */
+ RESET_I2C0, /*!< I2C0 reset control */
+ RESET_SSP1, /*!< SSP1 reset control */
+#if !defined(CHIP_LPC1125)
+ RESET_CAN0, /*!< CAN0 reset control */
+ RESET_UART0, /*!< UART0 reset control */
+ RESET_TIMER0_16, /*!< 16-bit Timer 0 reset control */
+ RESET_TIMER1_16, /*!< 16-bit Timer 1 reset control */
+ RESET_TIMER0_32, /*!< 32-bit Timer 0 reset control */
+ RESET_TIMER1_32, /*!< 32-bit Timer 1 reset control */
+ RESET_ACMP, /*!< Analog comparator reset control */
+ RESET_DAC0, /*!< DAC reset control */
+ RESET_ADC0 /*!< ADC reset control */
+#endif
+} CHIP_SYSCTL_PERIPH_RESET_T;
+
+/**
+ * @brief Assert reset for a peripheral
+ * @param periph : Peripheral to assert reset for
+ * @return Nothing
+ * @note The peripheral will stay in reset until reset is de-asserted. Call
+ * Chip_SYSCTL_DeassertPeriphReset() to de-assert the reset.
+ */
+STATIC INLINE void Chip_SYSCTL_AssertPeriphReset(CHIP_SYSCTL_PERIPH_RESET_T periph)
+{
+ LPC_SYSCTL->PRESETCTRL &= ~(1 << (uint32_t) periph);
+}
+
+/**
+ * @brief De-assert reset for a peripheral
+ * @param periph : Peripheral to de-assert reset for
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_DeassertPeriphReset(CHIP_SYSCTL_PERIPH_RESET_T periph)
+{
+ LPC_SYSCTL->PRESETCTRL |= (1 << (uint32_t) periph);
+}
+
+/**
+ * @brief Resets a peripheral
+ * @param periph : Peripheral to reset
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_PeriphReset(CHIP_SYSCTL_PERIPH_RESET_T periph)
+{
+ Chip_SYSCTL_AssertPeriphReset(periph);
+ Chip_SYSCTL_DeassertPeriphReset(periph);
+}
+
+/**
+ * System reset status
+ */
+#define SYSCTL_RST_POR (1 << 0) /*!< POR reset status */
+#define SYSCTL_RST_EXTRST (1 << 1) /*!< External reset status */
+#define SYSCTL_RST_WDT (1 << 2) /*!< Watchdog reset status */
+#define SYSCTL_RST_BOD (1 << 3) /*!< Brown-out detect reset status */
+#define SYSCTL_RST_SYSRST (1 << 4) /*!< software system reset status */
+
+/**
+ * Non-Maskable Interrupt Enable/Disable value
+ */
+#define SYSCTL_NMISRC_ENABLE ((uint32_t) 1 << 31) /*!< Enable the Non-Maskable Interrupt (NMI) source */
+
+/**
+ * @brief Get system reset status
+ * @return An Or'ed value of SYSCTL_RST_*
+ * @note This function returns the detected reset source(s).
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetSystemRSTStatus(void)
+{
+ return LPC_SYSCTL->SYSRSTSTAT;
+}
+
+/**
+ * @brief Clear system reset status
+ * @param reset : An Or'ed value of SYSCTL_RST_* status to clear
+ * @return Nothing
+ * @note This function returns the detected reset source(s).
+ */
+STATIC INLINE void Chip_SYSCTL_ClearSystemRSTStatus(uint32_t reset)
+{
+ LPC_SYSCTL->SYSRSTSTAT = reset;
+}
+
+/**
+ * @brief Read POR captured PIO status
+ * @param index : POR register index, 0 or 1
+ * @return captured POR PIO status
+ * @note Some devices only support index 0.
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetPORPIOStatus(int index)
+{
+ return LPC_SYSCTL->PIOPORCAP[index];
+}
+
+/**
+ * Brown-out detector reset level
+ */
+typedef enum CHIP_SYSCTL_BODRSTLVL {
+#if defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC1125)
+ SYSCTL_BODRSTLVL_1_46V, /*!< Brown-out reset at 1.46v */
+#else
+ SYSCTL_BODRSTLVL_RESERVED1, /*!< Only possible value for LPC11A/02/XXLV */
+#endif
+#if defined(CHIP_LPC11XXLV)
+ SYSCTL_BODRSTLVL_RESERVED1,
+ SYSCTL_BODRSTLVL_RESERVED2,
+ SYSCTL_BODRSTLVL_RESERVED3,
+#elif defined(CHIP_LPC11AXX)
+ SYSCTL_BODRSTLVL_RESERVED2,
+ SYSCTL_BODRSTLVL_2_52V, /*!< Brown-out reset at 2.52v */
+ SYSCTL_BODRSTLVL_2_80V, /*!< Brown-out reset at 2.80v */
+#else
+ SYSCTL_BODRSTLVL_2_06V, /*!< Brown-out reset at 2.06v */
+ SYSCTL_BODRSTLVL_2_35V, /*!< Brown-out reset at 2.35v */
+ SYSCTL_BODRSTLVL_2_63V, /*!< Brown-out reset at 2.63v */
+#endif
+} CHIP_SYSCTL_BODRSTLVL_T;
+
+/**
+ * Brown-out detector interrupt level
+ */
+typedef enum CHIP_SYSCTL_BODRINTVAL {
+ SYSCTL_BODINTVAL_RESERVED1,
+#if defined(CHIP_LPC110X) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC1125)
+ SYSCTL_BODINTVAL_2_22V, /*!< Brown-out interrupt at 2.22v */
+ SYSCTL_BODINTVAL_2_52V, /*!< Brown-out interrupt at 2.52v */
+ SYSCTL_BODINTVAL_2_80V, /*!< Brown-out interrupt at 2.8v */
+#endif
+} CHIP_SYSCTL_BODRINTVAL_T;
+
+/**
+ * @brief Set brown-out detection interrupt and reset levels
+ * @param rstlvl : Brown-out detector reset level
+ * @param intlvl : Brown-out interrupt level
+ * @return Nothing
+ * @note Brown-out detection reset will be disabled upon exiting this function.
+ * Use Chip_SYSCTL_EnableBODReset() to re-enable.
+ */
+STATIC INLINE void Chip_SYSCTL_SetBODLevels(CHIP_SYSCTL_BODRSTLVL_T rstlvl,
+ CHIP_SYSCTL_BODRINTVAL_T intlvl)
+{
+ LPC_SYSCTL->BODCTRL = ((uint32_t) rstlvl) | (((uint32_t) intlvl) << 2);
+}
+
+#if defined(CHIP_LPC11AXX)
+/**
+ * @brief Returns brown-out detection interrupt status
+ * @return true if the BOD interrupt is pending, otherwise false
+ */
+STATIC INLINE bool Chip_SYSCTL_GetBODIntStatus(void)
+{
+ return (bool) ((LPC_SYSCTL->BODCTRL & (1 << 6)) != 0);
+}
+
+#else
+/**
+ * @brief Enable brown-out detection reset
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_EnableBODReset(void)
+{
+ LPC_SYSCTL->BODCTRL |= (1 << 4);
+}
+
+/**
+ * @brief Disable brown-out detection reset
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_DisableBODReset(void)
+{
+ LPC_SYSCTL->BODCTRL &= ~(1 << 4);
+}
+
+#endif
+
+/**
+ * @brief Set System tick timer calibration value
+ * @param sysCalVal : System tick timer calibration value
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_SetSYSTCKCAL(uint32_t sysCalVal)
+{
+ LPC_SYSCTL->SYSTCKCAL = sysCalVal;
+}
+
+#if defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC1125)
+/**
+ * @brief Set System IRQ latency
+ * @param latency : Latency in clock ticks
+ * @return Nothing
+ * @note Sets the IRQ latency, a value between 0 and 255 clocks. Lower
+ * values allow better latency.
+ */
+STATIC INLINE void Chip_SYSCTL_SetIRQLatency(uint32_t latency)
+{
+ LPC_SYSCTL->IRQLATENCY = latency;
+}
+
+/**
+ * @brief Get System IRQ latency
+ * @return Latency in clock ticks
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetIRQLatency(void)
+{
+ return LPC_SYSCTL->IRQLATENCY;
+}
+
+#endif
+
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX) || defined(CHIP_LPC1125)
+/**
+ * @brief Set source for non-maskable interrupt (NMI)
+ * @param intsrc : IRQ number to assign to the NMI
+ * @return Nothing
+ * @note The NMI source will be disabled upon exiting this function. use the
+ * Chip_SYSCTL_EnableNMISource() function to enable the NMI source.
+ */
+STATIC INLINE void Chip_SYSCTL_SetNMISource(uint32_t intsrc)
+{
+ LPC_SYSCTL->NMISRC = intsrc;
+}
+
+/**
+ * @brief Enable interrupt used for NMI source
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_EnableNMISource(void)
+{
+ LPC_SYSCTL->NMISRC |= SYSCTL_NMISRC_ENABLE;
+}
+
+/**
+ * @brief Disable interrupt used for NMI source
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_DisableNMISource(void)
+{
+ LPC_SYSCTL->NMISRC &= ~(SYSCTL_NMISRC_ENABLE);
+}
+
+#endif
+
+#if defined(CHIP_LPC11AXX)
+/**
+ * @brief Setup a pin source for the pin interrupts (0-7)
+ * @param intno : IRQ number
+ * @param port : port number 0/1)
+ * @param pin : pin number (0->31)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_SetPinInterrupt(uint32_t intno, uint8_t port, uint8_t pin)
+{
+ LPC_SYSCTL->PINTSEL[intno] = (uint32_t) ((port << 5) | pin);
+}
+
+#elif defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+/**
+ * @brief Setup a pin source for the pin interrupts (0-7)
+ * @param intno : IRQ number
+ * @param port : port number 0/1)
+ * @param pin : pin number (0->23 for GPIO Port 0 and 0->31 for GPIO Port 1)
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_SetPinInterrupt(uint32_t intno, uint8_t port, uint8_t pin)
+{
+ LPC_SYSCTL->PINTSEL[intno] = (uint32_t) (port * 24 + pin);
+}
+
+#endif
+
+#if defined(CHIP_LPC11UXX)
+/**
+ * @brief Setup USB clock control
+ * @param ap_clk : USB need_clock signal control (0 or 1)
+ * @param pol_clk : USB need_clock polarity for triggering the USB wake-up interrupt (0 or 1)
+ * @return Nothing
+ * @note See the USBCLKCTRL register in the user manual for these settings.
+ */
+STATIC INLINE void Chip_SYSCTL_SetUSBCLKCTRL(uint32_t ap_clk, uint32_t pol_clk)
+{
+ LPC_SYSCTL->USBCLKCTRL = ap_clk | (pol_clk << 1);
+}
+
+/**
+ * @brief Returns the status of the USB need_clock signal
+ * @return true if USB need_clock statis is high, otherwise false
+ */
+STATIC INLINE bool Chip_SYSCTL_GetUSBCLKStatus(void)
+{
+ return (bool) ((LPC_SYSCTL->USBCLKST & 0x1) != 0);
+}
+
+#endif
+
+#if defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC1125)
+/**
+ * @brief Set edge for PIO start logic
+ * @param pin : PIO pin number
+ * @param edge : 0 for falling edge, 1 for rising edge
+ * @return Nothing
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE void Chip_SYSCTL_SetStartPin(uint32_t pin, uint32_t edge)
+{
+ if (edge) {
+ LPC_SYSCTL->STARTAPRP0 |= (1 << pin);
+ }
+ else {
+ LPC_SYSCTL->STARTAPRP0 &= ~(1 << pin);
+ }
+}
+
+/**
+ * @brief Enable PIO start logic for a pin
+ * @param pin : PIO pin number
+ * @return Nothing
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE void Chip_SYSCTL_EnableStartPin(uint32_t pin)
+{
+ LPC_SYSCTL->STARTERP0 |= (1 << pin);
+}
+
+/**
+ * @brief Disable PIO start logic for a pin
+ * @param pin : PIO pin number
+ * @return Nothing
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE void Chip_SYSCTL_DisableStartPin(uint32_t pin)
+{
+ LPC_SYSCTL->STARTERP0 &= ~(1 << pin);
+}
+
+/**
+ * @brief Clear PIO start logic state
+ * @param pin : PIO pin number
+ * @return Nothing
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE void Chip_SYSCTL_ResetStartPin(uint32_t pin)
+{
+ LPC_SYSCTL->STARTRSRP0CLR = (1 << pin);
+}
+
+/**
+ * @brief Returns status of pin wakeup
+ * @param pin : PIO pin number
+ * @return true if a pin start signal is pending, otherwise false
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE bool Chip_SYSCTL_GetStartPinStatus(uint32_t pin)
+{
+ return (bool) ((LPC_SYSCTL->STARTSRP0 & (1 << pin)) != 0);
+}
+
+#endif
+
+#if defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+/**
+ * @brief Enables a pin's (PINT) wakeup logic
+ * @param pin : pin number
+ * @return Nothing
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE void Chip_SYSCTL_EnablePINTWakeup(uint32_t pin)
+{
+ LPC_SYSCTL->STARTERP0 |= (1 << pin);
+}
+
+/**
+ * @brief Disables a pin's (PINT) wakeup logic
+ * @param pin : pin number
+ * @return Nothing
+ * @note Different devices support different pins, see the user manual for supported pins.
+ */
+STATIC INLINE void Chip_SYSCTL_DisablePINTWakeup(uint32_t pin)
+{
+ LPC_SYSCTL->STARTERP0 &= ~(1 << pin);
+}
+
+/**
+ * Peripheral interrupt wakeup events, LPC11E/Uxx only
+ */
+#define SYSCTL_WAKEUP_WWDTINT (1 << 12) /*!< WWDT interrupt wake-up */
+#define SYSCTL_WAKEUP_BODINT (1 << 13) /*!< Brown Out Detect (BOD) interrupt wake-up */
+#define SYSCTL_WAKEUP_USB_WAKEUP (1 << 19) /*!< USB need_clock signal wake-up, LPC11Uxx only */
+#define SYSCTL_WAKEUP_GPIOINT0 (1 << 20) /*!< GPIO GROUP0 interrupt wake-up */
+#define SYSCTL_WAKEUP_GPIOINT1 (1 << 21) /*!< GPIO GROUP1 interrupt wake-up */
+
+/**
+ * @brief Enables a peripheral's wakeup logic
+ * @param periphmask : OR'ed values of SYSCTL_WAKEUP_* for wakeup
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_EnablePeriphWakeup(uint32_t periphmask)
+{
+ LPC_SYSCTL->STARTERP1 |= periphmask;
+}
+
+/**
+ * @brief Disables a peripheral's wakeup logic
+ * @param periphmask : OR'ed values of SYSCTL_WAKEUP_* for wakeup
+ * @return Nothing
+ */
+STATIC INLINE void Chip_SYSCTL_DisablePeriphWakeup(uint32_t periphmask)
+{
+ LPC_SYSCTL->STARTERP1 &= ~periphmask;
+}
+
+#endif
+
+#if !defined(LPC11AXX)
+/**
+ * Deep sleep setup values
+ */
+#define SYSCTL_DEEPSLP_BOD_PD (1 << 3) /*!< BOD power-down control in Deep-sleep mode, powered down */
+#define SYSCTL_DEEPSLP_WDTOSC_PD (1 << 6) /*!< Watchdog oscillator power control in Deep-sleep, powered down */
+#if defined(CHIP_LPC11XXLV)
+#define SYSCTL_DEEPSLP_IRCOUT_PD (1 << 0) /*!< IRC oscillator output in Deep-sleep mode, powered down */
+#define SYSCTL_DEEPSLP_IRC_PD (1 << 1) /*!< IRC oscillator in Deep-sleep, powered down */
+#endif
+
+/**
+ * @brief Setup deep sleep behaviour for power down
+ * @param sleepmask : OR'ed values of SYSCTL_DEEPSLP_* values (high to powerdown on deepsleep)
+ * @return Nothing
+ * @note This must be setup prior to using deep sleep. See the user manual
+ ***(PDSLEEPCFG register) for more info on setting this up. This function selects
+ * which peripherals are powered down on deep sleep.
+ * This function should only be called once with all options for power-down
+ * in that call.
+ */
+void Chip_SYSCTL_SetDeepSleepPD(uint32_t sleepmask);
+
+/**
+ * @brief Returns current deep sleep mask
+ * @return OR'ed values of SYSCTL_DEEPSLP_* values
+ * @note A high bit indicates the peripheral will power down on deep sleep.
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetDeepSleepPD(void)
+{
+ return LPC_SYSCTL->PDSLEEPCFG;
+}
+
+/**
+ * Deep sleep to wakeup setup values
+ */
+#define SYSCTL_SLPWAKE_IRCOUT_PD (1 << 0) /*!< IRC oscillator output wake-up configuration */
+#define SYSCTL_SLPWAKE_IRC_PD (1 << 1) /*!< IRC oscillator power-down wake-up configuration */
+#define SYSCTL_SLPWAKE_FLASH_PD (1 << 2) /*!< Flash wake-up configuration */
+#define SYSCTL_SLPWAKE_BOD_PD (1 << 3) /*!< BOD wake-up configuration */
+#define SYSCTL_SLPWAKE_ADC_PD (1 << 4) /*!< ADC wake-up configuration */
+#define SYSCTL_SLPWAKE_SYSOSC_PD (1 << 5) /*!< System oscillator wake-up configuration */
+#define SYSCTL_SLPWAKE_WDTOSC_PD (1 << 6) /*!< Watchdog oscillator wake-up configuration */
+#define SYSCTL_SLPWAKE_SYSPLL_PD (1 << 7) /*!< System PLL wake-up configuration */
+#if defined(CHIP_LPC11UXX)
+#define SYSCTL_SLPWAKE_USBPLL_PD (1 << 8) /*!< USB PLL wake-up configuration */
+#define SYSCTL_SLPWAKE_USBPAD_PD (1 << 10) /*!< USB transceiver wake-up configuration */
+#endif
+
+/**
+ * @brief Setup wakeup behaviour from deep sleep
+ * @param wakeupmask : OR'ed values of SYSCTL_SLPWAKE_* values (high is powered down)
+ * @return Nothing
+ * @note This must be setup prior to using deep sleep. See the user manual
+ * (PDWAKECFG register) for more info on setting this up. This function selects
+ * which peripherals are powered up on exit from deep sleep.
+ * This function should only be called once with all options for wakeup
+ * in that call.
+ */
+void Chip_SYSCTL_SetWakeup(uint32_t wakeupmask);
+
+/**
+ * @brief Return current wakeup mask
+ * @return OR'ed values of SYSCTL_SLPWAKE_* values
+ * @note A high state indicates the peripehral will powerup on wakeup.
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetWakeup(void)
+{
+ return LPC_SYSCTL->PDWAKECFG;
+}
+
+#endif
+
+/**
+ * Power down configuration values
+ */
+#define SYSCTL_POWERDOWN_IRCOUT_PD (1 << 0) /*!< IRC oscillator output power down */
+#define SYSCTL_POWERDOWN_IRC_PD (1 << 1) /*!< IRC oscillator power-down */
+#define SYSCTL_POWERDOWN_FLASH_PD (1 << 2) /*!< Flash power down */
+#if !defined(CHIP_LPC11AXX)
+#define SYSCTL_POWERDOWN_BOD_PD (1 << 3) /*!< BOD power down */
+#endif
+#define SYSCTL_POWERDOWN_ADC_PD (1 << 4) /*!< ADC power down */
+#define SYSCTL_POWERDOWN_SYSOSC_PD (1 << 5) /*!< System oscillator power down */
+#define SYSCTL_POWERDOWN_WDTOSC_PD (1 << 6) /*!< Watchdog oscillator power down */
+#define SYSCTL_POWERDOWN_SYSPLL_PD (1 << 7) /*!< System PLL power down */
+#if defined(CHIP_LPC11UXX)
+#define SYSCTL_POWERDOWN_USBPLL_PD (1 << 8) /*!< USB PLL power-down */
+#define SYSCTL_POWERDOWN_USBPAD_PD (1 << 10)/*!< USB transceiver power-down */
+#endif
+#if defined(CHIP_LPC11AXX)
+#define SYSCTL_POWERDOWN_LFOSC_PD (1 << 13) /*!< Low frequency oscillator power-down */
+#define SYSCTL_POWERDOWN_DAC_PD (1 << 14) /*!< DAC power-down */
+#define SYSCTL_POWERDOWN_TS_PD (1 << 15) /*!< Temperature Sensor power-down */
+#define SYSCTL_POWERDOWN_ACOMP_PD (1 << 16) /*!< Analog Comparator power-down */
+#endif
+
+/**
+ * @brief Power down one or more blocks or peripherals
+ * @param powerdownmask : OR'ed values of SYSCTL_POWERDOWN_* values
+ * @return Nothing
+ */
+void Chip_SYSCTL_PowerDown(uint32_t powerdownmask);
+
+/**
+ * @brief Power up one or more blocks or peripherals
+ * @param powerupmask : OR'ed values of SYSCTL_POWERDOWN_* values
+ * @return Nothing
+ */
+void Chip_SYSCTL_PowerUp(uint32_t powerupmask);
+
+/**
+ * @brief Get power status
+ * @return OR'ed values of SYSCTL_POWERDOWN_* values
+ * @note A high state indicates the peripheral is powered down.
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetPowerStates(void)
+{
+ return LPC_SYSCTL->PDRUNCFG;
+}
+
+/**
+ * @brief Return the device ID
+ * @return the device ID
+ */
+STATIC INLINE uint32_t Chip_SYSCTL_GetDeviceID(void)
+{
+ return LPC_SYSCTL->DEVICEID;
+}
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*!< __SYSCTL_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/timer_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,446 @@
+/*
+ * @brief LPC11xx 16/32-bit Timer/PWM control functions
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __TIMER_11XX_H_
+#define __TIMER_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup TIMER_11XX CHIP: LPC11xx 16/32-bit Timer driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief 32-bit Standard timer register block structure
+ */
+typedef struct { /*!< TIMERn Structure */
+ __IO uint32_t IR; /*!< Interrupt Register. The IR can be written to clear interrupts. The IR can be read to identify which of eight possible interrupt sources are pending. */
+ __IO uint32_t TCR; /*!< Timer Control Register. The TCR is used to control the Timer Counter functions. The Timer Counter can be disabled or reset through the TCR. */
+ __IO uint32_t TC; /*!< Timer Counter. The 32 bit TC is incremented every PR+1 cycles of PCLK. The TC is controlled through the TCR. */
+ __IO uint32_t PR; /*!< Prescale Register. The Prescale Counter (below) is equal to this value, the next clock increments the TC and clears the PC. */
+ __IO uint32_t PC; /*!< Prescale Counter. The 32 bit PC is a counter which is incremented to the value stored in PR. When the value in PR is reached, the TC is incremented and the PC is cleared. The PC is observable and controllable through the bus interface. */
+ __IO uint32_t MCR; /*!< Match Control Register. The MCR is used to control if an interrupt is generated and if the TC is reset when a Match occurs. */
+ __IO uint32_t MR[4]; /*!< Match Register. MR can be enabled through the MCR to reset the TC, stop both the TC and PC, and/or generate an interrupt every time MR matches the TC. */
+ __IO uint32_t CCR; /*!< Capture Control Register. The CCR controls which edges of the capture inputs are used to load the Capture Registers and whether or not an interrupt is generated when a capture takes place. */
+ __IO uint32_t CR[4]; /*!< Capture Register. CR is loaded with the value of TC when there is an event on the CAPn.0 input. */
+ __IO uint32_t EMR; /*!< External Match Register. The EMR controls the external match pins MATn.0-3 (MAT0.0-3 and MAT1.0-3 respectively). */
+ __I uint32_t RESERVED0[12];
+ __IO uint32_t CTCR; /*!< Count Control Register. The CTCR selects between Timer and Counter mode, and in Counter mode selects the signal and edge(s) for counting. */
+ __IO uint32_t PWMC;
+} LPC_TIMER_T;
+
+/** Macro to clear interrupt pending */
+#define TIMER_IR_CLR(n) _BIT(n)
+
+/** Macro for getting a timer match interrupt bit */
+#define TIMER_MATCH_INT(n) (_BIT((n) & 0x0F))
+/** Macro for getting a capture event interrupt bit */
+#define TIMER_CAP_INT(n) (_BIT((((n) & 0x0F) + 4)))
+
+/** Timer/counter enable bit */
+#define TIMER_ENABLE ((uint32_t) (1 << 0))
+/** Timer/counter reset bit */
+#define TIMER_RESET ((uint32_t) (1 << 1))
+
+/** Bit location for interrupt on MRx match, n = 0 to 3 */
+#define TIMER_INT_ON_MATCH(n) (_BIT(((n) * 3)))
+/** Bit location for reset on MRx match, n = 0 to 3 */
+#define TIMER_RESET_ON_MATCH(n) (_BIT((((n) * 3) + 1)))
+/** Bit location for stop on MRx match, n = 0 to 3 */
+#define TIMER_STOP_ON_MATCH(n) (_BIT((((n) * 3) + 2)))
+
+/** Bit location for CAP.n on CRx rising edge, n = 0 to 3 */
+#define TIMER_CAP_RISING(n) (_BIT(((n) * 3)))
+/** Bit location for CAP.n on CRx falling edge, n = 0 to 3 */
+#define TIMER_CAP_FALLING(n) (_BIT((((n) * 3) + 1)))
+/** Bit location for CAP.n on CRx interrupt enable, n = 0 to 3 */
+#define TIMER_INT_ON_CAP(n) (_BIT((((n) * 3) + 2)))
+
+/**
+ * @brief Initialize a timer
+ * @param pTMR : Pointer to timer IP register address
+ * @return Nothing
+ */
+void Chip_TIMER_Init(LPC_TIMER_T *pTMR);
+
+/**
+ * @brief Shutdown a timer
+ * @param pTMR : Pointer to timer IP register address
+ * @return Nothing
+ */
+void Chip_TIMER_DeInit(LPC_TIMER_T *pTMR);
+
+/**
+ * @brief Determine if a match interrupt is pending
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match interrupt number to check
+ * @return false if the interrupt is not pending, otherwise true
+ * @note Determine if the match interrupt for the passed timer and match
+ * counter is pending.
+ */
+STATIC INLINE bool Chip_TIMER_MatchPending(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ return (bool) ((pTMR->IR & TIMER_MATCH_INT(matchnum)) != 0);
+}
+
+/**
+ * @brief Determine if a capture interrupt is pending
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture interrupt number to check
+ * @return false if the interrupt is not pending, otherwise true
+ * @note Determine if the capture interrupt for the passed capture pin is
+ * pending.
+ */
+STATIC INLINE bool Chip_TIMER_CapturePending(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ return (bool) ((pTMR->IR & TIMER_CAP_INT(capnum)) != 0);
+}
+
+/**
+ * @brief Clears a (pending) match interrupt
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match interrupt number to clear
+ * @return Nothing
+ * @note Clears a pending timer match interrupt.
+ */
+STATIC INLINE void Chip_TIMER_ClearMatch(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->IR = TIMER_IR_CLR(matchnum);
+}
+
+/**
+ * @brief Clears a (pending) capture interrupt
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture interrupt number to clear
+ * @return Nothing
+ * @note Clears a pending timer capture interrupt.
+ */
+STATIC INLINE void Chip_TIMER_ClearCapture(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->IR = (0x10 << capnum);
+}
+
+/**
+ * @brief Enables the timer (starts count)
+ * @param pTMR : Pointer to timer IP register address
+ * @return Nothing
+ * @note Enables the timer to start counting.
+ */
+STATIC INLINE void Chip_TIMER_Enable(LPC_TIMER_T *pTMR)
+{
+ pTMR->TCR |= TIMER_ENABLE;
+}
+
+/**
+ * @brief Disables the timer (stops count)
+ * @param pTMR : Pointer to timer IP register address
+ * @return Nothing
+ * @note Disables the timer to stop counting.
+ */
+STATIC INLINE void Chip_TIMER_Disable(LPC_TIMER_T *pTMR)
+{
+ pTMR->TCR &= ~TIMER_ENABLE;
+}
+
+/**
+ * @brief Returns the current timer count
+ * @param pTMR : Pointer to timer IP register address
+ * @return Current timer terminal count value
+ * @note Returns the current timer terminal count.
+ */
+STATIC INLINE uint32_t Chip_TIMER_ReadCount(LPC_TIMER_T *pTMR)
+{
+ return pTMR->TC;
+}
+
+/**
+ * @brief Returns the current prescale count
+ * @param pTMR : Pointer to timer IP register address
+ * @return Current timer prescale count value
+ * @note Returns the current prescale count.
+ */
+STATIC INLINE uint32_t Chip_TIMER_ReadPrescale(LPC_TIMER_T *pTMR)
+{
+ return pTMR->PC;
+}
+
+/**
+ * @brief Sets the prescaler value
+ * @param pTMR : Pointer to timer IP register address
+ * @param prescale : Prescale value to set the prescale register to
+ * @return Nothing
+ * @note Sets the prescale count value.
+ */
+STATIC INLINE void Chip_TIMER_PrescaleSet(LPC_TIMER_T *pTMR, uint32_t prescale)
+{
+ pTMR->PR = prescale;
+}
+
+/**
+ * @brief Sets a timer match value
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer to set match count for
+ * @param matchval : Match value for the selected match count
+ * @return Nothing
+ * @note Sets one of the timer match values.
+ */
+STATIC INLINE void Chip_TIMER_SetMatch(LPC_TIMER_T *pTMR, int8_t matchnum, uint32_t matchval)
+{
+ pTMR->MR[matchnum] = matchval;
+}
+
+/**
+ * @brief Reads a capture register
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture register to read
+ * @return The selected capture register value
+ * @note Returns the selected capture register value.
+ */
+STATIC INLINE uint32_t Chip_TIMER_ReadCapture(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ return pTMR->CR[capnum];
+}
+
+/**
+ * @brief Resets the timer terminal and prescale counts to 0
+ * @param pTMR : Pointer to timer IP register address
+ * @return Nothing
+ */
+void Chip_TIMER_Reset(LPC_TIMER_T *pTMR);
+
+/**
+ * @brief Enables a match interrupt that fires when the terminal count
+ * matches the match counter value.
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer, 0 to 3
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_MatchEnableInt(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->MCR |= TIMER_INT_ON_MATCH(matchnum);
+}
+
+/**
+ * @brief Disables a match interrupt for a match counter.
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer, 0 to 3
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_MatchDisableInt(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->MCR &= ~TIMER_INT_ON_MATCH(matchnum);
+}
+
+/**
+ * @brief For the specific match counter, enables reset of the terminal count register when a match occurs
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer, 0 to 3
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_ResetOnMatchEnable(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->MCR |= TIMER_RESET_ON_MATCH(matchnum);
+}
+
+/**
+ * @brief For the specific match counter, disables reset of the terminal count register when a match occurs
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer, 0 to 3
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_ResetOnMatchDisable(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->MCR &= ~TIMER_RESET_ON_MATCH(matchnum);
+}
+
+/**
+ * @brief Enable a match timer to stop the terminal count when a
+ * match count equals the terminal count.
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer, 0 to 3
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_StopOnMatchEnable(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->MCR |= TIMER_STOP_ON_MATCH(matchnum);
+}
+
+/**
+ * @brief Disable stop on match for a match timer. Disables a match timer
+ * to stop the terminal count when a match count equals the terminal count.
+ * @param pTMR : Pointer to timer IP register address
+ * @param matchnum : Match timer, 0 to 3
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_StopOnMatchDisable(LPC_TIMER_T *pTMR, int8_t matchnum)
+{
+ pTMR->MCR &= ~TIMER_STOP_ON_MATCH(matchnum);
+}
+
+/**
+ * @brief Enables capture on on rising edge of selected CAP signal for the
+ * selected capture register, enables the selected CAPn.capnum signal to load
+ * the capture register with the terminal coount on a rising edge.
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture signal/register to use
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_CaptureRisingEdgeEnable(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->CCR |= TIMER_CAP_RISING(capnum);
+}
+
+/**
+ * @brief Disables capture on on rising edge of selected CAP signal. For the
+ * selected capture register, disables the selected CAPn.capnum signal to load
+ * the capture register with the terminal coount on a rising edge.
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture signal/register to use
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_CaptureRisingEdgeDisable(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->CCR &= ~TIMER_CAP_RISING(capnum);
+}
+
+/**
+ * @brief Enables capture on on falling edge of selected CAP signal. For the
+ * selected capture register, enables the selected CAPn.capnum signal to load
+ * the capture register with the terminal coount on a falling edge.
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture signal/register to use
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_CaptureFallingEdgeEnable(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->CCR |= TIMER_CAP_FALLING(capnum);
+}
+
+/**
+ * @brief Disables capture on on falling edge of selected CAP signal. For the
+ * selected capture register, disables the selected CAPn.capnum signal to load
+ * the capture register with the terminal coount on a falling edge.
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture signal/register to use
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_CaptureFallingEdgeDisable(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->CCR &= ~TIMER_CAP_FALLING(capnum);
+}
+
+/**
+ * @brief Enables interrupt on capture of selected CAP signal. For the
+ * selected capture register, an interrupt will be generated when the enabled
+ * rising or falling edge on CAPn.capnum is detected.
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture signal/register to use
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_CaptureEnableInt(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->CCR |= TIMER_INT_ON_CAP(capnum);
+}
+
+/**
+ * @brief Disables interrupt on capture of selected CAP signal
+ * @param pTMR : Pointer to timer IP register address
+ * @param capnum : Capture signal/register to use
+ * @return Nothing
+ */
+STATIC INLINE void Chip_TIMER_CaptureDisableInt(LPC_TIMER_T *pTMR, int8_t capnum)
+{
+ pTMR->CCR &= ~TIMER_INT_ON_CAP(capnum);
+}
+
+/**
+ * @brief Standard timer initial match pin state and change state
+ */
+typedef enum IP_TIMER_PIN_MATCH_STATE {
+ TIMER_EXTMATCH_DO_NOTHING = 0, /*!< Timer match state does nothing on match pin */
+ TIMER_EXTMATCH_CLEAR = 1, /*!< Timer match state sets match pin low */
+ TIMER_EXTMATCH_SET = 2, /*!< Timer match state sets match pin high */
+ TIMER_EXTMATCH_TOGGLE = 3 /*!< Timer match state toggles match pin */
+} TIMER_PIN_MATCH_STATE_T;
+
+/**
+ * @brief Sets external match control (MATn.matchnum) pin control. For the pin
+ * selected with matchnum, sets the function of the pin that occurs on
+ * a terminal count match for the match count.
+ * @param pTMR : Pointer to timer IP register address
+ * @param initial_state : Initial state of the pin, high(1) or low(0)
+ * @param matchState : Selects the match state for the pin
+ * @param matchnum : MATn.matchnum signal to use
+ * @return Nothing
+ * @note For the pin selected with matchnum, sets the function of the pin that occurs on
+ * a terminal count match for the match count.
+ */
+void Chip_TIMER_ExtMatchControlSet(LPC_TIMER_T *pTMR, int8_t initial_state,
+ TIMER_PIN_MATCH_STATE_T matchState, int8_t matchnum);
+
+/**
+ * @brief Standard timer clock and edge for count source
+ */
+typedef enum IP_TIMER_CAP_SRC_STATE {
+ TIMER_CAPSRC_RISING_PCLK = 0, /*!< Timer ticks on PCLK rising edge */
+ TIMER_CAPSRC_RISING_CAPN = 1, /*!< Timer ticks on CAPn.x rising edge */
+ TIMER_CAPSRC_FALLING_CAPN = 2, /*!< Timer ticks on CAPn.x falling edge */
+ TIMER_CAPSRC_BOTH_CAPN = 3 /*!< Timer ticks on CAPn.x both edges */
+} TIMER_CAP_SRC_STATE_T;
+
+/**
+ * @brief Sets timer count source and edge with the selected passed from CapSrc.
+ * If CapSrc selected a CAPn pin, select the specific CAPn pin with the capnum value.
+ * @param pTMR : Pointer to timer IP register address
+ * @param capSrc : timer clock source and edge
+ * @param capnum : CAPn.capnum pin to use (if used)
+ * @return Nothing
+ * @note If CapSrc selected a CAPn pin, select the specific CAPn pin with the capnum value.
+ */
+STATIC INLINE void Chip_TIMER_TIMER_SetCountClockSrc(LPC_TIMER_T *pTMR,
+ TIMER_CAP_SRC_STATE_T capSrc,
+ int8_t capnum)
+{
+ pTMR->CTCR = (uint32_t) capSrc | ((uint32_t) capnum) << 2;
+}
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __TIMER_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/uart_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,787 @@
+/*
+ * @brief LPC11xx UART chip driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __UART_11XX_H_
+#define __UART_11XX_H_
+
+#include "ring_buffer.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup UART_11XX CHIP: LPC11xx UART driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+/**
+ * @brief USART register block structure
+ */
+typedef struct { /*!< USARTn Structure */
+
+ union {
+ __IO uint32_t DLL; /*!< Divisor Latch LSB. Least significant byte of the baud rate divisor value. The full divisor is used to generate a baud rate from the fractional rate divider (DLAB = 1). */
+ __O uint32_t THR; /*!< Transmit Holding Register. The next character to be transmitted is written here (DLAB = 0). */
+ __I uint32_t RBR; /*!< Receiver Buffer Register. Contains the next received character to be read (DLAB = 0). */
+ };
+
+ union {
+ __IO uint32_t IER; /*!< Interrupt Enable Register. Contains individual interrupt enable bits for the 7 potential UART interrupts (DLAB = 0). */
+ __IO uint32_t DLM; /*!< Divisor Latch MSB. Most significant byte of the baud rate divisor value. The full divisor is used to generate a baud rate from the fractional rate divider (DLAB = 1). */
+ };
+
+ union {
+ __O uint32_t FCR; /*!< FIFO Control Register. Controls UART FIFO usage and modes. */
+ __I uint32_t IIR; /*!< Interrupt ID Register. Identifies which interrupt(s) are pending. */
+ };
+
+ __IO uint32_t LCR; /*!< Line Control Register. Contains controls for frame formatting and break generation. */
+ __IO uint32_t MCR; /*!< Modem Control Register. Only present on USART ports with full modem support. */
+ __I uint32_t LSR; /*!< Line Status Register. Contains flags for transmit and receive status, including line errors. */
+ __I uint32_t MSR; /*!< Modem Status Register. Only present on USART ports with full modem support. */
+ __IO uint32_t SCR; /*!< Scratch Pad Register. Eight-bit temporary storage for software. */
+ __IO uint32_t ACR; /*!< Auto-baud Control Register. Contains controls for the auto-baud feature. */
+ __IO uint32_t ICR; /*!< IrDA control register (not all UARTS) */
+ __IO uint32_t FDR; /*!< Fractional Divider Register. Generates a clock input for the baud rate divider. */
+ __IO uint32_t OSR; /*!< Oversampling Register. Controls the degree of oversampling during each bit time. Only on some UARTS. */
+ __IO uint32_t TER1; /*!< Transmit Enable Register. Turns off USART transmitter for use with software flow control. */
+ uint32_t RESERVED0[3];
+ __IO uint32_t HDEN; /*!< Half-duplex enable Register- only on some UARTs */
+ __I uint32_t RESERVED1[1];
+ __IO uint32_t SCICTRL; /*!< Smart card interface control register- only on some UARTs */
+
+ __IO uint32_t RS485CTRL; /*!< RS-485/EIA-485 Control. Contains controls to configure various aspects of RS-485/EIA-485 modes. */
+ __IO uint32_t RS485ADRMATCH; /*!< RS-485/EIA-485 address match. Contains the address match value for RS-485/EIA-485 mode. */
+ __IO uint32_t RS485DLY; /*!< RS-485/EIA-485 direction control delay. */
+
+ union {
+ __IO uint32_t SYNCCTRL; /*!< Synchronous mode control register. Only on USARTs. */
+ __I uint32_t FIFOLVL; /*!< FIFO Level register. Provides the current fill levels of the transmit and receive FIFOs. */
+ };
+
+ __IO uint32_t TER2; /*!< Transmit Enable Register. Only on LPC177X_8X UART4 and LPC18XX/43XX USART0/2/3. */
+} LPC_USART_T;
+
+
+/**
+ * @brief Macro defines for UART Receive Buffer register
+ */
+#define UART_RBR_MASKBIT (0xFF) /*!< UART Received Buffer mask bit (8 bits) */
+
+/**
+ * @brief Macro defines for UART Divisor Latch LSB register
+ */
+#define UART_LOAD_DLL(div) ((div) & 0xFF) /*!< Macro for loading LSB of divisor */
+#define UART_DLL_MASKBIT (0xFF) /*!< Divisor latch LSB bit mask */
+
+/**
+ * @brief Macro defines for UART Divisor Latch MSB register
+ */
+#define UART_LOAD_DLM(div) (((div) >> 8) & 0xFF) /*!< Macro for loading MSB of divisors */
+#define UART_DLM_MASKBIT (0xFF) /*!< Divisor latch MSB bit mask */
+
+/**
+ * @brief Macro defines for UART Interrupt Enable Register
+ */
+#define UART_IER_RBRINT (1 << 0) /*!< RBR Interrupt enable */
+#define UART_IER_THREINT (1 << 1) /*!< THR Interrupt enable */
+#define UART_IER_RLSINT (1 << 2) /*!< RX line status interrupt enable */
+#define UART_IER_MSINT (1 << 3) /*!< Modem status interrupt enable - valid for 11xx, 17xx/40xx UART1, 18xx/43xx UART1 only */
+#define UART_IER_CTSINT (1 << 7) /*!< CTS signal transition interrupt enable - valid for 17xx/40xx UART1, 18xx/43xx UART1 only */
+#define UART_IER_ABEOINT (1 << 8) /*!< Enables the end of auto-baud interrupt */
+#define UART_IER_ABTOINT (1 << 9) /*!< Enables the auto-baud time-out interrupt */
+#define UART_IER_BITMASK (0x307) /*!< UART interrupt enable register bit mask - valid for 13xx, 17xx/40xx UART0/2/3, 18xx/43xx UART0/2/3 only*/
+#define UART1_IER_BITMASK (0x30F) /*!< UART1 interrupt enable register bit mask - valid for 11xx only */
+#define UART2_IER_BITMASK (0x38F) /*!< UART2 interrupt enable register bit mask - valid for 17xx/40xx UART1, 18xx/43xx UART1 only */
+
+/**
+ * @brief Macro defines for UART Interrupt Identification Register
+ */
+#define UART_IIR_INTSTAT_PEND (1 << 0) /*!< Interrupt pending status - Active low */
+#define UART_IIR_FIFO_EN (3 << 6) /*!< These bits are equivalent to FCR[0] */
+#define UART_IIR_ABEO_INT (1 << 8) /*!< End of auto-baud interrupt */
+#define UART_IIR_ABTO_INT (1 << 9) /*!< Auto-baud time-out interrupt */
+#define UART_IIR_BITMASK (0x3CF) /*!< UART interrupt identification register bit mask */
+
+/* Interrupt ID bit definitions */
+#define UART_IIR_INTID_MASK (7 << 1) /*!< Interrupt identification: Interrupt ID mask */
+#define UART_IIR_INTID_RLS (3 << 1) /*!< Interrupt identification: Receive line interrupt */
+#define UART_IIR_INTID_RDA (2 << 1) /*!< Interrupt identification: Receive data available interrupt */
+#define UART_IIR_INTID_CTI (6 << 1) /*!< Interrupt identification: Character time-out indicator interrupt */
+#define UART_IIR_INTID_THRE (1 << 1) /*!< Interrupt identification: THRE interrupt */
+#define UART_IIR_INTID_MODEM (0 << 1) /*!< Interrupt identification: Modem interrupt */
+
+/**
+ * @brief Macro defines for UART FIFO Control Register
+ */
+#define UART_FCR_FIFO_EN (1 << 0) /*!< UART FIFO enable */
+#define UART_FCR_RX_RS (1 << 1) /*!< UART RX FIFO reset */
+#define UART_FCR_TX_RS (1 << 2) /*!< UART TX FIFO reset */
+#define UART_FCR_DMAMODE_SEL (1 << 3) /*!< UART DMA mode selection - valid for 17xx/40xx, 18xx/43xx only */
+#define UART_FCR_BITMASK (0xCF) /*!< UART FIFO control bit mask */
+
+#define UART_TX_FIFO_SIZE (16)
+
+/* FIFO trigger level bit definitions */
+#define UART_FCR_TRG_LEV0 (0) /*!< UART FIFO trigger level 0: 1 character */
+#define UART_FCR_TRG_LEV1 (1 << 6) /*!< UART FIFO trigger level 1: 4 character */
+#define UART_FCR_TRG_LEV2 (2 << 6) /*!< UART FIFO trigger level 2: 8 character */
+#define UART_FCR_TRG_LEV3 (3 << 6) /*!< UART FIFO trigger level 3: 14 character */
+
+/**
+ * @brief Macro defines for UART Line Control Register
+ */
+/* UART word length select bit definitions */
+#define UART_LCR_WLEN_MASK (3 << 0) /*!< UART word length select bit mask */
+#define UART_LCR_WLEN5 (0 << 0) /*!< UART word length select: 5 bit data mode */
+#define UART_LCR_WLEN6 (1 << 0) /*!< UART word length select: 6 bit data mode */
+#define UART_LCR_WLEN7 (2 << 0) /*!< UART word length select: 7 bit data mode */
+#define UART_LCR_WLEN8 (3 << 0) /*!< UART word length select: 8 bit data mode */
+
+/* UART Stop bit select bit definitions */
+#define UART_LCR_SBS_MASK (1 << 2) /*!< UART stop bit select: bit mask */
+#define UART_LCR_SBS_1BIT (0 << 2) /*!< UART stop bit select: 1 stop bit */
+#define UART_LCR_SBS_2BIT (1 << 2) /*!< UART stop bit select: 2 stop bits (in 5 bit data mode, 1.5 stop bits) */
+
+/* UART Parity enable bit definitions */
+#define UART_LCR_PARITY_EN (1 << 3) /*!< UART Parity Enable */
+#define UART_LCR_PARITY_DIS (0 << 3) /*!< UART Parity Disable */
+#define UART_LCR_PARITY_ODD (0 << 4) /*!< UART Parity select: Odd parity */
+#define UART_LCR_PARITY_EVEN (1 << 4) /*!< UART Parity select: Even parity */
+#define UART_LCR_PARITY_F_1 (2 << 4) /*!< UART Parity select: Forced 1 stick parity */
+#define UART_LCR_PARITY_F_0 (3 << 4) /*!< UART Parity select: Forced 0 stick parity */
+#define UART_LCR_BREAK_EN (1 << 6) /*!< UART Break transmission enable */
+#define UART_LCR_DLAB_EN (1 << 7) /*!< UART Divisor Latches Access bit enable */
+#define UART_LCR_BITMASK (0xFF) /*!< UART line control bit mask */
+
+/**
+ * @brief Macro defines for UART Modem Control Register
+ */
+#define UART_MCR_DTR_CTRL (1 << 0) /*!< Source for modem output pin DTR */
+#define UART_MCR_RTS_CTRL (1 << 1) /*!< Source for modem output pin RTS */
+#define UART_MCR_LOOPB_EN (1 << 4) /*!< Loop back mode select */
+#define UART_MCR_AUTO_RTS_EN (1 << 6) /*!< Enable Auto RTS flow-control */
+#define UART_MCR_AUTO_CTS_EN (1 << 7) /*!< Enable Auto CTS flow-control */
+#define UART_MCR_BITMASK (0xD3) /*!< UART bit mask value */
+
+/**
+ * @brief Macro defines for UART Line Status Register
+ */
+#define UART_LSR_RDR (1 << 0) /*!< Line status: Receive data ready */
+#define UART_LSR_OE (1 << 1) /*!< Line status: Overrun error */
+#define UART_LSR_PE (1 << 2) /*!< Line status: Parity error */
+#define UART_LSR_FE (1 << 3) /*!< Line status: Framing error */
+#define UART_LSR_BI (1 << 4) /*!< Line status: Break interrupt */
+#define UART_LSR_THRE (1 << 5) /*!< Line status: Transmit holding register empty */
+#define UART_LSR_TEMT (1 << 6) /*!< Line status: Transmitter empty */
+#define UART_LSR_RXFE (1 << 7) /*!< Line status: Error in RX FIFO */
+#define UART_LSR_TXFE (1 << 8) /*!< Line status: Error in RX FIFO */
+#define UART_LSR_BITMASK (0xFF) /*!< UART Line status bit mask */
+#define UART1_LSR_BITMASK (0x1FF) /*!< UART1 Line status bit mask - valid for 11xx, 18xx/43xx UART0/2/3 only */
+
+/**
+ * @brief Macro defines for UART Modem Status Register
+ */
+#define UART_MSR_DELTA_CTS (1 << 0) /*!< Modem status: State change of input CTS */
+#define UART_MSR_DELTA_DSR (1 << 1) /*!< Modem status: State change of input DSR */
+#define UART_MSR_LO2HI_RI (1 << 2) /*!< Modem status: Low to high transition of input RI */
+#define UART_MSR_DELTA_DCD (1 << 3) /*!< Modem status: State change of input DCD */
+#define UART_MSR_CTS (1 << 4) /*!< Modem status: Clear To Send State */
+#define UART_MSR_DSR (1 << 5) /*!< Modem status: Data Set Ready State */
+#define UART_MSR_RI (1 << 6) /*!< Modem status: Ring Indicator State */
+#define UART_MSR_DCD (1 << 7) /*!< Modem status: Data Carrier Detect State */
+#define UART_MSR_BITMASK (0xFF) /*!< Modem status: MSR register bit-mask value */
+
+/**
+ * @brief Macro defines for UART Auto baudrate control register
+ */
+#define UART_ACR_START (1 << 0) /*!< UART Auto-baud start */
+#define UART_ACR_MODE (1 << 1) /*!< UART Auto baudrate Mode 1 */
+#define UART_ACR_AUTO_RESTART (1 << 2) /*!< UART Auto baudrate restart */
+#define UART_ACR_ABEOINT_CLR (1 << 8) /*!< UART End of auto-baud interrupt clear */
+#define UART_ACR_ABTOINT_CLR (1 << 9) /*!< UART Auto-baud time-out interrupt clear */
+#define UART_ACR_BITMASK (0x307) /*!< UART Auto Baudrate register bit mask */
+
+/**
+ * @brief Macro defines for UART RS485 Control register
+ */
+#define UART_RS485CTRL_NMM_EN (1 << 0) /*!< RS-485/EIA-485 Normal Multi-drop Mode (NMM) is disabled */
+#define UART_RS485CTRL_RX_DIS (1 << 1) /*!< The receiver is disabled */
+#define UART_RS485CTRL_AADEN (1 << 2) /*!< Auto Address Detect (AAD) is enabled */
+#define UART_RS485CTRL_SEL_DTR (1 << 3) /*!< If direction control is enabled (bit DCTRL = 1), pin DTR is
+ used for direction control */
+#define UART_RS485CTRL_DCTRL_EN (1 << 4) /*!< Enable Auto Direction Control */
+#define UART_RS485CTRL_OINV_1 (1 << 5) /*!< This bit reverses the polarity of the direction
+ control signal on the RTS (or DTR) pin. The direction control pin
+ will be driven to logic "1" when the transmitter has data to be sent */
+#define UART_RS485CTRL_BITMASK (0x3F) /*!< RS485 control bit-mask value */
+
+/**
+ * @brief Macro defines for UART IrDA Control Register - valid for 11xx, 17xx/40xx UART0/2/3, 18xx/43xx UART3 only
+ */
+#define UART_ICR_IRDAEN (1 << 0) /*!< IrDA mode enable */
+#define UART_ICR_IRDAINV (1 << 1) /*!< IrDA serial input inverted */
+#define UART_ICR_FIXPULSE_EN (1 << 2) /*!< IrDA fixed pulse width mode */
+#define UART_ICR_PULSEDIV(n) ((n & 0x07) << 3) /*!< PulseDiv - Configures the pulse when FixPulseEn = 1 */
+#define UART_ICR_BITMASK (0x3F) /*!< UART IRDA bit mask */
+
+/**
+ * @brief Macro defines for UART half duplex register - ????
+ */
+#define UART_HDEN_HDEN ((1 << 0)) /*!< enable half-duplex mode*/
+
+/**
+ * @brief Macro defines for UART Smart card interface Control Register - valid for 11xx, 18xx/43xx UART0/2/3 only
+ */
+#define UART_SCICTRL_SCIEN (1 << 0) /*!< enable asynchronous half-duplex smart card interface*/
+#define UART_SCICTRL_NACKDIS (1 << 1) /*!< NACK response is inhibited*/
+#define UART_SCICTRL_PROTSEL_T1 (1 << 2) /*!< ISO7816-3 protocol T1 is selected*/
+#define UART_SCICTRL_TXRETRY(n) ((n & 0x07) << 5) /*!< number of retransmission*/
+#define UART_SCICTRL_GUARDTIME(n) ((n & 0xFF) << 8) /*!< Extra guard time*/
+
+/**
+ * @brief Macro defines for UART Fractional Divider Register
+ */
+#define UART_FDR_DIVADDVAL(n) (n & 0x0F) /*!< Baud-rate generation pre-scaler divisor */
+#define UART_FDR_MULVAL(n) ((n << 4) & 0xF0) /*!< Baud-rate pre-scaler multiplier value */
+#define UART_FDR_BITMASK (0xFF) /*!< UART Fractional Divider register bit mask */
+
+/**
+ * @brief Macro defines for UART Tx Enable Register
+ */
+#define UART_TER1_TXEN (1 << 7) /*!< Transmit enable bit - valid for 11xx, 13xx, 17xx/40xx only */
+#define UART_TER2_TXEN (1 << 0) /*!< Transmit enable bit - valid for 18xx/43xx only */
+
+/**
+ * @brief Macro defines for UART Synchronous Control Register - 11xx, 18xx/43xx UART0/2/3 only
+ */
+#define UART_SYNCCTRL_SYNC (1 << 0) /*!< enable synchronous mode*/
+#define UART_SYNCCTRL_CSRC_MASTER (1 << 1) /*!< synchronous master mode*/
+#define UART_SYNCCTRL_FES (1 << 2) /*!< sample on falling edge*/
+#define UART_SYNCCTRL_TSBYPASS (1 << 3) /*!< to be defined*/
+#define UART_SYNCCTRL_CSCEN (1 << 4) /*!< Continuous running clock enable (master mode only)*/
+#define UART_SYNCCTRL_STARTSTOPDISABLE (1 << 5) /*!< Do not send start/stop bit*/
+#define UART_SYNCCTRL_CCCLR (1 << 6) /*!< stop continuous clock*/
+
+/**
+ * @brief Enable transmission on UART TxD pin
+ * @param pUART : Pointer to selected pUART peripheral
+ * @return Nothing
+ */
+STATIC INLINE void Chip_UART_TXEnable(LPC_USART_T *pUART)
+{
+ pUART->TER1 = UART_TER1_TXEN;
+}
+
+/**
+ * @brief Disable transmission on UART TxD pin
+ * @param pUART : Pointer to selected pUART peripheral
+ * @return Nothing
+ */
+STATIC INLINE void Chip_UART_TXDisable(LPC_USART_T *pUART)
+{
+ pUART->TER1 = 0;
+}
+
+/**
+ * @brief Transmit a single data byte through the UART peripheral
+ * @param pUART : Pointer to selected UART peripheral
+ * @param data : Byte to transmit
+ * @return Nothing
+ * @note This function attempts to place a byte into the UART transmit
+ * FIFO or transmit hold register regard regardless of UART state
+ */
+STATIC INLINE void Chip_UART_SendByte(LPC_USART_T *pUART, uint8_t data)
+{
+ pUART->THR = (uint32_t) data;
+}
+
+/**
+ * @brief Read a single byte data from the UART peripheral
+ * @param pUART : Pointer to selected UART peripheral
+ * @return A single byte of data read
+ * @note This function reads a byte from the UART receive FIFO or
+ * receive hold register regard regardless of UART state. The
+ * FIFO status should be read first prior to using this function
+ */
+STATIC INLINE uint8_t Chip_UART_ReadByte(LPC_USART_T *pUART)
+{
+ return (uint8_t) (pUART->RBR & UART_RBR_MASKBIT);
+}
+
+/**
+ * @brief Enable UART interrupts
+ * @param pUART : Pointer to selected UART peripheral
+ * @param intMask : OR'ed Interrupts to enable in the Interrupt Enable Register (IER)
+ * @return Nothing
+ * @note Use an OR'ed value of UART_IER_* definitions with this function
+ * to enable specific UART interrupts. The Divisor Latch Access Bit
+ * (DLAB) in LCR must be cleared in order to access the IER register.
+ * This function doesn't alter the DLAB state
+ */
+STATIC INLINE void Chip_UART_IntEnable(LPC_USART_T *pUART, uint32_t intMask)
+{
+ pUART->IER |= intMask;
+}
+
+/**
+ * @brief Disable UART interrupts
+ * @param pUART : Pointer to selected UART peripheral
+ * @param intMask : OR'ed Interrupts to disable in the Interrupt Enable Register (IER)
+ * @return Nothing
+ * @note Use an OR'ed value of UART_IER_* definitions with this function
+ * to disable specific UART interrupts. The Divisor Latch Access Bit
+ * (DLAB) in LCR must be cleared in order to access the IER register.
+ * This function doesn't alter the DLAB state
+ */
+STATIC INLINE void Chip_UART_IntDisable(LPC_USART_T *pUART, uint32_t intMask)
+{
+ pUART->IER &= ~intMask;
+}
+
+/**
+ * @brief Returns UART interrupts that are enabled
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Returns the enabled UART interrupts
+ * @note Use an OR'ed value of UART_IER_* definitions with this function
+ * to determine which interrupts are enabled. You can check
+ * for multiple enabled bits if needed.
+ */
+STATIC INLINE uint32_t Chip_UART_GetIntsEnabled(LPC_USART_T *pUART)
+{
+ return pUART->IER;
+}
+
+/**
+ * @brief Read the Interrupt Identification Register (IIR)
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Current pending interrupt status per the IIR register
+ */
+STATIC INLINE uint32_t Chip_UART_ReadIntIDReg(LPC_USART_T *pUART)
+{
+ return pUART->IIR;
+}
+
+/**
+ * @brief Setup the UART FIFOs
+ * @param pUART : Pointer to selected UART peripheral
+ * @param fcr : FIFO control register setup OR'ed flags
+ * @return Nothing
+ * @note Use OR'ed value of UART_FCR_* definitions with this function
+ * to select specific options. For example, to enable the FIFOs
+ * with a RX trip level of 8 characters, use something like
+ * (UART_FCR_FIFO_EN | UART_FCR_TRG_LEV2)
+ */
+STATIC INLINE void Chip_UART_SetupFIFOS(LPC_USART_T *pUART, uint32_t fcr)
+{
+ pUART->FCR = fcr;
+}
+
+/**
+ * @brief Configure data width, parity and stop bits
+ * @param pUART : Pointer to selected pUART peripheral
+ * @param config : UART configuration, OR'ed values of UART_LCR_* defines
+ * @return Nothing
+ * @note Select OR'ed config options for the UART from the UART_LCR_*
+ * definitions. For example, a configuration of 8 data bits, 1
+ * stop bit, and even (enabled) parity would be
+ * (UART_LCR_WLEN8 | UART_LCR_SBS_1BIT | UART_LCR_PARITY_EN | UART_LCR_PARITY_EVEN)
+ */
+STATIC INLINE void Chip_UART_ConfigData(LPC_USART_T *pUART, uint32_t config)
+{
+ pUART->LCR = config;
+}
+
+/**
+ * @brief Enable access to Divisor Latches
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Nothing
+ */
+STATIC INLINE void Chip_UART_EnableDivisorAccess(LPC_USART_T *pUART)
+{
+ pUART->LCR |= UART_LCR_DLAB_EN;
+}
+
+/**
+ * @brief Disable access to Divisor Latches
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Nothing
+ */
+STATIC INLINE void Chip_UART_DisableDivisorAccess(LPC_USART_T *pUART)
+{
+ pUART->LCR &= ~UART_LCR_DLAB_EN;
+}
+
+/**
+ * @brief Set LSB and MSB divisor latch registers
+ * @param pUART : Pointer to selected UART peripheral
+ * @param dll : Divisor Latch LSB value
+ * @param dlm : Divisor Latch MSB value
+ * @return Nothing
+ * @note The Divisor Latch Access Bit (DLAB) in LCR must be set in
+ * order to access the USART Divisor Latches. This function
+ * doesn't alter the DLAB state.
+ */
+STATIC INLINE void Chip_UART_SetDivisorLatches(LPC_USART_T *pUART, uint8_t dll, uint8_t dlm)
+{
+ pUART->DLL = (uint32_t) dll;
+ pUART->DLM = (uint32_t) dlm;
+}
+
+/**
+ * @brief Return modem control register/status
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Modem control register (status)
+ * @note Mask bits of the returned status value with UART_MCR_*
+ * definitions for specific statuses.
+ */
+STATIC INLINE uint32_t Chip_UART_ReadModemControl(LPC_USART_T *pUART)
+{
+ return pUART->MCR;
+}
+
+/**
+ * @brief Set modem control register/status
+ * @param pUART : Pointer to selected UART peripheral
+ * @param mcr : Modem control register flags to set
+ * @return Nothing
+ * @note Use an Or'ed value of UART_MCR_* definitions with this
+ * call to set specific options.
+ */
+STATIC INLINE void Chip_UART_SetModemControl(LPC_USART_T *pUART, uint32_t mcr)
+{
+ pUART->MCR |= mcr;
+}
+
+/**
+ * @brief Clear modem control register/status
+ * @param pUART : Pointer to selected UART peripheral
+ * @param mcr : Modem control register flags to clear
+ * @return Nothing
+ * @note Use an Or'ed value of UART_MCR_* definitions with this
+ * call to clear specific options.
+ */
+STATIC INLINE void Chip_UART_ClearModemControl(LPC_USART_T *pUART, uint32_t mcr)
+{
+ pUART->MCR &= ~mcr;
+}
+
+/**
+ * @brief Return Line Status register/status (LSR)
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Line Status register (status)
+ * @note Mask bits of the returned status value with UART_LSR_*
+ * definitions for specific statuses.
+ */
+STATIC INLINE uint32_t Chip_UART_ReadLineStatus(LPC_USART_T *pUART)
+{
+ return pUART->LSR;
+}
+
+/**
+ * @brief Return Modem Status register/status (MSR)
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Modem Status register (status)
+ * @note Mask bits of the returned status value with UART_MSR_*
+ * definitions for specific statuses.
+ */
+STATIC INLINE uint32_t Chip_UART_ReadModemStatus(LPC_USART_T *pUART)
+{
+ return pUART->MSR;
+}
+
+/**
+ * @brief Write a byte to the scratchpad register
+ * @param pUART : Pointer to selected UART peripheral
+ * @param data : Byte value to write
+ * @return Nothing
+ */
+STATIC INLINE void Chip_UART_SetScratch(LPC_USART_T *pUART, uint8_t data)
+{
+ pUART->SCR = (uint32_t) data;
+}
+
+/**
+ * @brief Returns current byte value in the scratchpad register
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Byte value read from scratchpad register
+ */
+STATIC INLINE uint8_t Chip_UART_ReadScratch(LPC_USART_T *pUART)
+{
+ return (uint8_t) (pUART->SCR & 0xFF);
+}
+
+/**
+ * @brief Set autobaud register options
+ * @param pUART : Pointer to selected UART peripheral
+ * @param acr : Or'ed values to set for ACR register
+ * @return Nothing
+ * @note Use an Or'ed value of UART_ACR_* definitions with this
+ * call to set specific options.
+ */
+STATIC INLINE void Chip_UART_SetAutoBaudReg(LPC_USART_T *pUART, uint32_t acr)
+{
+ pUART->ACR |= acr;
+}
+
+/**
+ * @brief Clear autobaud register options
+ * @param pUART : Pointer to selected UART peripheral
+ * @param acr : Or'ed values to clear for ACR register
+ * @return Nothing
+ * @note Use an Or'ed value of UART_ACR_* definitions with this
+ * call to clear specific options.
+ */
+STATIC INLINE void Chip_UART_ClearAutoBaudReg(LPC_USART_T *pUART, uint32_t acr)
+{
+ pUART->ACR &= ~acr;
+}
+
+/**
+ * @brief Set RS485 control register options
+ * @param pUART : Pointer to selected UART peripheral
+ * @param ctrl : Or'ed values to set for RS485 control register
+ * @return Nothing
+ * @note Use an Or'ed value of UART_RS485CTRL_* definitions with this
+ * call to set specific options.
+ */
+STATIC INLINE void Chip_UART_SetRS485Flags(LPC_USART_T *pUART, uint32_t ctrl)
+{
+ pUART->RS485CTRL |= ctrl;
+}
+
+/**
+ * @brief Clear RS485 control register options
+ * @param pUART : Pointer to selected UART peripheral
+ * @param ctrl : Or'ed values to clear for RS485 control register
+ * @return Nothing
+ * @note Use an Or'ed value of UART_RS485CTRL_* definitions with this
+ * call to clear specific options.
+ */
+STATIC INLINE void Chip_UART_ClearRS485Flags(LPC_USART_T *pUART, uint32_t ctrl)
+{
+ pUART->RS485CTRL &= ~ctrl;
+}
+
+/**
+ * @brief Set RS485 address match value
+ * @param pUART : Pointer to selected UART peripheral
+ * @param addr : Address match value for RS-485/EIA-485 mode
+ * @return Nothing
+ */
+STATIC INLINE void Chip_UART_SetRS485Addr(LPC_USART_T *pUART, uint8_t addr)
+{
+ pUART->RS485ADRMATCH = (uint32_t) addr;
+}
+
+/**
+ * @brief Read RS485 address match value
+ * @param pUART : Pointer to selected UART peripheral
+ * @return Address match value for RS-485/EIA-485 mode
+ */
+STATIC INLINE uint8_t Chip_UART_GetRS485Addr(LPC_USART_T *pUART)
+{
+ return (uint8_t) (pUART->RS485ADRMATCH & 0xFF);
+}
+
+/**
+ * @brief Set RS485 direction control (RTS or DTR) delay value
+ * @param pUART : Pointer to selected UART peripheral
+ * @param dly : direction control (RTS or DTR) delay value
+ * @return Nothing
+ * @note This delay time is in periods of the baud clock. Any delay
+ * time from 0 to 255 bit times may be programmed.
+ */
+STATIC INLINE void Chip_UART_SetRS485Delay(LPC_USART_T *pUART, uint8_t dly)
+{
+ pUART->RS485DLY = (uint32_t) dly;
+}
+
+/**
+ * @brief Read RS485 direction control (RTS or DTR) delay value
+ * @param pUART : Pointer to selected UART peripheral
+ * @return direction control (RTS or DTR) delay value
+ * @note This delay time is in periods of the baud clock. Any delay
+ * time from 0 to 255 bit times may be programmed.
+ */
+STATIC INLINE uint8_t Chip_UART_GetRS485Delay(LPC_USART_T *pUART)
+{
+ return (uint8_t) (pUART->RS485DLY & 0xFF);
+}
+
+/**
+ * @brief Initializes the pUART peripheral
+ * @param pUART : Pointer to selected pUART peripheral
+ * @return Nothing
+ */
+void Chip_UART_Init(LPC_USART_T *pUART);
+
+/**
+ * @brief De-initializes the pUART peripheral.
+ * @param pUART : Pointer to selected pUART peripheral
+ * @return Nothing
+ */
+void Chip_UART_DeInit(LPC_USART_T *pUART);
+
+/**
+ * @brief Transmit a byte array through the UART peripheral (non-blocking)
+ * @param pUART : Pointer to selected UART peripheral
+ * @param data : Pointer to bytes to transmit
+ * @param numBytes : Number of bytes to transmit
+ * @return The actual number of bytes placed into the FIFO
+ * @note This function places data into the transmit FIFO until either
+ * all the data is in the FIFO or the FIFO is full. This function
+ * will not block in the FIFO is full. The actual number of bytes
+ * placed into the FIFO is returned. This function ignores errors.
+ */
+int Chip_UART_Send(LPC_USART_T *pUART, const void *data, int numBytes);
+
+/**
+ * @brief Read data through the UART peripheral (non-blocking)
+ * @param pUART : Pointer to selected UART peripheral
+ * @param data : Pointer to bytes array to fill
+ * @param numBytes : Size of the passed data array
+ * @return The actual number of bytes read
+ * @note This function reads data from the receive FIFO until either
+ * all the data has been read or the passed buffer is completely full.
+ * This function will not block. This function ignores errors.
+ */
+int Chip_UART_Read(LPC_USART_T *pUART, void *data, int numBytes);
+
+/**
+ * @brief Sets best dividers to get a target bit rate (without fractional divider)
+ * @param pUART : Pointer to selected UART peripheral
+ * @param baudrate : Target baud rate (baud rate = bit rate)
+ * @return The actual baud rate, or 0 if no rate can be found
+ */
+uint32_t Chip_UART_SetBaud(LPC_USART_T *pUART, uint32_t baudrate);
+
+/**
+ * @brief Sets best dividers to get a target bit rate (with fractional divider)
+ * @param pUART : Pointer to selected UART peripheral
+ * @param baudrate : Target baud rate (baud rate = bit rate)
+ * @return The actual baud rate, or 0 if no rate can be found
+ */
+uint32_t Chip_UART_SetBaudFDR(LPC_USART_T *pUART, uint32_t baudrate);
+
+/**
+ * @brief Transmit a byte array through the UART peripheral (blocking)
+ * @param pUART : Pointer to selected UART peripheral
+ * @param data : Pointer to data to transmit
+ * @param numBytes : Number of bytes to transmit
+ * @return The number of bytes transmitted
+ * @note This function will send or place all bytes into the transmit
+ * FIFO. This function will block until the last bytes are in the FIFO.
+ */
+int Chip_UART_SendBlocking(LPC_USART_T *pUART, const void *data, int numBytes);
+
+/**
+ * @brief Read data through the UART peripheral (blocking)
+ * @param pUART : Pointer to selected UART peripheral
+ * @param data : Pointer to data array to fill
+ * @param numBytes : Size of the passed data array
+ * @return The size of the dat array
+ * @note This function reads data from the receive FIFO until the passed
+ * buffer is completely full. The function will block until full.
+ * This function ignores errors.
+ */
+int Chip_UART_ReadBlocking(LPC_USART_T *pUART, void *data, int numBytes);
+
+/**
+ * @brief UART receive-only interrupt handler for ring buffers
+ * @param pUART : Pointer to selected UART peripheral
+ * @param pRB : Pointer to ring buffer structure to use
+ * @return Nothing
+ * @note If ring buffer support is desired for the receive side
+ * of data transfer, the UART interrupt should call this
+ * function for a receive based interrupt status.
+ */
+void Chip_UART_RXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB);
+
+/**
+ * @brief UART transmit-only interrupt handler for ring buffers
+ * @param pUART : Pointer to selected UART peripheral
+ * @param pRB : Pointer to ring buffer structure to use
+ * @return Nothing
+ * @note If ring buffer support is desired for the transmit side
+ * of data transfer, the UART interrupt should call this
+ * function for a transmit based interrupt status.
+ */
+void Chip_UART_TXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB);
+
+/**
+ * @brief Populate a transmit ring buffer and start UART transmit
+ * @param pUART : Pointer to selected UART peripheral
+ * @param pRB : Pointer to ring buffer structure to use
+ * @param data : Pointer to buffer to move to ring buffer
+ * @param bytes : Number of bytes to move
+ * @return The number of bytes placed into the ring buffer
+ * @note Will move the data into the TX ring buffer and start the
+ * transfer. If the number of bytes returned is less than the
+ * number of bytes to send, the ring buffer is considered full.
+ */
+uint32_t Chip_UART_SendRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, const void *data, int bytes);
+
+/**
+ * @brief Copy data from a receive ring buffer
+ * @param pUART : Pointer to selected UART peripheral
+ * @param pRB : Pointer to ring buffer structure to use
+ * @param data : Pointer to buffer to fill from ring buffer
+ * @param bytes : Size of the passed buffer in bytes
+ * @return The number of bytes placed into the ring buffer
+ * @note Will move the data from the RX ring buffer up to the
+ * the maximum passed buffer size. Returns 0 if there is
+ * no data in the ring buffer.
+ */
+int Chip_UART_ReadRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, void *data, int bytes);
+
+/**
+ * @brief UART receive/transmit interrupt handler for ring buffers
+ * @param pUART : Pointer to selected UART peripheral
+ * @param pRXRB : Pointer to transmit ring buffer
+ * @param pTXRB : Pointer to receive ring buffer
+ * @return Nothing
+ * @note This provides a basic implementation of the UART IRQ
+ * handler for support of a ring buffer implementation for
+ * transmit and receive.
+ */
+void Chip_UART_IRQRBHandler(LPC_USART_T *pUART, RINGBUFF_T *pRXRB, RINGBUFF_T *pTXRB);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UART_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/inc/wwdt_11xx.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,266 @@
+/*
+ * @brief LPC11xx WWDT chip driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#ifndef __WWDT_11XX_H_
+#define __WWDT_11XX_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @defgroup WWDT_11XX CHIP: LPC11xx Windowed Watchdog driver
+ * @ingroup CHIP_11XX_Drivers
+ * @{
+ */
+
+#if !defined(CHIP_LPC11CXX) || defined(CHIP_LPC1125)
+#define WATCHDOG_WINDOW_SUPPORT
+#endif
+
+#if defined(CHIP_LPC11AXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC11UXX)
+#define WATCHDOG_CLKSEL_SUPPORT
+#endif
+
+/**
+ * @brief Windowed Watchdog register block structure
+ */
+typedef struct { /*!< WWDT Structure */
+ __IO uint32_t MOD; /*!< Watchdog mode register. This register contains the basic mode and status of the Watchdog Timer. */
+ __IO uint32_t TC; /*!< Watchdog timer constant register. This register determines the time-out value. */
+ __O uint32_t FEED; /*!< Watchdog feed sequence register. Writing 0xAA followed by 0x55 to this register reloads the Watchdog timer with the value contained in WDTC. */
+ __I uint32_t TV; /*!< Watchdog timer value register. This register reads out the current value of the Watchdog timer. */
+#ifdef WATCHDOG_CLKSEL_SUPPORT
+ __IO uint32_t CLKSEL; /*!< Watchdog clock select register. */
+#else
+ __I uint32_t RESERVED0;
+#endif
+#ifdef WATCHDOG_WINDOW_SUPPORT
+ __IO uint32_t WARNINT; /*!< Watchdog warning interrupt register. This register contains the Watchdog warning interrupt compare value. */
+ __IO uint32_t WINDOW; /*!< Watchdog timer window register. This register contains the Watchdog window value. */
+#endif
+} LPC_WWDT_T;
+
+/**
+ * @brief Watchdog Mode register definitions
+ */
+/** Watchdog Mode Bitmask */
+#define WWDT_WDMOD_BITMASK ((uint32_t) 0x1F)
+/** WWDT interrupt enable bit */
+#define WWDT_WDMOD_WDEN ((uint32_t) (1 << 0))
+/** WWDT interrupt enable bit */
+#define WWDT_WDMOD_WDRESET ((uint32_t) (1 << 1))
+/** WWDT time out flag bit */
+#define WWDT_WDMOD_WDTOF ((uint32_t) (1 << 2))
+/** WDT Time Out flag bit */
+#define WWDT_WDMOD_WDINT ((uint32_t) (1 << 3))
+/** WWDT Protect flag bit */
+#define WWDT_WDMOD_WDPROTECT ((uint32_t) (1 << 4))
+
+/**
+ * @brief Initialize the Watchdog timer
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @return None
+ */
+void Chip_WWDT_Init(LPC_WWDT_T *pWWDT);
+
+/**
+ * @brief Shutdown the Watchdog timer
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @return None
+ */
+void Chip_WWDT_DeInit(LPC_WWDT_T *pWWDT);
+
+/**
+ * @brief Set WDT timeout constant value used for feed
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param timeout : WDT timeout in ticks, between WWDT_TICKS_MIN and WWDT_TICKS_MAX
+ * @return none
+ */
+STATIC INLINE void Chip_WWDT_SetTimeOut(LPC_WWDT_T *pWWDT, uint32_t timeout)
+{
+ pWWDT->TC = timeout;
+}
+
+/**
+ * @brief Feed watchdog timer
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @return None
+ * @note If this function isn't called, a watchdog timer warning will occur.
+ * After the warning, a timeout will occur if a feed has happened.
+ */
+STATIC INLINE void Chip_WWDT_Feed(LPC_WWDT_T *pWWDT)
+{
+ pWWDT->FEED = 0xAA;
+ pWWDT->FEED = 0x55;
+}
+
+#if defined(WATCHDOG_WINDOW_SUPPORT)
+/**
+ * @brief Set WWDT warning interrupt
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param timeout : WDT warning in ticks, between 0 and 1023
+ * @return None
+ * @note This is the number of ticks after the watchdog interrupt that the
+ * warning interrupt will be generated.
+ */
+STATIC INLINE void Chip_WWDT_SetWarning(LPC_WWDT_T *pWWDT, uint32_t timeout)
+{
+ pWWDT->WARNINT = timeout;
+}
+
+/**
+ * @brief Set WWDT window time
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param timeout : WDT timeout in ticks, between WWDT_TICKS_MIN and WWDT_TICKS_MAX
+ * @return None
+ * @note The watchdog timer must be fed between the timeout from the Chip_WWDT_SetTimeOut()
+ * function and this function, with this function defining the last tick before the
+ * watchdog window interrupt occurs.
+ */
+STATIC INLINE void Chip_WWDT_SetWindow(LPC_WWDT_T *pWWDT, uint32_t timeout)
+{
+ pWWDT->WINDOW = timeout;
+}
+
+#endif
+
+/**
+ * @brief Enable watchdog timer options
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param options : An or'ed set of options of values
+ * WWDT_WDMOD_WDEN, WWDT_WDMOD_WDRESET, and WWDT_WDMOD_WDPROTECT
+ * @return None
+ * @note You can enable more than one option at once (ie, WWDT_WDMOD_WDRESET |
+ * WWDT_WDMOD_WDPROTECT), but use the WWDT_WDMOD_WDEN after all other options
+ * are set (or unset) with no other options. If WWDT_WDMOD_LOCK is used, it cannot
+ * be unset.
+ */
+STATIC INLINE void Chip_WWDT_SetOption(LPC_WWDT_T *pWWDT, uint32_t options)
+{
+ pWWDT->MOD |= options;
+}
+
+/**
+ * @brief Disable/clear watchdog timer options
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param options : An or'ed set of options of values
+ * WWDT_WDMOD_WDEN, WWDT_WDMOD_WDRESET, and WWDT_WDMOD_WDPROTECT
+ * @return None
+ * @note You can disable more than one option at once (ie, WWDT_WDMOD_WDRESET |
+ * WWDT_WDMOD_WDTOF).
+ */
+STATIC INLINE void Chip_WWDT_UnsetOption(LPC_WWDT_T *pWWDT, uint32_t options)
+{
+ pWWDT->MOD &= (~options) & WWDT_WDMOD_BITMASK;
+}
+
+/**
+ * @brief Enable WWDT activity
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @return None
+ */
+STATIC INLINE void Chip_WWDT_Start(LPC_WWDT_T *pWWDT)
+{
+ Chip_WWDT_SetOption(pWWDT, WWDT_WDMOD_WDEN);
+ Chip_WWDT_Feed(pWWDT);
+}
+
+/**
+ * @brief Read WWDT status flag
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @return Watchdog status, an Or'ed value of WWDT_WDMOD_*
+ */
+STATIC INLINE uint32_t Chip_WWDT_GetStatus(LPC_WWDT_T *pWWDT)
+{
+ return pWWDT->MOD;
+}
+
+/**
+ * @brief Clear WWDT interrupt status flags
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param status : Or'ed value of status flag(s) that you want to clear, should be:
+ * - WWDT_WDMOD_WDTOF: Clear watchdog timeout flag
+ * - WWDT_WDMOD_WDINT: Clear watchdog warning flag
+ * @return None
+ */
+void Chip_WWDT_ClearStatusFlag(LPC_WWDT_T *pWWDT, uint32_t status);
+
+/**
+ * @brief Get the current value of WDT
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @return current value of WDT
+ */
+STATIC INLINE uint32_t Chip_WWDT_GetCurrentCount(LPC_WWDT_T *pWWDT)
+{
+ return pWWDT->TV;
+}
+
+#if defined(WATCHDOG_CLKSEL_SUPPORT)
+/**
+ * @brief Watchdog Timer Clock Source Selection register definitions
+ */
+/** Clock source select bitmask */
+#define WWDT_CLKSEL_BITMASK ((uint32_t) 0x10000003)
+/** Clock source select */
+#define WWDT_CLKSEL_SOURCE(n) ((uint32_t) (n & 0x03))
+/** Lock the clock source selection */
+#define WWDT_CLKSEL_LOCK ((uint32_t) (1 << 31))
+
+/**
+ * @brief Watchdog Clock Source definitions
+ */
+typedef enum {
+ WWDT_CLKSRC_IRC = WWDT_CLKSEL_SOURCE(0), /*!< Internal RC oscillator */
+ WWDT_CLKSRC_WATCHDOG_WDOSC = WWDT_CLKSEL_SOURCE(1), /*!< Watchdog oscillator (WDOSC) */
+} CHIP_WWDT_CLK_SRC_T;
+
+/**
+ * @brief Get the current value of WDT
+ * @param pWWDT : The base of WatchDog Timer peripheral on the chip
+ * @param wdtClkSrc : Selected watchdog clock source
+ * @return Nothing
+ */
+STATIC INLINE void Chip_WWDT_SelClockSource(LPC_WWDT_T *pWWDT, CHIP_WWDT_CLK_SRC_T wdtClkSrc)
+{
+ pWWDT->CLKSEL = wdtClkSrc & WWDT_CLKSEL_BITMASK;
+}
+
+#endif
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __WWDT_11XX_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/clock_11xx.c Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,285 @@
+/*
+ * @brief LPC11XX System clock control functions
+ *
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#include "chip.h"
+
+/*****************************************************************************
+ * Private types/enumerations/variables
+ ****************************************************************************/
+
+/* Inprecise clock rates for the watchdog oscillator */
+STATIC const uint32_t wdtOSCRate[WDTLFO_OSC_4_60 + 1] = {
+ 0, /* WDT_OSC_ILLEGAL */
+ 600000, /* WDT_OSC_0_60 */
+ 1050000, /* WDT_OSC_1_05 */
+ 1400000, /* WDT_OSC_1_40 */
+ 1750000, /* WDT_OSC_1_75 */
+ 2100000, /* WDT_OSC_2_10 */
+ 2400000, /* WDT_OSC_2_40 */
+ 2700000, /* WDT_OSC_2_70 */
+ 3000000, /* WDT_OSC_3_00 */
+ 3250000, /* WDT_OSC_3_25 */
+ 3500000, /* WDT_OSC_3_50 */
+ 3750000, /* WDT_OSC_3_75 */
+ 4000000, /* WDT_OSC_4_00 */
+ 4200000, /* WDT_OSC_4_20 */
+ 4400000, /* WDT_OSC_4_40 */
+ 4600000 /* WDT_OSC_4_60 */
+};
+
+/*****************************************************************************
+ * Public types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Private functions
+ ****************************************************************************/
+
+/* Compute a WDT or LFO rate */
+STATIC uint32_t Chip_Clock_GetWDTLFORate(uint32_t reg)
+{
+ uint32_t div;
+ CHIP_WDTLFO_OSC_T clk;
+
+ /* Get WDT oscillator settings */
+ clk = (CHIP_WDTLFO_OSC_T) ((reg >> 5) & 0xF);
+ div = reg & 0x1F;
+
+ /* Compute clock rate and divided by divde value */
+ return wdtOSCRate[clk] / ((div + 1) << 1);
+}
+
+/* Compute a PLL frequency */
+STATIC uint32_t Chip_Clock_GetPLLFreq(uint32_t PLLReg, uint32_t inputRate)
+{
+ uint32_t msel = ((PLLReg & 0x1F) + 1);
+
+ return inputRate * msel;
+}
+
+/*****************************************************************************
+ * Public functions
+ ****************************************************************************/
+
+/* Set System PLL clock source */
+void Chip_Clock_SetSystemPLLSource(CHIP_SYSCTL_PLLCLKSRC_T src)
+{
+ LPC_SYSCTL->SYSPLLCLKSEL = (uint32_t) src;
+ LPC_SYSCTL->SYSPLLCLKUEN = 0;
+ LPC_SYSCTL->SYSPLLCLKUEN = 1;
+}
+
+/* Bypass System Oscillator and set oscillator frequency range */
+void Chip_Clock_SetPLLBypass(bool bypass, bool highfr)
+{
+ uint32_t ctrl = 0;
+
+ if (bypass) {
+ ctrl |= (1 << 0);
+ }
+ if (highfr) {
+ ctrl |= (1 << 1);
+ }
+
+ LPC_SYSCTL->SYSOSCCTRL = ctrl;
+}
+
+#if defined(CHIP_LPC11UXX)
+/* Set USB PLL clock source */
+void Chip_Clock_SetUSBPLLSource(CHIP_SYSCTL_PLLCLKSRC_T src)
+{
+ LPC_SYSCTL->USBPLLCLKSEL = (uint32_t) src;
+ LPC_SYSCTL->USBPLLCLKUEN = 0;
+ LPC_SYSCTL->USBPLLCLKUEN = 1;
+}
+
+#endif
+
+/* Set main system clock source */
+void Chip_Clock_SetMainClockSource(CHIP_SYSCTL_MAINCLKSRC_T src)
+{
+ LPC_SYSCTL->MAINCLKSEL = (uint32_t) src;
+ LPC_SYSCTL->MAINCLKUEN = 0;
+ LPC_SYSCTL->MAINCLKUEN = 1;
+}
+
+#if defined(CHIP_LPC11UXX)
+/* Set USB clock source and divider */
+void Chip_Clock_SetUSBClockSource(CHIP_SYSCTL_USBCLKSRC_T src, uint32_t div)
+{
+ LPC_SYSCTL->USBCLKSEL = (uint32_t) src;
+ LPC_SYSCTL->USBCLKUEN = 0;
+ LPC_SYSCTL->USBCLKUEN = 1;
+ LPC_SYSCTL->USBCLKDIV = div;
+}
+
+#endif /*CHIP_LPC11UXX*/
+
+#if defined(CHIP_LPC110X) || defined(CHIP_LPC11XXLV) || defined(CHIP_LPC11CXX) || defined(CHIP_LPC11EXX) || defined(CHIP_LPC1125)
+/* Set WDT clock source and divider */
+void Chip_Clock_SetWDTClockSource(CHIP_SYSCTL_WDTCLKSRC_T src, uint32_t div)
+{
+ LPC_SYSCTL->WDTCLKSEL = (uint32_t) src;
+ LPC_SYSCTL->WDTCLKUEN = 0;
+ LPC_SYSCTL->WDTCLKUEN = 1;
+ LPC_SYSCTL->WDTCLKDIV = div;
+}
+
+#endif
+
+#if !defined(CHIP_LPC110X)
+/* Set CLKOUT clock source and divider */
+void Chip_Clock_SetCLKOUTSource(CHIP_SYSCTL_CLKOUTSRC_T src, uint32_t div)
+{
+ LPC_SYSCTL->CLKOUTSEL = (uint32_t) src;
+ LPC_SYSCTL->CLKOUTUEN = 0;
+ LPC_SYSCTL->CLKOUTUEN = 1;
+ LPC_SYSCTL->CLKOUTDIV = div;
+}
+
+#endif
+
+/* Return estimated watchdog oscillator rate */
+uint32_t Chip_Clock_GetWDTOSCRate(void)
+{
+ return Chip_Clock_GetWDTLFORate(LPC_SYSCTL->WDTOSCCTRL);
+}
+
+#if defined(CHIP_LPC11AXX)
+/* Return estimated low frequency oscillator rate */
+uint32_t Chip_Clock_GetLFOOSCRate(void)
+{
+ return Chip_Clock_GetWDTLFORate(LPC_SYSCTL->LFOSCCTRL);
+}
+
+#endif
+
+/* Return System PLL input clock rate */
+uint32_t Chip_Clock_GetSystemPLLInClockRate(void)
+{
+ uint32_t clkRate;
+
+ switch ((CHIP_SYSCTL_PLLCLKSRC_T) (LPC_SYSCTL->SYSPLLCLKSEL & 0x3)) {
+ case SYSCTL_PLLCLKSRC_IRC:
+ clkRate = Chip_Clock_GetIntOscRate();
+ break;
+
+ case SYSCTL_PLLCLKSRC_MAINOSC:
+ clkRate = Chip_Clock_GetMainOscRate();
+ break;
+
+#if defined(CHIP_LPC11AXX)
+ case SYSCTL_PLLCLKSRC_EXT_CLKIN:
+ clkRate = Chip_Clock_GetExtClockInRate();
+ break;
+#endif
+
+ default:
+ clkRate = 0;
+ }
+
+ return clkRate;
+}
+
+/* Return System PLL output clock rate */
+uint32_t Chip_Clock_GetSystemPLLOutClockRate(void)
+{
+ return Chip_Clock_GetPLLFreq(LPC_SYSCTL->SYSPLLCTRL,
+ Chip_Clock_GetSystemPLLInClockRate());
+}
+
+#if defined(CHIP_LPC11UXX)
+/* Return USB PLL input clock rate */
+uint32_t Chip_Clock_GetUSBPLLInClockRate(void)
+{
+ uint32_t clkRate;
+
+ switch ((CHIP_SYSCTL_PLLCLKSRC_T) (LPC_SYSCTL->USBPLLCLKSEL & 0x3)) {
+ case SYSCTL_PLLCLKSRC_IRC:
+ clkRate = Chip_Clock_GetIntOscRate();
+ break;
+
+ case SYSCTL_PLLCLKSRC_MAINOSC:
+ clkRate = Chip_Clock_GetMainOscRate();
+ break;
+
+ default:
+ clkRate = 0;
+ }
+
+ return clkRate;
+}
+
+/* Return USB PLL output clock rate */
+uint32_t Chip_Clock_GetUSBPLLOutClockRate(void)
+{
+ return Chip_Clock_GetPLLFreq(LPC_SYSCTL->USBPLLCTRL,
+ Chip_Clock_GetUSBPLLInClockRate());
+}
+
+#endif
+
+/* Return main clock rate */
+uint32_t Chip_Clock_GetMainClockRate(void)
+{
+ uint32_t clkRate = 0;
+
+ switch ((CHIP_SYSCTL_MAINCLKSRC_T) (LPC_SYSCTL->MAINCLKSEL & 0x3)) {
+ case SYSCTL_MAINCLKSRC_IRC:
+ clkRate = Chip_Clock_GetIntOscRate();
+ break;
+
+ case SYSCTL_MAINCLKSRC_PLLIN:
+ clkRate = Chip_Clock_GetSystemPLLInClockRate();
+ break;
+
+#if defined(CHIP_LPC11AXX)
+ case SYSCTL_MAINCLKSRC_LFOSC:
+ clkRate = Chip_Clock_GetLFOOSCRate();
+ break;
+
+#else
+ case SYSCTL_MAINCLKSRC_WDTOSC:
+ clkRate = Chip_Clock_GetWDTOSCRate();
+ break;
+#endif
+
+ case SYSCTL_MAINCLKSRC_PLLOUT:
+ clkRate = Chip_Clock_GetSystemPLLOutClockRate();
+ break;
+ }
+
+ return clkRate;
+}
+
+/* Return system clock rate */
+uint32_t Chip_Clock_GetSystemClockRate(void)
+{
+ /* No point in checking for divide by 0 */
+ return Chip_Clock_GetMainClockRate() / LPC_SYSCTL->SYSAHBCLKDIV;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/uart_11xx.c Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,289 @@
+/*
+ * @brief LPC11xx UART chip driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#include "chip.h"
+
+#if __GNUC__
+# pragma GCC diagnostic ignored "-Wsign-conversion"
+# pragma GCC diagnostic ignored "-Wconversion"
+# pragma GCC diagnostic ignored "-Wmissing-declarations"
+# pragma GCC diagnostic ignored "-Wunused-parameter"
+#endif
+
+/*****************************************************************************
+ * Private types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Public types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Private functions
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Public functions
+ ****************************************************************************/
+
+/* Initializes the pUART peripheral */
+void Chip_UART_Init(LPC_USART_T *pUART)
+{
+ Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_UART0);
+ Chip_Clock_SetUARTClockDiv(1);
+
+ /* Enable FIFOs by default, reset them */
+ Chip_UART_SetupFIFOS(pUART, (UART_FCR_FIFO_EN | UART_FCR_RX_RS | UART_FCR_TX_RS));
+
+ /* Default 8N1, with DLAB disabled */
+ Chip_UART_ConfigData(pUART, (UART_LCR_WLEN8 | UART_LCR_SBS_1BIT | UART_LCR_PARITY_DIS));
+
+ /* Disable fractional divider */
+ pUART->FDR = 0x10;
+}
+
+/* De-initializes the pUART peripheral */
+void Chip_UART_DeInit(LPC_USART_T *pUART)
+{
+ Chip_Clock_DisablePeriphClock(SYSCTL_CLOCK_UART0);
+}
+
+/* Transmit a byte array through the UART peripheral (non-blocking) */
+int Chip_UART_Send(LPC_USART_T *pUART, const void *data, int numBytes)
+{
+ int sent = 0;
+ uint8_t *p8 = (uint8_t *) data;
+
+ /* Send until the transmit FIFO is full or out of bytes */
+ while ((sent < numBytes) &&
+ ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_THRE) != 0)) {
+ Chip_UART_SendByte(pUART, *p8);
+ p8++;
+ sent++;
+ }
+
+ return sent;
+}
+
+/* Transmit a byte array through the UART peripheral (blocking) */
+int Chip_UART_SendBlocking(LPC_USART_T *pUART, const void *data, int numBytes)
+{
+ int pass, sent = 0;
+ uint8_t *p8 = (uint8_t *) data;
+
+ while (numBytes > 0) {
+ pass = Chip_UART_Send(pUART, p8, numBytes);
+ numBytes -= pass;
+ sent += pass;
+ p8 += pass;
+ }
+
+ return sent;
+}
+
+/* Read data through the UART peripheral (non-blocking) */
+int Chip_UART_Read(LPC_USART_T *pUART, void *data, int numBytes)
+{
+ int readBytes = 0;
+ uint8_t *p8 = (uint8_t *) data;
+
+ /* Send until the transmit FIFO is full or out of bytes */
+ while ((readBytes < numBytes) &&
+ ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_RDR) != 0)) {
+ *p8 = Chip_UART_ReadByte(pUART);
+ p8++;
+ readBytes++;
+ }
+
+ return readBytes;
+}
+
+/* Read data through the UART peripheral (blocking) */
+int Chip_UART_ReadBlocking(LPC_USART_T *pUART, void *data, int numBytes)
+{
+ int pass, readBytes = 0;
+ uint8_t *p8 = (uint8_t *) data;
+
+ while (readBytes < numBytes) {
+ pass = Chip_UART_Read(pUART, p8, numBytes);
+ numBytes -= pass;
+ readBytes += pass;
+ p8 += pass;
+ }
+
+ return readBytes;
+}
+
+/* Determines and sets best dividers to get a target bit rate */
+uint32_t Chip_UART_SetBaud(LPC_USART_T *pUART, uint32_t baudrate)
+{
+ uint32_t div, divh, divl, clkin;
+
+ /* Determine UART clock in rate without FDR */
+ clkin = Chip_Clock_GetMainClockRate();
+ div = clkin / (baudrate * 16);
+
+ /* High and low halves of the divider */
+ divh = div / 256;
+ divl = div - (divh * 256);
+
+ Chip_UART_EnableDivisorAccess(pUART);
+ Chip_UART_SetDivisorLatches(pUART, divl, divh);
+ Chip_UART_DisableDivisorAccess(pUART);
+
+ /* Fractional FDR alreadt setup for 1 in UART init */
+
+ return clkin / div;
+}
+
+/* UART receive-only interrupt handler for ring buffers */
+void Chip_UART_RXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB)
+{
+ /* New data will be ignored if data not popped in time */
+ while (Chip_UART_ReadLineStatus(pUART) & UART_LSR_RDR) {
+ uint8_t ch = Chip_UART_ReadByte(pUART);
+ RingBuffer_Insert(pRB, &ch);
+ }
+}
+
+/* UART transmit-only interrupt handler for ring buffers */
+void Chip_UART_TXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB)
+{
+ uint8_t ch;
+
+ /* Fill FIFO until full or until TX ring buffer is empty */
+ while ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_THRE) != 0 &&
+ RingBuffer_Pop(pRB, &ch)) {
+ Chip_UART_SendByte(pUART, ch);
+ }
+}
+
+/* Populate a transmit ring buffer and start UART transmit */
+uint32_t Chip_UART_SendRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, const void *data, int bytes)
+{
+ uint32_t ret;
+ uint8_t *p8 = (uint8_t *) data;
+
+ /* Don't let UART transmit ring buffer change in the UART IRQ handler */
+ Chip_UART_IntDisable(pUART, UART_IER_THREINT);
+
+ /* Move as much data as possible into transmit ring buffer */
+ ret = RingBuffer_InsertMult(pRB, p8, bytes);
+ Chip_UART_TXIntHandlerRB(pUART, pRB);
+
+ /* Add additional data to transmit ring buffer if possible */
+ ret += RingBuffer_InsertMult(pRB, (p8 + ret), (bytes - ret));
+
+ /* Enable UART transmit interrupt */
+ Chip_UART_IntEnable(pUART, UART_IER_THREINT);
+
+ return ret;
+}
+
+/* Copy data from a receive ring buffer */
+int Chip_UART_ReadRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, void *data, int bytes)
+{
+ (void) pUART;
+
+ return RingBuffer_PopMult(pRB, (uint8_t *) data, bytes);
+}
+
+/* UART receive/transmit interrupt handler for ring buffers */
+void Chip_UART_IRQRBHandler(LPC_USART_T *pUART, RINGBUFF_T *pRXRB, RINGBUFF_T *pTXRB)
+{
+ /* Handle transmit interrupt if enabled */
+ if (pUART->IER & UART_IER_THREINT) {
+ Chip_UART_TXIntHandlerRB(pUART, pTXRB);
+
+ /* Disable transmit interrupt if the ring buffer is empty */
+ if (RingBuffer_IsEmpty(pTXRB)) {
+ Chip_UART_IntDisable(pUART, UART_IER_THREINT);
+ }
+ }
+
+ /* Handle receive interrupt */
+ Chip_UART_RXIntHandlerRB(pUART, pRXRB);
+}
+
+/* Determines and sets best dividers to get a target baud rate */
+uint32_t Chip_UART_SetBaudFDR(LPC_USART_T *pUART, uint32_t baudrate)
+
+{
+ uint32_t uClk = 0;
+ uint32_t dval = 0;
+ uint32_t mval = 0;
+ uint32_t dl = 0;
+ uint32_t rate16 = 16 * baudrate;
+ uint32_t actualRate = 0;
+
+ /* Get Clock rate */
+ uClk = Chip_Clock_GetMainClockRate();
+
+ /* The fractional is calculated as (PCLK % (16 * Baudrate)) / (16 * Baudrate)
+ * Let's make it to be the ratio DivVal / MulVal
+ */
+ dval = uClk % rate16;
+
+ /* The PCLK / (16 * Baudrate) is fractional
+ * => dval = pclk % rate16
+ * mval = rate16;
+ * now mormalize the ratio
+ * dval / mval = 1 / new_mval
+ * new_mval = mval / dval
+ * new_dval = 1
+ */
+ if (dval > 0) {
+ mval = rate16 / dval;
+ dval = 1;
+
+ /* In case mval still bigger then 4 bits
+ * no adjustment require
+ */
+ if (mval > 12) {
+ dval = 0;
+ }
+ }
+ dval &= 0xf;
+ mval &= 0xf;
+ dl = uClk / (rate16 + rate16 *dval / mval);
+
+ /* Update UART registers */
+ Chip_UART_EnableDivisorAccess(pUART);
+ Chip_UART_SetDivisorLatches(pUART, UART_LOAD_DLL(dl), UART_LOAD_DLM(dl));
+ Chip_UART_DisableDivisorAccess(pUART);
+
+ /* Set best fractional divider */
+ pUART->FDR = (UART_FDR_MULVAL(mval) | UART_FDR_DIVADDVAL(dval));
+
+ /* Return actual baud rate */
+ actualRate = uClk / (16 * dl + 16 * dl * dval / mval);
+ return actualRate;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/wwdt_11xx.c Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,86 @@
+/*
+ * @brief LPC11xx WWDT chip driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products. This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights. NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers. This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+/*
+ * Compiler warning fixes
+ * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ */
+
+#include "chip.h"
+
+/*****************************************************************************
+ * Private types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Public types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Private functions
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Public functions
+ ****************************************************************************/
+
+/* Initialize the Watchdog timer */
+void Chip_WWDT_Init(LPC_WWDT_T *pWWDT)
+{
+ Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_WDT);
+
+ /* Disable watchdog */
+ pWWDT->MOD = 0;
+ pWWDT->TC = 0xFF;
+#if defined(WATCHDOG_WINDOW_SUPPORT)
+ pWWDT->WARNINT = 0xFFFF;
+ pWWDT->WINDOW = 0xFFFFFF;
+#endif
+}
+
+/* Shutdown the Watchdog timer */
+void Chip_WWDT_DeInit(LPC_WWDT_T *pWWDT)
+{
+ (void)pWWDT;
+ Chip_Clock_DisablePeriphClock(SYSCTL_CLOCK_WDT);
+}
+
+/* Clear WWDT interrupt status flags */
+void Chip_WWDT_ClearStatusFlag(LPC_WWDT_T *pWWDT, uint32_t status)
+{
+ if (status & WWDT_WDMOD_WDTOF) {
+ pWWDT->MOD &= (~WWDT_WDMOD_WDTOF) & WWDT_WDMOD_BITMASK;
+ }
+
+ if (status & WWDT_WDMOD_WDINT) {
+ pWWDT->MOD |= WWDT_WDMOD_WDINT;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,277 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdio>
+#include <cstdlib>
+#include <algorithm>
+#include <board.hpp>
+#include <chip.h>
+#include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include <uavcan/protocol/logger.hpp>
+
+/*
+ * GCC 4.9 cannot generate a working binary with higher optimization levels, although
+ * rest of the firmware can be compiled with -Os.
+ * GCC 4.8 and earlier don't work at all on this firmware.
+ */
+#if __GNUC__
+# pragma GCC optimize 1
+#endif
+
+/**
+ * This function re-defines the standard ::rand(), which is used by the class uavcan::DynamicNodeIDClient.
+ * Redefinition is normally not needed, but GCC 4.9 tends to generate broken binaries if it is not redefined.
+ */
+int rand()
+{
+ static int x = 1;
+ x = x * 48271 % 2147483647;
+ return x;
+}
+
+namespace
+{
+
+static constexpr unsigned NodeMemoryPoolSize = 2800;
+
+/**
+ * This is a compact, reentrant and thread-safe replacement to standard llto().
+ * It returns the string by value, no extra storage is needed.
+ */
+typename uavcan::MakeString<22>::Type intToString(long long n)
+{
+ char buf[24] = {};
+ const short sign = (n < 0) ? -1 : 1;
+ if (sign < 0)
+ {
+ n = -n;
+ }
+ unsigned pos = 0;
+ do
+ {
+ buf[pos++] = char(n % 10 + '0');
+ }
+ while ((n /= 10) > 0);
+ if (sign < 0)
+ {
+ buf[pos++] = '-';
+ }
+ buf[pos] = '\0';
+ for (unsigned i = 0, j = pos - 1U; i < j; i++, j--)
+ {
+ std::swap(buf[i], buf[j]);
+ }
+ return static_cast<const char*>(buf);
+}
+
+uavcan::Node<NodeMemoryPoolSize>& getNode()
+{
+ static uavcan::Node<NodeMemoryPoolSize> node(uavcan_lpc11c24::CanDriver::instance(),
+ uavcan_lpc11c24::SystemClock::instance());
+ return node;
+}
+
+uavcan::GlobalTimeSyncSlave& getTimeSyncSlave()
+{
+ static uavcan::GlobalTimeSyncSlave tss(getNode());
+ return tss;
+}
+
+uavcan::Logger& getLogger()
+{
+ static uavcan::Logger logger(getNode());
+ return logger;
+}
+
+uavcan::NodeID performDynamicNodeIDAllocation()
+{
+ uavcan::DynamicNodeIDClient client(getNode());
+
+ const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
+ if (client_start_res < 0)
+ {
+ board::die();
+ }
+
+ while (!client.isAllocationComplete())
+ {
+ board::resetWatchdog();
+ (void)getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
+ }
+
+ return client.getAllocatedNodeID();
+}
+
+void init()
+{
+ board::resetWatchdog();
+ board::syslog("Boot\r\n");
+
+ board::setErrorLed(false);
+ board::setStatusLed(true);
+
+ /*
+ * Configuring the clock - this must be done before the CAN controller is initialized
+ */
+ uavcan_lpc11c24::clock::init();
+
+ /*
+ * Configuring the CAN controller
+ */
+ std::uint32_t bit_rate = 0;
+ while (bit_rate == 0)
+ {
+ board::syslog("CAN auto bitrate...\r\n");
+ bit_rate = uavcan_lpc11c24::CanDriver::detectBitRate(&board::resetWatchdog);
+ }
+ board::syslog("Bitrate: ");
+ board::syslog(intToString(bit_rate).c_str());
+ board::syslog("\r\n");
+
+ if (uavcan_lpc11c24::CanDriver::instance().init(bit_rate) < 0)
+ {
+ board::die();
+ }
+
+ board::syslog("CAN init ok\r\n");
+
+ board::resetWatchdog();
+
+ /*
+ * Configuring the node
+ */
+ getNode().setName("org.uavcan.lpc11c24_test");
+
+ uavcan::protocol::SoftwareVersion swver;
+ swver.major = FW_VERSION_MAJOR;
+ swver.minor = FW_VERSION_MINOR;
+ swver.vcs_commit = GIT_HASH;
+ swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
+ getNode().setSoftwareVersion(swver);
+
+ uavcan::protocol::HardwareVersion hwver;
+ std::uint8_t uid[board::UniqueIDSize] = {};
+ board::readUniqueID(uid);
+ std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
+ getNode().setHardwareVersion(hwver);
+
+ board::resetWatchdog();
+
+ /*
+ * Starting the node and performing dynamic node ID allocation
+ */
+ if (getNode().start() < 0)
+ {
+ board::die();
+ }
+
+ board::syslog("Node ID allocation...\r\n");
+
+ getNode().setNodeID(performDynamicNodeIDAllocation());
+
+ board::syslog("Node ID ");
+ board::syslog(intToString(getNode().getNodeID().get()).c_str());
+ board::syslog("\r\n");
+
+ board::resetWatchdog();
+
+ /*
+ * Example filter configuration.
+ * Can be removed safely.
+ */
+ {
+ constexpr unsigned NumFilters = 3;
+ uavcan::CanFilterConfig filters[NumFilters];
+
+ // Acepting all service transfers addressed to us
+ filters[0].id = (unsigned(getNode().getNodeID().get()) << 8) | (1U << 7) | uavcan::CanFrame::FlagEFF;
+ filters[0].mask = 0x7F80 | uavcan::CanFrame::FlagEFF;
+
+ // Accepting time sync messages
+ filters[1].id = (4U << 8) | uavcan::CanFrame::FlagEFF;
+ filters[1].mask = 0xFFFF80 | uavcan::CanFrame::FlagEFF;
+
+ // Accepting zero CAN ID (just for the sake of testing)
+ filters[2].id = 0 | uavcan::CanFrame::FlagEFF;
+ filters[2].mask = uavcan::CanFrame::MaskExtID | uavcan::CanFrame::FlagEFF;
+
+ if (uavcan_lpc11c24::CanDriver::instance().configureFilters(filters, NumFilters) < 0)
+ {
+ board::syslog("Filter init failed\r\n");
+ board::die();
+ }
+ }
+
+ /*
+ * Initializing other libuavcan-related objects
+ */
+ if (getTimeSyncSlave().start() < 0)
+ {
+ board::die();
+ }
+
+ if (getLogger().init() < 0)
+ {
+ board::die();
+ }
+
+ getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+ board::resetWatchdog();
+}
+
+}
+
+int main()
+{
+ init();
+
+ getNode().setModeOperational();
+
+ uavcan::MonotonicTime prev_log_at;
+
+ while (true)
+ {
+ const int res = getNode().spin(uavcan::MonotonicDuration::fromMSec(25));
+ board::setErrorLed(res < 0);
+ board::setStatusLed(uavcan_lpc11c24::CanDriver::instance().hadActivity());
+
+ const auto ts = uavcan_lpc11c24::clock::getMonotonic();
+ if ((ts - prev_log_at).toMSec() >= 1000)
+ {
+ prev_log_at = ts;
+
+ /*
+ * CAN bus off state monitoring
+ */
+ if (uavcan_lpc11c24::CanDriver::instance().isInBusOffState())
+ {
+ board::syslog("CAN BUS OFF\r\n");
+ }
+
+ /*
+ * CAN error counter, for debugging purposes
+ */
+ board::syslog("CAN errors: ");
+ board::syslog(intToString(static_cast<long long>(uavcan_lpc11c24::CanDriver::instance().getErrorCount())).c_str());
+ board::syslog(" ");
+ board::syslog(intToString(uavcan_lpc11c24::CanDriver::instance().getRxQueueOverflowCount()).c_str());
+ board::syslog("\r\n");
+
+ /*
+ * We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf(),
+ * so we need to construct the message manually:
+ */
+ uavcan::protocol::debug::LogMessage logmsg;
+ logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO;
+ logmsg.source = "app";
+ logmsg.text = intToString(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec()).c_str();
+ (void)getLogger().log(logmsg);
+ }
+
+ board::resetWatchdog();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,195 @@
+/*
+ * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ * Board initialization for Olimex LPC11C24
+ */
+
+#include "board.hpp"
+#include <chip.h>
+#include <cstdlib>
+#include <cstring>
+#include <numeric>
+
+static constexpr unsigned long PDRUNCFGUSEMASK = 0x0000ED00U;
+static constexpr unsigned long PDRUNCFGMASKTMP = 0x000000FFU;
+
+const std::uint32_t OscRateIn = 12000000; ///< External crystal
+const std::uint32_t ExtRateIn = 0;
+
+std::uint32_t SystemCoreClock = 12000000; ///< Initialized to default clock value, will be changed on init
+
+namespace board
+{
+namespace
+{
+
+constexpr unsigned TargetSystemCoreClock = 48000000;
+
+constexpr unsigned ErrorLedPort = 1;
+constexpr unsigned ErrorLedPin = 10;
+
+constexpr unsigned StatusLedPort = 1;
+constexpr unsigned StatusLedPin = 11;
+
+struct PinMuxGroup
+{
+ unsigned pin : 8;
+ unsigned modefunc : 24;
+};
+
+constexpr PinMuxGroup pinmux[] =
+{
+ { IOCON_PIO1_10, IOCON_FUNC0 | IOCON_MODE_INACT }, // Error LED
+ { IOCON_PIO1_11, IOCON_FUNC0 | IOCON_MODE_INACT }, // Status LED
+ { IOCON_PIO1_7, IOCON_FUNC1 | IOCON_HYS_EN | IOCON_MODE_PULLUP }, // UART_TXD
+};
+
+
+void sysctlPowerDown(unsigned long powerdownmask)
+{
+ unsigned long pdrun = LPC_SYSCTL->PDRUNCFG & PDRUNCFGMASKTMP;
+ pdrun |= (powerdownmask & PDRUNCFGMASKTMP);
+ LPC_SYSCTL->PDRUNCFG = pdrun | PDRUNCFGUSEMASK;
+}
+
+void sysctlPowerUp(unsigned long powerupmask)
+{
+ unsigned long pdrun = LPC_SYSCTL->PDRUNCFG & PDRUNCFGMASKTMP;
+ pdrun &= ~(powerupmask & PDRUNCFGMASKTMP);
+ LPC_SYSCTL->PDRUNCFG = pdrun | PDRUNCFGUSEMASK;
+}
+
+void initWatchdog()
+{
+ Chip_WWDT_Init(LPC_WWDT); // Initialize watchdog
+ sysctlPowerUp(SYSCTL_POWERDOWN_WDTOSC_PD); // Enable watchdog oscillator
+ Chip_Clock_SetWDTOSC(WDTLFO_OSC_0_60, 4); // WDT osc rate 0.6 MHz / 4 = 150 kHz
+ Chip_Clock_SetWDTClockSource(SYSCTL_WDTCLKSRC_WDTOSC, 1); // Clocking watchdog from its osc, div rate 1
+ Chip_WWDT_SetTimeOut(LPC_WWDT, 37500); // 1 sec (hardcoded to reduce code size)
+ Chip_WWDT_SetOption(LPC_WWDT, WWDT_WDMOD_WDRESET); // Mode: reset on timeout
+ Chip_WWDT_Start(LPC_WWDT); // Go
+}
+
+void initClock()
+{
+ sysctlPowerUp(SYSCTL_POWERDOWN_SYSOSC_PD); // Enable system oscillator
+ for (volatile int i = 0; i < 1000; i++) { }
+
+ Chip_Clock_SetSystemPLLSource(SYSCTL_PLLCLKSRC_MAINOSC);
+ sysctlPowerDown(SYSCTL_POWERDOWN_SYSPLL_PD);
+
+ /*
+ * Setup PLL for main oscillator rate (FCLKIN = 12MHz) * 4 = 48MHz
+ * MSEL = 3 (this is pre-decremented), PSEL = 1 (for P = 2)
+ * FCLKOUT = FCLKIN * (MSEL + 1) = 12MHz * 4 = 48MHz
+ * FCCO = FCLKOUT * 2 * P = 48MHz * 2 * 2 = 192MHz (within FCCO range)
+ */
+ Chip_Clock_SetupSystemPLL(3, 1);
+ sysctlPowerUp(SYSCTL_POWERDOWN_SYSPLL_PD);
+ while (!Chip_Clock_IsSystemPLLLocked()) { }
+
+ Chip_Clock_SetSysClockDiv(1);
+
+ Chip_FMC_SetFLASHAccess(FLASHTIM_50MHZ_CPU);
+
+ Chip_Clock_SetMainClockSource(SYSCTL_MAINCLKSRC_PLLOUT);
+
+ SystemCoreClock = Chip_Clock_GetSystemClockRate();
+
+ while (SystemCoreClock != TargetSystemCoreClock) { } // Loop forever if the clock failed to initialize properly
+}
+
+void initGpio()
+{
+ LPC_SYSCTL->SYSAHBCLKCTRL |= 1 << SYSCTL_CLOCK_IOCON;
+ LPC_SYSCTL->SYSAHBCLKCTRL |= 1 << SYSCTL_CLOCK_GPIO;
+
+ for (unsigned i = 0; i < (sizeof(pinmux) / sizeof(PinMuxGroup)); i++)
+ {
+ LPC_IOCON->REG[pinmux[i].pin] = pinmux[i].modefunc;
+ }
+
+ LPC_GPIO[ErrorLedPort].DIR |= 1 << ErrorLedPin;
+ LPC_GPIO[StatusLedPort].DIR |= 1 << StatusLedPin;
+}
+
+void initUart()
+{
+ Chip_UART_Init(LPC_USART);
+ Chip_UART_SetBaud(LPC_USART, 115200);
+ Chip_UART_TXEnable(LPC_USART);
+}
+
+void init()
+{
+ Chip_SYSCTL_SetBODLevels(SYSCTL_BODRSTLVL_2_06V, SYSCTL_BODINTVAL_RESERVED1);
+ Chip_SYSCTL_EnableBODReset();
+
+ initWatchdog();
+ initClock();
+ initGpio();
+ initUart();
+
+ resetWatchdog();
+}
+
+} // namespace
+
+void die()
+{
+ static const volatile unsigned& DHCSR = *reinterpret_cast<unsigned*>(0xE000EDF0U);
+
+ syslog("FATAL\r\n");
+
+ while (true)
+ {
+ if ((DHCSR & 1U) != 0)
+ {
+ __asm volatile ("bkpt #0\n"); // Break into the debugger
+ }
+ }
+}
+
+#if __GNUC__
+__attribute__((optimize(0))) // Optimization must be disabled lest it hardfaults in the IAP call
+#endif
+void readUniqueID(std::uint8_t out_uid[UniqueIDSize])
+{
+ unsigned aligned_array[5] = {}; // out_uid may be unaligned, so we need to use temp array
+ unsigned iap_command = 58;
+ reinterpret_cast<void(*)(void*, void*)>(0x1FFF1FF1)(&iap_command, aligned_array);
+ std::memcpy(out_uid, &aligned_array[1], 16);
+}
+
+void setStatusLed(bool state)
+{
+ LPC_GPIO[StatusLedPort].DATA[1 << StatusLedPin] = static_cast<unsigned long>(!state) << StatusLedPin;
+}
+
+void setErrorLed(bool state)
+{
+ LPC_GPIO[ErrorLedPort].DATA[1 << ErrorLedPin] = static_cast<unsigned long>(!state) << ErrorLedPin;
+}
+
+void resetWatchdog()
+{
+ Chip_WWDT_Feed(LPC_WWDT);
+}
+
+void syslog(const char* msg)
+{
+ Chip_UART_SendBlocking(LPC_USART, msg, static_cast<int>(std::strlen(msg)));
+}
+
+} // namespace board
+
+extern "C"
+{
+
+void SystemInit();
+
+void SystemInit()
+{
+ board::init();
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,32 @@
+/*
+ * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdint>
+
+namespace board
+{
+
+#if __GNUC__
+__attribute__((noreturn))
+#endif
+void die();
+
+static constexpr unsigned UniqueIDSize = 16;
+
+/**
+ * Reads the globally unique 128-bit hardware ID from the MCU.
+ */
+void readUniqueID(std::uint8_t out_uid[UniqueIDSize]);
+
+void setStatusLed(bool state);
+void setErrorLed(bool state);
+
+void resetWatchdog();
+
+/**
+ * Sends the string to UART.
+ */
+void syslog(const char* msg);
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/crt0.c Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,223 @@
+/*
+ * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ * ARM Cortex-M0(+)/M1/M3 startup file.
+ */
+
+typedef void (*funptr_t)(void);
+
+#define fill32(start, end, filler) { \
+ unsigned *p1 = start; \
+ const unsigned * const p2 = end; \
+ while (p1 < p2) \
+ *p1++ = filler; \
+}
+
+extern const unsigned _etext;
+
+extern unsigned _data;
+extern unsigned _edata;
+
+extern unsigned _bss;
+extern unsigned _ebss;
+
+extern funptr_t __init_array_start;
+extern funptr_t __init_array_end;
+
+__attribute__((noreturn))
+extern int main(void);
+
+extern void SystemInit(void);
+
+#pragma GCC optimize 1
+
+/**
+ * Prototypes for the functions below
+ */
+void Reset_Handler(void);
+void Default_Handler(void);
+void NMI_Handler(void);
+void HardFault_Handler(void);
+void SVC_Handler(void);
+void PendSV_Handler(void);
+void SysTick_Handler(void);
+
+/**
+ * Firmware entry point
+ */
+__attribute__((naked, noreturn))
+void Reset_Handler(void)
+{
+ // Data section
+ {
+ const unsigned* tp = &_etext;
+ unsigned* dp = &_data;
+ while (dp < &_edata)
+ {
+ *dp++ = *tp++;
+ }
+ }
+
+ // BSS section
+ fill32(&_bss, &_ebss, 0);
+
+ SystemInit();
+
+ // Constructors
+ {
+ funptr_t* fpp = &__init_array_start;
+ while (fpp < &__init_array_end)
+ {
+ (*fpp)();
+ fpp++;
+ }
+ }
+
+ (void)main();
+
+ while (1) { }
+}
+
+/**
+ * Default handlers
+ */
+__attribute__((weak))
+void Default_Handler(void)
+{
+ while(1) { }
+}
+
+__attribute__((weak))
+void NMI_Handler(void)
+{
+ while(1) { }
+}
+
+__attribute__((weak))
+void HardFault_Handler(void)
+{
+ while(1) { }
+}
+
+__attribute__((weak))
+void SVC_Handler(void)
+{
+ while(1) { }
+}
+
+__attribute__((weak))
+void PendSV_Handler(void)
+{
+ while(1) { }
+}
+
+__attribute__((weak))
+void SysTick_Handler(void)
+{
+ while(1) { }
+}
+
+/**
+ * Default vectors for LPC11C24, to be overriden by the firmware as needed
+ */
+#define ALIAS(f) __attribute__ ((weak, alias (#f)))
+
+void CAN_IRQHandler(void) ALIAS(Default_Handler);
+void SSP1_IRQHandler(void) ALIAS(Default_Handler);
+void I2C_IRQHandler(void) ALIAS(Default_Handler);
+void TIMER16_0_IRQHandler(void) ALIAS(Default_Handler);
+void TIMER16_1_IRQHandler(void) ALIAS(Default_Handler);
+void TIMER32_0_IRQHandler(void) ALIAS(Default_Handler);
+void TIMER32_1_IRQHandler(void) ALIAS(Default_Handler);
+void SSP0_IRQHandler(void) ALIAS(Default_Handler);
+void UART_IRQHandler(void) ALIAS(Default_Handler);
+void ADC_IRQHandler(void) ALIAS(Default_Handler);
+void WDT_IRQHandler(void) ALIAS(Default_Handler);
+void BOD_IRQHandler(void) ALIAS(Default_Handler);
+void PIOINT3_IRQHandler(void) ALIAS(Default_Handler);
+void PIOINT2_IRQHandler(void) ALIAS(Default_Handler);
+void PIOINT1_IRQHandler(void) ALIAS(Default_Handler);
+void PIOINT0_IRQHandler(void) ALIAS(Default_Handler);
+void WAKEUP_IRQHandler(void) ALIAS(Default_Handler);
+
+/**
+ * Refer to the linker script
+ */
+extern void __stack_end(void);
+
+/**
+ * Vector table for LPC11Cxx
+ * Must be explicitly defined 'used', otherwise LTO optimizer will discard it.
+ */
+__attribute__ ((used, section("vectors")))
+void (* const VectorTable[64])(void) =
+{
+ __stack_end, // The initial stack pointer
+ Reset_Handler, // The reset handler
+ NMI_Handler, // The NMI handler
+ HardFault_Handler, // The hard fault handler
+ 0, // Reserved
+ 0, // Reserved
+ 0, // Reserved
+ 0, // Reserved
+ 0, // Reserved
+ 0, // Reserved
+ 0, // Reserved
+ SVC_Handler, // SVCall handler
+ 0, // Reserved
+ 0, // Reserved
+ PendSV_Handler, // The PendSV handler
+ SysTick_Handler, // The SysTick handler
+
+ WAKEUP_IRQHandler, // PIO0_0 Wakeup
+ WAKEUP_IRQHandler, // PIO0_1 Wakeup
+ WAKEUP_IRQHandler, // PIO0_2 Wakeup
+ WAKEUP_IRQHandler, // PIO0_3 Wakeup
+ WAKEUP_IRQHandler, // PIO0_4 Wakeup
+ WAKEUP_IRQHandler, // PIO0_5 Wakeup
+ WAKEUP_IRQHandler, // PIO0_6 Wakeup
+ WAKEUP_IRQHandler, // PIO0_7 Wakeup
+ WAKEUP_IRQHandler, // PIO0_8 Wakeup
+ WAKEUP_IRQHandler, // PIO0_9 Wakeup
+ WAKEUP_IRQHandler, // PIO0_10 Wakeup
+ WAKEUP_IRQHandler, // PIO0_11 Wakeup
+ WAKEUP_IRQHandler, // PIO1_0 Wakeup
+
+ CAN_IRQHandler, // C_CAN Interrupt
+ SSP1_IRQHandler, // SPI/SSP1 Interrupt
+ I2C_IRQHandler, // I2C0
+ TIMER16_0_IRQHandler, // CT16B0 (16-bit Timer 0)
+ TIMER16_1_IRQHandler, // CT16B1 (16-bit Timer 1)
+ TIMER32_0_IRQHandler, // CT32B0 (32-bit Timer 0)
+ TIMER32_1_IRQHandler, // CT32B1 (32-bit Timer 1)
+ SSP0_IRQHandler, // SPI/SSP0 Interrupt
+ UART_IRQHandler, // UART0
+
+ 0, // Reserved
+ 0, // Reserved
+
+ ADC_IRQHandler, // ADC (A/D Converter)
+ WDT_IRQHandler, // WDT (Watchdog Timer)
+ BOD_IRQHandler, // BOD (Brownout Detect)
+ 0, // Reserved
+ PIOINT3_IRQHandler, // PIO INT3
+ PIOINT2_IRQHandler, // PIO INT2
+ PIOINT1_IRQHandler, // PIO INT1
+ PIOINT0_IRQHandler, // PIO INT0
+
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,148 @@
+/*
+ * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ * Standard library stubs
+ */
+
+#include <cstdlib>
+#include <unistd.h>
+#include <sys/types.h>
+
+#if __GNUC__
+# pragma GCC diagnostic ignored "-Wmissing-declarations"
+#endif
+
+void* __dso_handle;
+
+void* operator new(std::size_t)
+{
+ std::abort();
+ return reinterpret_cast<void*>(0xFFFFFFFF);
+}
+
+void* operator new[](std::size_t)
+{
+ std::abort();
+ return reinterpret_cast<void*>(0xFFFFFFFF);
+}
+
+void operator delete(void*)
+{
+ std::abort();
+}
+
+void operator delete[](void*)
+{
+ std::abort();
+}
+
+namespace __gnu_cxx
+{
+
+void __verbose_terminate_handler()
+{
+ std::abort();
+}
+
+}
+
+/*
+ * libstdc++ stubs
+ */
+extern "C"
+{
+
+int __aeabi_atexit(void*, void(*)(void*), void*)
+{
+ return 0;
+}
+
+__extension__ typedef int __guard __attribute__((mode (__DI__)));
+
+void __cxa_atexit(void(*)(void *), void*, void*)
+{
+}
+
+int __cxa_guard_acquire(__guard* g)
+{
+ return !*g;
+}
+
+void __cxa_guard_release (__guard* g)
+{
+ *g = 1;
+}
+
+void __cxa_guard_abort (__guard*)
+{
+}
+
+void __cxa_pure_virtual()
+{
+ std::abort();
+}
+
+}
+
+/*
+ * stdio
+ */
+extern "C"
+{
+
+__attribute__((used))
+void abort()
+{
+ while (true) { }
+}
+
+int _read_r(struct _reent*, int, char*, int)
+{
+ return -1;
+}
+
+int _lseek_r(struct _reent*, int, int, int)
+{
+ return -1;
+}
+
+int _write_r(struct _reent*, int, char*, int)
+{
+ return -1;
+}
+
+int _close_r(struct _reent*, int)
+{
+ return -1;
+}
+
+__attribute__((used))
+caddr_t _sbrk_r(struct _reent*, int)
+{
+ return 0;
+}
+
+int _fstat_r(struct _reent*, int, struct stat*)
+{
+ return -1;
+}
+
+int _isatty_r(struct _reent*, int)
+{
+ return -1;
+}
+
+void _exit(int)
+{
+ abort();
+}
+
+pid_t _getpid(void)
+{
+ return 1;
+}
+
+void _kill(pid_t)
+{
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/posix/CMakeLists.txt Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,12 @@ +# +# Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com> +# + +cmake_minimum_required(VERSION 2.8) + +project(libuavcan_posix) + +# +# Library (header only) +# +install(DIRECTORY include/uavcan_posix DESTINATION include)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/posix/include.mk Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,7 @@ +# +# Copyright (C) 2015 David Sidrane <david_s5@nscdg.net> +# + +LIBUAVCAN_POSIX_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_POSIX_INC := $(LIBUAVCAN_POSIX_DIR)include/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,461 @@
+/****************************************************************************
+*
+* Copyright (c) 2015 PX4 Development Team. All rights reserved.
+* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+* David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED
+#define UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED
+
+#include <sys/stat.h>
+#include <cstdio>
+#include <cstddef>
+#include <cstdlib>
+#include <cstring>
+#include <cerrno>
+#include <ctime>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <uavcan/node/timer.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/protocol/file/Error.hpp>
+#include <uavcan/protocol/file/EntryType.hpp>
+#include <uavcan/protocol/file/Read.hpp>
+#include <uavcan/protocol/file_server.hpp>
+#include <uavcan/data_type.hpp>
+
+namespace uavcan_posix
+{
+/**
+ * This interface implements a POSIX compliant IFileServerBackend interface
+ */
+class BasicFileServerBackend : public uavcan::IFileServerBackend
+{
+ enum { FilePermissions = 438 }; ///< 0o666
+
+protected:
+ class FDCacheBase
+ {
+ public:
+ FDCacheBase() { }
+ virtual ~FDCacheBase() { }
+
+ virtual int open(const char* path, int oflags)
+ {
+ using namespace std;
+
+ return ::open(path, oflags);
+ }
+
+ virtual int close(int fd, bool done = true)
+ {
+ (void)done;
+ using namespace std;
+
+ return ::close(fd);
+ }
+
+ virtual void init() { }
+ };
+
+ FDCacheBase fallback_;
+
+ class FDCache : public FDCacheBase, protected uavcan::TimerBase
+ {
+ /// Age in Seconds an entry will stay in the cache if not accessed.
+ enum { MaxAgeSeconds = 7 };
+
+ /// Rate in Seconds that the cache will be flushed of stale entries.
+ enum { GarbageCollectionSeconds = 60 };
+
+ class FDCacheItem : uavcan::Noncopyable
+ {
+ friend FDCache;
+
+ FDCacheItem* next_;
+ std::time_t last_access_;
+ const int fd_;
+ const int oflags_;
+ const char* const path_;
+
+ public:
+ enum { InvalidFD = -1 };
+
+ FDCacheItem() :
+ next_(UAVCAN_NULLPTR),
+ last_access_(0),
+ fd_(InvalidFD),
+ oflags_(0),
+ path_(UAVCAN_NULLPTR)
+ { }
+
+ FDCacheItem(int fd, const char* path, int oflags) :
+ next_(UAVCAN_NULLPTR),
+ last_access_(0),
+ fd_(fd),
+ oflags_(oflags),
+ path_(::strndup(path, uavcan::protocol::file::Path::FieldTypes::path::MaxSize))
+ { }
+
+ ~FDCacheItem()
+ {
+ using namespace std;
+ if (valid())
+ {
+ ::free(const_cast<char*>(path_));
+ }
+ }
+
+ bool valid() const
+ {
+ return path_ != UAVCAN_NULLPTR;
+ }
+
+ int getFD() const
+ {
+ return fd_;
+ }
+
+ std::time_t getAccess() const
+ {
+ return last_access_;
+ }
+
+ std::time_t acessed()
+ {
+ using namespace std;
+ last_access_ = time(UAVCAN_NULLPTR);
+ return getAccess();
+ }
+
+ void expire()
+ {
+ last_access_ = 0;
+ }
+
+ bool expired() const
+ {
+ using namespace std;
+ return 0 == last_access_ || (time(UAVCAN_NULLPTR) - last_access_) > MaxAgeSeconds;
+ }
+
+ bool equals(const char* path, int oflags) const
+ {
+ using namespace std;
+ return oflags_ == oflags && 0 == ::strcmp(path, path_);
+ }
+
+ bool equals(int fd) const
+ {
+ return fd_ == fd;
+ }
+ };
+
+ FDCacheItem* head_;
+
+ FDCacheItem* find(const char* path, int oflags)
+ {
+ for (FDCacheItem* pi = head_; pi; pi = pi->next_)
+ {
+ if (pi->equals(path, oflags))
+ {
+ return pi;
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+
+ FDCacheItem* find(int fd)
+ {
+ for (FDCacheItem* pi = head_; pi; pi = pi->next_)
+ {
+ if (pi->equals(fd))
+ {
+ return pi;
+ }
+ }
+ return UAVCAN_NULLPTR;
+ }
+
+ FDCacheItem* add(FDCacheItem* pi)
+ {
+ pi->next_ = head_;
+ head_ = pi;
+ pi->acessed();
+ return pi;
+ }
+
+ void removeExpired(FDCacheItem** pi)
+ {
+ while (*pi)
+ {
+ if ((*pi)->expired())
+ {
+ FDCacheItem* next = (*pi)->next_;
+ (void)FDCacheBase::close((*pi)->fd_);
+ delete (*pi);
+ *pi = next;
+ continue;
+ }
+ pi = &(*pi)->next_;
+ }
+ }
+
+ void remove(FDCacheItem* pi, bool done)
+ {
+ if (done)
+ {
+ pi->expire();
+ }
+ removeExpired(&head_);
+ }
+
+ void clear()
+ {
+ FDCacheItem* tmp;
+ for (FDCacheItem* pi = head_; pi; pi = tmp)
+ {
+ tmp = pi->next_;
+ (void)FDCacheBase::close(pi->fd_);
+ delete pi;
+ }
+ }
+
+ /* Removed stale entries. In the normal case a node will read the
+ * complete contents of a file and the read of the last block will
+ * cause the method remove() to be invoked with done true. Thereby
+ * flushing the entry from the cache. But if the node does not
+ * stay the course of the read, it may leave a dangling entry.
+ * This call back handles the garbage collection.
+ */
+ virtual void handleTimerEvent(const uavcan::TimerEvent&)
+ {
+ removeExpired(&head_);
+ }
+
+ public:
+ FDCache(uavcan::INode& node) :
+ TimerBase(node),
+ head_(UAVCAN_NULLPTR)
+ { }
+
+ virtual ~FDCache()
+ {
+ stop();
+ clear();
+ }
+
+ virtual void init()
+ {
+ startPeriodic(uavcan::MonotonicDuration::fromMSec(GarbageCollectionSeconds * 1000));
+ }
+
+ virtual int open(const char* path, int oflags)
+ {
+ int fd = FDCacheItem::InvalidFD;
+
+ FDCacheItem* pi = find(path, oflags);
+
+ if (pi != UAVCAN_NULLPTR)
+ {
+ pi->acessed();
+ }
+ else
+ {
+ fd = FDCacheBase::open(path, oflags);
+ if (fd < 0)
+ {
+ return fd;
+ }
+
+ /* Allocate and clone path */
+
+ pi = new FDCacheItem(fd, path, oflags);
+
+ /* Allocation worked but check clone */
+
+ if (pi && !pi->valid())
+ {
+ /* Allocation worked but clone or path failed */
+ delete pi;
+ pi = UAVCAN_NULLPTR;
+ }
+
+ if (pi == UAVCAN_NULLPTR)
+ {
+ /*
+ * If allocation fails no harm just can not cache it
+ * return open fd
+ */
+ return fd;
+ }
+ /* add new */
+ add(pi);
+ }
+ return pi->getFD();
+ }
+
+ virtual int close(int fd, bool done)
+ {
+ FDCacheItem* pi = find(fd);
+ if (pi == UAVCAN_NULLPTR)
+ {
+ /*
+ * If not found just close it
+ */
+ return FDCacheBase::close(fd);
+ }
+ remove(pi, done);
+ return 0;
+ }
+ };
+
+ FDCacheBase* fdcache_;
+ uavcan::INode& node_;
+
+ FDCacheBase& getFDCache()
+ {
+ if (fdcache_ == UAVCAN_NULLPTR)
+ {
+ fdcache_ = new FDCache(node_);
+
+ if (fdcache_ == UAVCAN_NULLPTR)
+ {
+ fdcache_ = &fallback_;
+ }
+
+ fdcache_->init();
+ }
+ return *fdcache_;
+ }
+
+ /**
+ * Back-end for uavcan.protocol.file.GetInfo.
+ * Implementation of this method is required.
+ * On success the method must return zero.
+ */
+ virtual uavcan::int16_t getInfo(const Path& path, uavcan::uint64_t& out_size, EntryType& out_type)
+ {
+ int rv = uavcan::protocol::file::Error::INVALID_VALUE;
+
+ if (path.size() > 0)
+ {
+ using namespace std;
+
+ struct stat sb;
+
+ rv = stat(path.c_str(), &sb);
+
+ if (rv < 0)
+ {
+ rv = errno;
+ }
+ else
+ {
+ rv = 0;
+ out_size = sb.st_size;
+ out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE;
+ if (S_ISDIR(sb.st_mode))
+ {
+ out_type.flags |= uavcan::protocol::file::EntryType::FLAG_DIRECTORY;
+ }
+ else if (S_ISREG(sb.st_mode))
+ {
+ out_type.flags |= uavcan::protocol::file::EntryType::FLAG_FILE;
+ }
+ // TODO Using fixed flag FLAG_READABLE until we add file permission checks to return actual value.
+ }
+ }
+ return rv;
+ }
+
+ /**
+ * Back-end for uavcan.protocol.file.Read.
+ * Implementation of this method is required.
+ * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except
+ * if the end of file is reached.
+ * On success the method must return zero.
+ */
+ virtual uavcan::int16_t read(const Path& path, const uavcan::uint64_t offset, uavcan::uint8_t* out_buffer,
+ uavcan::uint16_t& inout_size)
+ {
+ int rv = uavcan::protocol::file::Error::INVALID_VALUE;
+
+ if (path.size() > 0 && inout_size != 0)
+ {
+ using namespace std;
+
+ FDCacheBase& cache = getFDCache();
+ int fd = cache.open(path.c_str(), O_RDONLY);
+
+ if (fd < 0)
+ {
+ rv = errno;
+ }
+ else
+ {
+ ssize_t total_read = 0;
+
+ rv = ::lseek(fd, offset, SEEK_SET);
+
+ if (rv < 0)
+ {
+ rv = errno;
+ }
+ else
+ {
+ rv = 0;
+ ssize_t remaining = inout_size;
+ ssize_t nread = 0;
+ do
+ {
+ nread = ::read(fd, &out_buffer[total_read], remaining);
+ if (nread < 0)
+ {
+ rv = errno;
+ }
+ else
+ {
+ remaining -= nread,
+ total_read += nread;
+ }
+ }
+ while (nread > 0 && remaining > 0);
+ }
+
+ (void)cache.close(fd, rv != 0 || total_read != inout_size);
+ inout_size = total_read;
+ }
+ }
+ return rv;
+ }
+
+public:
+ BasicFileServerBackend(uavcan::INode& node) :
+ fdcache_(UAVCAN_NULLPTR),
+ node_(node)
+ { }
+
+ ~BasicFileServerBackend()
+ {
+ if (fdcache_ != &fallback_)
+ {
+ delete fdcache_;
+ fdcache_ = UAVCAN_NULLPTR;
+ }
+ }
+};
+
+#if __GNUC__
+/// Typo fix in a backwards-compatible way (only for GCC projects). Will be removed someday.
+typedef BasicFileServerBackend
+ BasicFileSeverBackend // Missing 'r'
+ __attribute__((deprecated));
+#endif
+
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,100 @@
+/****************************************************************************
+*
+* Copyright (c) 2015 PX4 Development Team. All rights reserved.
+* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+* David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED
+#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED
+
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+#include <cstdio>
+#include <time.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+namespace uavcan_posix
+{
+namespace dynamic_node_id_server
+{
+/**
+ * This interface implements a POSIX compliant file based IEventTracer interface
+ */
+class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer
+{
+ /**
+ * Maximum length of full path to log file
+ */
+ enum { MaxPathLength = 128 };
+
+ enum { FilePermissions = 438 }; ///< 0o666
+
+ /**
+ * This type is used for the path
+ */
+ typedef uavcan::MakeString<MaxPathLength>::Type PathString;
+
+ PathString path_;
+
+protected:
+ virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument)
+ {
+ using namespace std;
+
+ timespec ts = timespec(); // If clock_gettime() fails, zero time will be used
+ (void)clock_gettime(CLOCK_REALTIME, &ts);
+
+ int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions);
+ if (fd >= 0)
+ {
+ const int FormatBufferLength = 63;
+ char buffer[FormatBufferLength + 1];
+ ssize_t remaining = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n",
+ static_cast<long>(ts.tv_sec), static_cast<long>(ts.tv_nsec / 1000L),
+ static_cast<int>(code), static_cast<long long>(argument));
+
+ ssize_t total_written = 0;
+ ssize_t written = 0;
+ do
+ {
+ written = write(fd, &buffer[total_written], remaining);
+ if (written > 0)
+ {
+ total_written += written;
+ remaining -= written;
+ }
+ }
+ while (written > 0 && remaining > 0);
+ (void)close(fd);
+ }
+ }
+
+public:
+ /**
+ * Initializes the file based event tracer.
+ */
+ int init(const PathString& path)
+ {
+ using namespace std;
+
+ int rv = -uavcan::ErrInvalidParam;
+
+ if (path.size() > 0)
+ {
+ rv = 0;
+ path_ = path.c_str();
+ int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions);
+ if (fd >= 0)
+ {
+ (void)close(fd);
+ }
+ }
+ return rv;
+ }
+};
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,160 @@
+/****************************************************************************
+*
+* Copyright (c) 2015 PX4 Development Team. All rights reserved.
+* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+* David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED
+#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED
+
+#include <sys/stat.h>
+#include <cstdio>
+#include <cstddef>
+#include <cstdlib>
+#include <cstring>
+#include <cerrno>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <uavcan/protocol/dynamic_node_id_server/storage_backend.hpp>
+
+namespace uavcan_posix
+{
+namespace dynamic_node_id_server
+{
+/**
+ * This interface implements a POSIX compliant IStorageBackend interface
+ */
+class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend
+{
+ /**
+ * Maximum length of full path including / and key max
+ */
+ enum { MaxPathLength = 128 };
+
+ enum { FilePermissions = 438 }; ///< 0o666
+
+ /**
+ * This type is used for the path
+ */
+ typedef uavcan::MakeString<MaxPathLength>::Type PathString;
+
+ PathString base_path;
+
+protected:
+ virtual String get(const String& key) const
+ {
+ using namespace std;
+ PathString path = base_path.c_str();
+ path += key;
+ String value;
+ int fd = open(path.c_str(), O_RDONLY);
+ if (fd >= 0)
+ {
+ char buffer[MaxStringLength + 1];
+ (void)memset(buffer, 0, sizeof(buffer));
+ ssize_t remaining = MaxStringLength;
+ ssize_t total_read = 0;
+ ssize_t nread = 0;
+ do
+ {
+ nread = ::read(fd, &buffer[total_read], remaining);
+ if (nread > 0)
+ {
+ remaining -= nread,
+ total_read += nread;
+ }
+ }
+ while (nread > 0 && remaining > 0);
+ (void)close(fd);
+ if (total_read > 0)
+ {
+ for (int i = 0; i < total_read; i++)
+ {
+ if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r')
+ {
+ buffer[i] = '\0';
+ break;
+ }
+ }
+ value = buffer;
+ }
+ }
+ return value;
+ }
+
+ virtual void set(const String& key, const String& value)
+ {
+ using namespace std;
+ PathString path = base_path.c_str();
+ path += key;
+ int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions);
+ if (fd >= 0)
+ {
+ ssize_t remaining = value.size();
+ ssize_t total_written = 0;
+ ssize_t written = 0;
+ do
+ {
+ written = write(fd, &value.c_str()[total_written], remaining);
+ if (written > 0)
+ {
+ total_written += written;
+ remaining -= written;
+ }
+ }
+ while (written > 0 && remaining > 0);
+
+ (void)fsync(fd);
+ (void)close(fd);
+ }
+ }
+
+public:
+ /**
+ * Initializes the file based backend storage by passing a path to
+ * the directory where the key named files will be stored.
+ * The return value should be 0 on success.
+ * If it is -ErrInvalidConfiguration then the the path name is too long to
+ * accommodate the trailing slash and max key length.
+ */
+ int init(const PathString& path)
+ {
+ using namespace std;
+
+ int rv = -uavcan::ErrInvalidParam;
+
+ if (path.size() > 0)
+ {
+ base_path = path.c_str();
+
+ if (base_path.back() == '/')
+ {
+ base_path.pop_back();
+ }
+
+ rv = 0;
+ struct stat sb;
+ if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))
+ {
+ // coverity[toctou]
+ rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
+ }
+ if (rv >= 0)
+ {
+ base_path.push_back('/');
+ if ((base_path.size() + MaxStringLength) > MaxPathLength)
+ {
+ rv = -uavcan::ErrInvalidConfiguration;
+ }
+ }
+ }
+ return rv;
+ }
+};
+}
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,427 @@
+/****************************************************************************
+*
+* Copyright (c) 2015 PX4 Development Team. All rights reserved.
+* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+* David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED
+#define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED
+
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <fcntl.h>
+#include <cerrno>
+#include <dirent.h>
+
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+
+// TODO Get rid of the macro
+#if !defined(DIRENT_ISFILE) && defined(DT_REG)
+# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG)
+#endif
+
+namespace uavcan_posix
+{
+/**
+ * Firmware version checking logic.
+ * Refer to @ref FirmwareUpdateTrigger for details.
+ */
+class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
+{
+ enum { FilePermissions = 438 }; ///< 0o666
+
+ enum { MaxBasePathLength = 128 };
+
+ /**
+ * This type is used for the base path
+ */
+ typedef uavcan::MakeString<MaxBasePathLength>::Type BasePathString;
+
+ /**
+ * Maximum length of full path including / the file name
+ */
+ enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength };
+
+ /**
+ * This type is used internally for the full path to file
+ */
+ typedef uavcan::MakeString<MaxPathLength>::Type PathString;
+
+ BasePathString base_path_;
+ BasePathString cache_path_;
+
+ /**
+ * The folder where the files will be copied and read from
+ */
+ static const char* getCacheDir() { return "c"; }
+
+ static void addSlash(BasePathString& path)
+ {
+ if (path.back() != getPathSeparator())
+ {
+ path.push_back(getPathSeparator());
+ }
+ }
+
+ static void removeSlash(BasePathString& path)
+ {
+ if (path.back() == getPathSeparator())
+ {
+ path.pop_back();
+ }
+ }
+
+ void setFirmwareBasePath(const char* path)
+ {
+ base_path_ = path;
+ }
+
+ void setFirmwareCachePath(const char* path)
+ {
+ cache_path_ = path;
+ }
+
+ int copyIfNot(const char* srcpath, const char* destpath)
+ {
+ using namespace std;
+
+ // Does the file exist
+ int rv = 0;
+ int dfd = open(destpath, O_RDONLY, 0);
+
+ if (dfd >= 0)
+ {
+ // Close it and exit 0
+ (void)close(dfd);
+ }
+ else
+ {
+ uint8_t buffer[512];
+
+ dfd = open(destpath, O_WRONLY | O_CREAT, FilePermissions);
+ if (dfd < 0)
+ {
+ rv = -errno;
+ }
+ else
+ {
+ int sfd = open(srcpath, O_RDONLY, 0);
+ if (sfd < 0)
+ {
+ rv = -errno;
+ }
+ else
+ {
+ ssize_t size = 0;
+ do
+ {
+ size = ::read(sfd, buffer, sizeof(buffer));
+ if (size != 0)
+ {
+ if (size < 0)
+ {
+ rv = -errno;
+ }
+ else
+ {
+ rv = 0;
+ ssize_t remaining = size;
+ ssize_t total_written = 0;
+ ssize_t written = 0;
+ do
+ {
+ written = write(dfd, &buffer[total_written], remaining);
+ if (written < 0)
+ {
+ rv = -errno;
+ }
+ else
+ {
+ total_written += written;
+ remaining -= written;
+ }
+ }
+ while (written > 0 && remaining > 0);
+ }
+ }
+ }
+ while (rv == 0 && size != 0);
+
+ (void)close(sfd);
+ }
+ (void)close(dfd);
+ }
+ }
+ return rv;
+ }
+
+ struct AppDescriptor
+ {
+ uavcan::uint8_t signature[sizeof(uavcan::uint64_t)];
+ uavcan::uint64_t image_crc;
+ uavcan::uint32_t image_size;
+ uavcan::uint32_t vcs_commit;
+ uavcan::uint8_t major_version;
+ uavcan::uint8_t minor_version;
+ uavcan::uint8_t reserved[6];
+ };
+
+ static int getFileInfo(const char* path, AppDescriptor& descriptor)
+ {
+ using namespace std;
+
+ const unsigned MaxChunk = 512 / sizeof(uint64_t);
+
+ uint64_t signature = 0;
+ std::memcpy(&signature, "APDesc00", 8);
+
+ int rv = -ENOENT;
+ uint64_t chunk[MaxChunk];
+ int fd = open(path, O_RDONLY);
+
+ if (fd >= 0)
+ {
+ AppDescriptor* pdescriptor = UAVCAN_NULLPTR;
+
+ while (pdescriptor == UAVCAN_NULLPTR)
+ {
+ int len = read(fd, chunk, sizeof(chunk));
+
+ if (len == 0)
+ {
+ break;
+ }
+
+ if (len < 0)
+ {
+ rv = -errno;
+ goto out_close;
+ }
+
+ uint64_t* p = &chunk[0];
+
+ do
+ {
+ if (*p == signature)
+ {
+ pdescriptor = reinterpret_cast<AppDescriptor*>(p); // FIXME TODO This breaks strict aliasing
+ descriptor = *pdescriptor;
+ rv = 0;
+ break;
+ }
+ }
+ while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]);
+ }
+
+ out_close:
+ (void)close(fd);
+ }
+ return rv;
+ }
+
+protected:
+ /**
+ * This method will be invoked when the class obtains a response to GetNodeInfo request.
+ *
+ * @param node_id Node ID that this GetNodeInfo response was received from.
+ *
+ * @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details.
+ *
+ * @param out_firmware_file_path The implementation should return the firmware image path via this argument.
+ * Note that this path must be reachable via uavcan.protocol.file.Read service.
+ * Refer to @ref FileServer and @ref BasicFileServer for details.
+ *
+ * @return True - the class will begin sending update requests.
+ * False - the node will be ignored, no request will be sent.
+ */
+ virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID,
+ const uavcan::protocol::GetNodeInfo::Response& node_info,
+ FirmwareFilePath& out_firmware_file_path)
+ {
+ using namespace std;
+
+ /* This is a work around for two issues.
+ * 1) FirmwareFilePath is 40
+ * 2) OK using is using 32 for max file names.
+ *
+ * So for the file:
+ * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin
+ * +---fw
+ * +-c <----------- Files are cashed here.
+ * +--- px4cannode-v1.59efc137.bin <------ A Firmware file
+ * +---org.pixhawk.px4cannode-v1 <---------- node_info.name
+ * +---1.0 <-------------------------------- node_info.name's hardware_version.major,minor
+ * + - px4cannode-v1.59efc137.bin <---- A well known file must match the name
+ * in the root fw folder, so if it does not exist
+ * it is copied up MUST BE < 32 Characters
+ */
+ bool rv = false;
+
+ char fname_root[MaxBasePathLength + 1];
+ int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d",
+ getFirmwareBasePath().c_str(),
+ node_info.name.c_str(),
+ node_info.hardware_version.major,
+ node_info.hardware_version.minor);
+
+ if (n > 0 && n < (int)sizeof(fname_root) - 2)
+ {
+ DIR* const fwdir = opendir(fname_root);
+
+ fname_root[n++] = getPathSeparator();
+ fname_root[n++] = '\0';
+
+ if (fwdir != UAVCAN_NULLPTR)
+ {
+ struct dirent* pfile = UAVCAN_NULLPTR;
+ while ((pfile = readdir(fwdir)) != UAVCAN_NULLPTR)
+ {
+ if (DIRENT_ISFILE(pfile->d_type))
+ {
+ // Open any bin file in there.
+ if (strstr(pfile->d_name, ".bin") != UAVCAN_NULLPTR)
+ {
+ PathString full_src_path = fname_root;
+ full_src_path += pfile->d_name;
+
+ PathString full_dst_path = getFirmwareCachePath().c_str();
+ full_dst_path += pfile->d_name;
+
+ // ease the burden on the user
+ int cr = copyIfNot(full_src_path.c_str(), full_dst_path.c_str());
+
+ // We have a file, is it a valid image
+ AppDescriptor descriptor;
+
+ std::memset(&descriptor, 0, sizeof(descriptor));
+
+ if (cr == 0 && getFileInfo(full_dst_path.c_str(), descriptor) == 0)
+ {
+ volatile AppDescriptor descriptorC = descriptor;
+ descriptorC.reserved[1]++;
+
+ if (node_info.software_version.image_crc == 0 ||
+ (node_info.software_version.major == 0 && node_info.software_version.minor == 0) ||
+ descriptor.image_crc != node_info.software_version.image_crc)
+ {
+ rv = true;
+ out_firmware_file_path = pfile->d_name;
+ }
+ break;
+ }
+ }
+ }
+ }
+ (void)closedir(fwdir);
+ }
+ }
+ return rv;
+ }
+
+ /**
+ * This method will be invoked when a node responds to the update request with an error. If the request simply
+ * times out, this method will not be invoked.
+ * Note that if by the time of arrival of the response the node is already removed, this method will not be called.
+ *
+ * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting
+ * is not needed anymore. This method will not be invoked.
+ *
+ * @param node_id Node ID that returned this error.
+ *
+ * @param error_response Contents of the error response. It contains error code and text.
+ *
+ * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be
+ * initialized with old path, so if the same path needs to be reused, this
+ * argument should be left unchanged.
+ *
+ * @return True - the class will continue sending update requests with new firmware path.
+ * False - the node will be forgotten, new requests will not be sent.
+ */
+ virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID,
+ const uavcan::protocol::file::BeginFirmwareUpdate::Response&,
+ FirmwareFilePath&)
+ {
+ // TODO: Limit the number of attempts per node
+ return true;
+ }
+
+public:
+ const BasePathString& getFirmwareBasePath() const { return base_path_; }
+
+ const BasePathString& getFirmwareCachePath() const { return cache_path_; }
+
+ static char getPathSeparator()
+ {
+ return static_cast<char>(uavcan::protocol::file::Path::SEPARATOR);
+ }
+
+ /**
+ * Creates the Directories were the files will be stored
+ *
+ * This is directory structure is in support of a workaround
+ * for the issues that FirmwareFilePath is 40
+ *
+ * It creates a path structure:
+ * +---(base_path)
+ * +-c <----------- Files are cached here.
+ */
+ int createFwPaths(const char* base_path)
+ {
+ using namespace std;
+ int rv = -uavcan::ErrInvalidParam;
+
+ if (base_path)
+ {
+ const int len = strlen(base_path);
+
+ if (len > 0 && len < base_path_.MaxSize)
+ {
+ setFirmwareBasePath(base_path);
+ removeSlash(base_path_);
+ const char* path = getFirmwareBasePath().c_str();
+
+ setFirmwareCachePath(path);
+ addSlash(cache_path_);
+ cache_path_ += getCacheDir();
+
+ rv = 0;
+ struct stat sb;
+ if (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))
+ {
+ rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO);
+ }
+
+ path = getFirmwareCachePath().c_str();
+
+ if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode)))
+ {
+ rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO);
+ }
+
+ addSlash(base_path_);
+ addSlash(cache_path_);
+
+ if (rv >= 0)
+ {
+ if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) >
+ MaxPathLength)
+ {
+ rv = -uavcan::ErrInvalidConfiguration;
+ }
+ }
+ }
+ }
+ return rv;
+ }
+
+ const char* getFirmwarePath() const
+ {
+ return getFirmwareCachePath().c_str();
+ }
+};
+}
+
+#endif // Include guard
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/driver/CMakeLists.txt Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,17 @@ +include_directories( + ./include + ) + +add_library(uavcan_stm32_driver STATIC + ./src/uc_stm32_can.cpp + ./src/uc_stm32_clock.cpp + ./src/uc_stm32_thread.cpp + ) + +add_dependencies(uavcan_stm32_driver uavcan) + +install(DIRECTORY include/uavcan_stm32 DESTINATION include) +install(TARGETS uavcan_stm32_driver DESTINATION lib) + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :) +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/driver/include.mk Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,9 @@ +# +# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> +# + +LIBUAVCAN_STM32_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_STM32_SRC := $(shell find $(LIBUAVCAN_STM32_DIR)src -type f -name '*.cpp') + +LIBUAVCAN_STM32_INC := $(LIBUAVCAN_STM32_DIR)include/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +/** + * OS detection + */ +#ifndef UAVCAN_STM32_CHIBIOS +# define UAVCAN_STM32_CHIBIOS 0 +#endif + +#ifndef UAVCAN_STM32_NUTTX +# define UAVCAN_STM32_NUTTX 0 +#endif + +#ifndef UAVCAN_STM32_BAREMETAL +# define UAVCAN_STM32_BAREMETAL 0 +#endif + +#ifndef UAVCAN_STM32_FREERTOS +# define UAVCAN_STM32_FREERTOS 0 +#endif + +/** + * Number of interfaces must be enabled explicitly + */ +#if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2) +# error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2" +#endif + +/** + * Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5) + * e.g. -DUAVCAN_STM32_TIMER_NUMBER=2 + */ +#ifndef UAVCAN_STM32_TIMER_NUMBER +// In this case the clock driver should be implemented by the application +# define UAVCAN_STM32_TIMER_NUMBER 0 +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Bit definitions were copied from NuttX STM32 CAN driver.
+ */
+
+#pragma once
+
+#include <uavcan_stm32/build_config.hpp>
+
+#include <uavcan/uavcan.hpp>
+#include <stdint.h>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+// #undef'ed at the end of this file
+# define constexpr const
+#endif
+
+namespace uavcan_stm32
+{
+namespace bxcan
+{
+
+struct TxMailboxType
+{
+ volatile uint32_t TIR;
+ volatile uint32_t TDTR;
+ volatile uint32_t TDLR;
+ volatile uint32_t TDHR;
+};
+
+struct RxMailboxType
+{
+ volatile uint32_t RIR;
+ volatile uint32_t RDTR;
+ volatile uint32_t RDLR;
+ volatile uint32_t RDHR;
+};
+
+struct FilterRegisterType
+{
+ volatile uint32_t FR1;
+ volatile uint32_t FR2;
+};
+
+struct CanType
+{
+ volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
+ volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */
+ volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
+ volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */
+ volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */
+ volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */
+ volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */
+ volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */
+ uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */
+ TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */
+ RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */
+ uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */
+ volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */
+ volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */
+ uint32_t RESERVED2; /*!< Reserved, 0x208 */
+ volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */
+ uint32_t RESERVED3; /*!< Reserved, 0x210 */
+ volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */
+ uint32_t RESERVED4; /*!< Reserved, 0x218 */
+ volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */
+ uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */
+ FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
+};
+
+/**
+ * CANx register sets
+ */
+CanType* const Can[UAVCAN_STM32_NUM_IFACES] =
+{
+ reinterpret_cast<CanType*>(0x40006400)
+#if UAVCAN_STM32_NUM_IFACES > 1
+ ,
+ reinterpret_cast<CanType*>(0x40006800)
+#endif
+};
+
+/* CAN master control register */
+
+constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
+constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
+constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */
+constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */
+constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
+constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */
+constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */
+constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */
+constexpr unsigned long MCR_RESET = (1U << 15);/* Bit 15: bxCAN software master reset */
+constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */
+
+/* CAN master status register */
+
+constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */
+constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
+constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */
+constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
+constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */
+constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */
+constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */
+constexpr unsigned long MSR_SAMP = (1U << 10);/* Bit 10: Last Sample Point */
+constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */
+
+/* CAN transmit status register */
+
+constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */
+constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */
+constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */
+constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */
+constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */
+constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */
+constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */
+constexpr unsigned long TSR_ALST1 = (1U << 10);/* Bit 10 : Arbitration Lost for Mailbox 1 */
+constexpr unsigned long TSR_TERR1 = (1U << 11);/* Bit 11 : Transmission Error of Mailbox 1 */
+constexpr unsigned long TSR_ABRQ1 = (1U << 15);/* Bit 15 : Abort Request for Mailbox 1 */
+constexpr unsigned long TSR_RQCP2 = (1U << 16);/* Bit 16 : Request Completed Mailbox 2 */
+constexpr unsigned long TSR_TXOK2 = (1U << 17);/* Bit 17 : Transmission OK of Mailbox 2 */
+constexpr unsigned long TSR_ALST2 = (1U << 18);/* Bit 18: Arbitration Lost for Mailbox 2 */
+constexpr unsigned long TSR_TERR2 = (1U << 19);/* Bit 19: Transmission Error of Mailbox 2 */
+constexpr unsigned long TSR_ABRQ2 = (1U << 23);/* Bit 23: Abort Request for Mailbox 2 */
+constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
+constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT);
+constexpr unsigned long TSR_TME0 = (1U << 26);/* Bit 26: Transmit Mailbox 0 Empty */
+constexpr unsigned long TSR_TME1 = (1U << 27);/* Bit 27: Transmit Mailbox 1 Empty */
+constexpr unsigned long TSR_TME2 = (1U << 28);/* Bit 28: Transmit Mailbox 2 Empty */
+constexpr unsigned long TSR_LOW0 = (1U << 29);/* Bit 29: Lowest Priority Flag for Mailbox 0 */
+constexpr unsigned long TSR_LOW1 = (1U << 30);/* Bit 30: Lowest Priority Flag for Mailbox 1 */
+constexpr unsigned long TSR_LOW2 = (1U << 31);/* Bit 31: Lowest Priority Flag for Mailbox 2 */
+
+/* CAN receive FIFO 0/1 registers */
+
+constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */
+constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT);
+constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */
+constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */
+constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
+
+/* CAN interrupt enable register */
+
+constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */
+constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */
+constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */
+constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */
+constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */
+constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */
+constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */
+constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */
+constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */
+constexpr unsigned long IER_BOFIE = (1U << 10);/* Bit 10: Bus-Off Interrupt Enable */
+constexpr unsigned long IER_LECIE = (1U << 11);/* Bit 11: Last Error Code Interrupt Enable */
+constexpr unsigned long IER_ERRIE = (1U << 15);/* Bit 15: Error Interrupt Enable */
+constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup Interrupt Enable */
+constexpr unsigned long IER_SLKIE = (1U << 17);/* Bit 17: Sleep Interrupt Enable */
+
+/* CAN error status register */
+
+constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */
+constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
+constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */
+constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */
+constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT);
+constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT);/* 000: No Error */
+constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff Error */
+constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT);/* 010: Form Error */
+constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment Error */
+constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT);/* 100: Bit recessive Error */
+constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT);/* 101: Bit dominant Error */
+constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT);/* 110: CRC Error */
+constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT);/* 111: Set by software */
+constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */
+constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
+constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
+constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT);
+
+/* CAN bit timing register */
+
+constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */
+constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
+constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */
+constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
+constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */
+constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT);
+constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
+constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT);
+constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/
+constexpr unsigned long BTR_SILM = (1U << 31);/* Bit 31: Silent Mode (Debug);*/
+
+constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/
+constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
+constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
+
+/* TX mailbox identifier register */
+
+constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */
+constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
+constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
+constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
+constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT);
+constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
+constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
+
+/* Mailbox data length control and time stamp register */
+
+constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
+constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT);
+constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */
+constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
+constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
+
+/* Mailbox data low register */
+
+constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
+constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
+constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
+constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
+constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
+constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
+constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
+constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
+
+/* Mailbox data high register */
+
+constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
+constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
+constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
+constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
+constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
+constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
+constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
+constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
+
+/* Rx FIFO mailbox identifier register */
+
+constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
+constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
+constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
+constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
+constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
+constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
+
+/* Receive FIFO mailbox data length control and time stamp register */
+
+constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
+constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT);
+constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
+constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT);
+constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
+constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
+
+/* Receive FIFO mailbox data low register */
+
+constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
+constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
+constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
+constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
+constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
+constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
+constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
+constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
+
+/* Receive FIFO mailbox data high register */
+
+constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
+constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
+constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
+constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
+constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
+constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
+constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
+constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
+
+/* CAN filter master register */
+
+constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */
+
+}
+}
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+# undef constexpr
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,382 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan_stm32/build_config.hpp>
+#include <uavcan_stm32/thread.hpp>
+#include <uavcan/driver/can.hpp>
+#include <uavcan_stm32/bxcan.hpp>
+
+namespace uavcan_stm32
+{
+/**
+ * Driver error codes.
+ * These values can be returned from driver functions negated.
+ */
+//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use
+static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented
+static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported
+static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error
+static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
+static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1
+static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0
+static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished
+static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported
+
+/**
+ * RX queue item.
+ * The application shall not use this directly.
+ */
+struct CanRxItem
+{
+ uavcan::uint64_t utc_usec;
+ uavcan::CanFrame frame;
+ uavcan::CanIOFlags flags;
+ CanRxItem()
+ : utc_usec(0)
+ , flags(0)
+ { }
+};
+
+/**
+ * Single CAN iface.
+ * The application shall not use this directly.
+ */
+class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
+{
+ class RxQueue
+ {
+ CanRxItem* const buf_;
+ const uavcan::uint8_t capacity_;
+ uavcan::uint8_t in_;
+ uavcan::uint8_t out_;
+ uavcan::uint8_t len_;
+ uavcan::uint32_t overflow_cnt_;
+
+ void registerOverflow();
+
+ public:
+ RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
+ : buf_(buf)
+ , capacity_(capacity)
+ , in_(0)
+ , out_(0)
+ , len_(0)
+ , overflow_cnt_(0)
+ { }
+
+ void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
+ void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
+
+ void reset();
+
+ unsigned getLength() const { return len_; }
+
+ uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; }
+ };
+
+ struct Timings
+ {
+ uavcan::uint16_t prescaler;
+ uavcan::uint8_t sjw;
+ uavcan::uint8_t bs1;
+ uavcan::uint8_t bs2;
+
+ Timings()
+ : prescaler(0)
+ , sjw(0)
+ , bs1(0)
+ , bs2(0)
+ { }
+ };
+
+ struct TxItem
+ {
+ uavcan::MonotonicTime deadline;
+ uavcan::CanFrame frame;
+ bool pending;
+ bool loopback;
+ bool abort_on_error;
+
+ TxItem()
+ : pending(false)
+ , loopback(false)
+ , abort_on_error(false)
+ { }
+ };
+
+ enum { NumTxMailboxes = 3 };
+ enum { NumFilters = 14 };
+
+ static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
+
+ RxQueue rx_queue_;
+ bxcan::CanType* const can_;
+ uavcan::uint64_t error_cnt_;
+ uavcan::uint32_t served_aborts_cnt_;
+ BusEvent& update_event_;
+ TxItem pending_tx_[NumTxMailboxes];
+ uavcan::uint8_t peak_tx_mailbox_index_;
+ const uavcan::uint8_t self_index_;
+ bool had_activity_;
+
+ int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
+
+ virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+ uavcan::CanIOFlags flags);
+
+ virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
+
+ virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
+ uavcan::uint16_t num_configs);
+
+ virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
+
+ void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
+
+ bool waitMsrINakBitStateChange(bool target_state);
+
+public:
+ enum { MaxRxQueueCapacity = 254 };
+
+ enum OperatingMode
+ {
+ NormalMode,
+ SilentMode
+ };
+
+ CanIface(bxcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
+ CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
+ : rx_queue_(rx_queue_buffer, rx_queue_capacity)
+ , can_(can)
+ , error_cnt_(0)
+ , served_aborts_cnt_(0)
+ , update_event_(update_event)
+ , peak_tx_mailbox_index_(0)
+ , self_index_(self_index)
+ , had_activity_(false)
+ {
+ UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES);
+ }
+
+ /**
+ * Initializes the hardware CAN controller.
+ * Assumes:
+ * - Iface clock is enabled
+ * - Iface has been resetted via RCC
+ * - Caller will configure NVIC by itself
+ */
+ int init(const uavcan::uint32_t bitrate, const OperatingMode mode);
+
+ void handleTxInterrupt(uavcan::uint64_t utc_usec);
+ void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec);
+
+ /**
+ * This method is used to count errors and abort transmission on error if necessary.
+ * This functionality used to be implemented in the SCE interrupt handler, but that approach was
+ * generating too much processing overhead, especially on disconnected interfaces.
+ *
+ * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled.
+ */
+ void pollErrorFlagsFromISR();
+
+ void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
+
+ bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const;
+ bool isRxBufferEmpty() const;
+
+ /**
+ * Number of RX frames lost due to queue overflow.
+ * This is an atomic read, it doesn't require a critical section.
+ */
+ uavcan::uint32_t getRxQueueOverflowCount() const { return rx_queue_.getOverflowCount(); }
+
+ /**
+ * Total number of hardware failures and other kinds of errors (e.g. queue overruns).
+ * May increase continuously if the interface is not connected to the bus.
+ */
+ virtual uavcan::uint64_t getErrorCount() const;
+
+ /**
+ * Number of times the driver exercised library's requirement to abort transmission on first error.
+ * This is an atomic read, it doesn't require a critical section.
+ * See @ref uavcan::CanIOFlagAbortOnError.
+ */
+ uavcan::uint32_t getVoluntaryTxAbortCount() const { return served_aborts_cnt_; }
+
+ /**
+ * Returns the number of frames pending in the RX queue.
+ * This is intended for debug use only.
+ */
+ unsigned getRxQueueLength() const;
+
+ /**
+ * Whether this iface had at least one successful IO since the previous call of this method.
+ * This is designed for use with iface activity LEDs.
+ */
+ bool hadActivity();
+
+ /**
+ * Peak number of TX mailboxes used concurrently since initialization.
+ * Range is [1, 3].
+ * Value of 3 suggests that priority inversion could be taking place.
+ */
+ uavcan::uint8_t getPeakNumTxMailboxesUsed() const { return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); }
+};
+
+/**
+ * CAN driver, incorporates all available CAN ifaces.
+ * Please avoid direct use, prefer @ref CanInitHelper instead.
+ */
+class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
+{
+ BusEvent update_event_;
+ CanIface if0_;
+#if UAVCAN_STM32_NUM_IFACES > 1
+ CanIface if1_;
+#endif
+
+ virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
+ uavcan::MonotonicTime blocking_deadline);
+
+ static void initOnce();
+
+public:
+ template <unsigned RxQueueCapacity>
+ CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
+ : update_event_(*this)
+ , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
+#if UAVCAN_STM32_NUM_IFACES > 1
+ , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
+#endif
+ {
+ uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
+ }
+
+ /**
+ * This function returns select masks indicating which interfaces are available for read/write.
+ */
+ uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const;
+
+ /**
+ * Whether there's at least one interface where receive() would return a frame.
+ */
+ bool hasReadableInterfaces() const;
+
+ /**
+ * Returns zero if OK.
+ * Returns negative value if failed (e.g. invalid bitrate).
+ */
+ int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
+
+ virtual CanIface* getIface(uavcan::uint8_t iface_index);
+
+ virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; }
+
+ /**
+ * Whether at least one iface had at least one successful IO since previous call of this method.
+ * This is designed for use with iface activity LEDs.
+ */
+ bool hadActivity();
+};
+
+/**
+ * Helper class.
+ * Normally only this class should be used by the application.
+ * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns.
+ */
+template <unsigned RxQueueCapacity = 128>
+class CanInitHelper
+{
+ CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
+
+public:
+ enum { BitRateAutoDetect = 0 };
+
+ CanDriver driver;
+
+ CanInitHelper() :
+ driver(queue_storage_)
+ { }
+
+ /**
+ * This overload simply configures the provided bitrate.
+ * Auto bit rate detection will not be performed.
+ * Bitrate value must be positive.
+ * @return Negative value on error; non-negative on success. Refer to constants Err*.
+ */
+ int init(uavcan::uint32_t bitrate)
+ {
+ return driver.init(bitrate, CanIface::NormalMode);
+ }
+
+ /**
+ * This function can either initialize the driver at a fixed bit rate, or it can perform
+ * automatic bit rate detection. For theory please refer to the CiA application note #801.
+ *
+ * @param delay_callable A callable entity that suspends execution for strictly more than one second.
+ * The callable entity will be invoked without arguments.
+ * @ref getRecommendedListeningDelay().
+ *
+ * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process.
+ * If auto detection was used, the function will update the argument
+ * with established bit rate. In case of an error the value will be undefined.
+ *
+ * @return Negative value on error; non-negative on success. Refer to constants Err*.
+ */
+ template <typename DelayCallable>
+ int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect)
+ {
+ if (inout_bitrate > 0)
+ {
+ return driver.init(inout_bitrate, CanIface::NormalMode);
+ }
+ else
+ {
+ static const uavcan::uint32_t StandardBitRates[] =
+ {
+ 1000000,
+ 500000,
+ 250000,
+ 125000
+ };
+
+ for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++)
+ {
+ inout_bitrate = StandardBitRates[br];
+
+ const int res = driver.init(inout_bitrate, CanIface::SilentMode);
+
+ delay_callable();
+
+ if (res >= 0)
+ {
+ for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++)
+ {
+ if (!driver.getIface(iface)->isRxBufferEmpty())
+ {
+ // Re-initializing in normal mode
+ return driver.init(inout_bitrate, CanIface::NormalMode);
+ }
+ }
+ }
+ }
+
+ return -ErrBitRateNotDetected;
+ }
+ }
+
+ /**
+ * Use this value for listening delay during automatic bit rate detection.
+ */
+ static uavcan::MonotonicDuration getRecommendedListeningDelay()
+ {
+ return uavcan::MonotonicDuration::fromMSec(1050);
+ }
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan_stm32/build_config.hpp>
+#include <uavcan/driver/system_clock.hpp>
+
+namespace uavcan_stm32
+{
+
+namespace clock
+{
+/**
+ * Starts the clock.
+ * Can be called multiple times, only the first call will be effective.
+ */
+void init();
+
+/**
+ * Returns current monotonic time since the moment when clock::init() was called.
+ * This function is thread safe.
+ */
+uavcan::MonotonicTime getMonotonic();
+
+/**
+ * Sets the driver's notion of the system UTC. It should be called
+ * at startup and any time the system clock is updated from an
+ * external source that is not the UAVCAN Timesync master.
+ * This function is thread safe.
+ */
+void setUtc(uavcan::UtcTime time);
+
+/**
+ * Returns UTC time if it has been set, otherwise returns zero time.
+ * This function is thread safe.
+ */
+uavcan::UtcTime getUtc();
+
+/**
+ * Performs UTC phase and frequency adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ * This function is thread safe.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment);
+
+/**
+ * UTC clock synchronization parameters
+ */
+struct UtcSyncParams
+{
+ float offset_p; ///< PPM per one usec error
+ float rate_i; ///< PPM per one PPM error for second
+ float rate_error_corner_freq;
+ float max_rate_correction_ppm;
+ float lock_thres_rate_ppm;
+ uavcan::UtcDuration lock_thres_offset;
+ uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
+
+ UtcSyncParams()
+ : offset_p(0.01F)
+ , rate_i(0.02F)
+ , rate_error_corner_freq(0.01F)
+ , max_rate_correction_ppm(300.0F)
+ , lock_thres_rate_ppm(2.0F)
+ , lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
+ , min_jump(uavcan::UtcDuration::fromMSec(10))
+ { }
+};
+
+/**
+ * Clock rate error.
+ * Positive if the hardware timer is slower than reference time.
+ * This function is thread safe.
+ */
+float getUtcRateCorrectionPPM();
+
+/**
+ * Number of non-gradual adjustments performed so far.
+ * Ideally should be zero.
+ * This function is thread safe.
+ */
+uavcan::uint32_t getUtcJumpCount();
+
+/**
+ * Whether UTC is synchronized and locked.
+ * This function is thread safe.
+ */
+bool isUtcLocked();
+
+/**
+ * UTC sync params get/set.
+ * Both functions are thread safe.
+ */
+UtcSyncParams getUtcSyncParams();
+void setUtcSyncParams(const UtcSyncParams& params);
+
+}
+
+/**
+ * Adapter for uavcan::ISystemClock.
+ */
+class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
+{
+ SystemClock() { }
+
+ virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
+
+public:
+ virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
+ virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
+
+ /**
+ * Calls clock::init() as needed.
+ * This function is thread safe.
+ */
+ static SystemClock& instance();
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,239 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan_stm32/build_config.hpp>
+
+#if UAVCAN_STM32_CHIBIOS
+# include <ch.hpp>
+#elif UAVCAN_STM32_NUTTX
+# include <nuttx/config.h>
+# include <nuttx/fs/fs.h>
+# include <poll.h>
+# include <errno.h>
+# include <cstdio>
+# include <ctime>
+# include <cstring>
+#elif UAVCAN_STM32_BAREMETAL
+#elif UAVCAN_STM32_FREERTOS
+# include <cmsis_os.h>
+#else
+# error "Unknown OS"
+#endif
+
+#include <uavcan/uavcan.hpp>
+
+namespace uavcan_stm32
+{
+
+class CanDriver;
+
+#if UAVCAN_STM32_CHIBIOS
+
+class BusEvent
+{
+ chibios_rt::CounterSemaphore sem_;
+
+public:
+ BusEvent(CanDriver& can_driver)
+ : sem_(0)
+ {
+ (void)can_driver;
+ }
+
+ bool wait(uavcan::MonotonicDuration duration);
+
+ void signal();
+
+ void signalFromInterrupt();
+};
+
+class Mutex
+{
+ chibios_rt::Mutex mtx_;
+
+public:
+ void lock();
+ void unlock();
+};
+
+#elif UAVCAN_STM32_NUTTX
+
+/**
+ * All bus events are reported as POLLIN.
+ */
+class BusEvent : uavcan::Noncopyable
+{
+ static const unsigned MaxPollWaiters = 8;
+
+ ::file_operations file_ops_;
+ ::pollfd* pollset_[MaxPollWaiters];
+ CanDriver& can_driver_;
+ bool signal_;
+
+ static int openTrampoline(::file* filp);
+ static int closeTrampoline(::file* filp);
+ static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup);
+
+ int open(::file* filp);
+ int close(::file* filp);
+ int poll(::file* filp, ::pollfd* fds, bool setup);
+
+ int addPollWaiter(::pollfd* fds);
+ int removePollWaiter(::pollfd* fds);
+
+public:
+ static const char* const DevName;
+
+ BusEvent(CanDriver& can_driver);
+ ~BusEvent();
+
+ bool wait(uavcan::MonotonicDuration duration);
+
+ void signalFromInterrupt();
+};
+
+class Mutex
+{
+ pthread_mutex_t mutex_;
+
+public:
+ Mutex()
+ {
+ init();
+ }
+
+ int init()
+ {
+ return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
+ }
+
+ int deinit()
+ {
+ return pthread_mutex_destroy(&mutex_);
+ }
+
+ void lock()
+ {
+ (void)pthread_mutex_lock(&mutex_);
+ }
+
+ void unlock()
+ {
+ (void)pthread_mutex_unlock(&mutex_);
+ }
+};
+#elif UAVCAN_STM32_BAREMETAL
+
+class BusEvent
+{
+ volatile bool ready;
+
+public:
+ BusEvent(CanDriver& can_driver)
+ : ready(false)
+ {
+ (void)can_driver;
+ }
+
+ bool wait(uavcan::MonotonicDuration duration)
+ {
+ (void)duration;
+ bool lready = ready;
+ #if defined ( __CC_ARM )
+ return __sync_lock_test_and_set(&lready, false);
+ #elif defined ( __GNUC__ )
+ return __atomic_exchange_n (&lready, false, __ATOMIC_SEQ_CST);
+ #else
+ # error "This compiler is not supported"
+ #endif
+ }
+
+ void signal()
+ {
+ #if defined ( __CC_ARM )
+ __sync_lock_release(&ready);
+ #elif defined ( __GNUC__ )
+ __atomic_store_n (&ready, true, __ATOMIC_SEQ_CST);
+ #else
+ # error "This compiler is not supported"
+ #endif
+ }
+
+ void signalFromInterrupt()
+ {
+ #if defined ( __CC_ARM )
+ __sync_lock_release(&ready);
+ #elif defined ( __GNUC__ )
+ __atomic_store_n (&ready, true, __ATOMIC_SEQ_CST);
+ #else
+ # error "This compiler is not supported"
+ #endif
+ }
+};
+
+class Mutex
+{
+public:
+ void lock() { }
+ void unlock() { }
+};
+
+#elif UAVCAN_STM32_FREERTOS
+
+class BusEvent
+{
+ SemaphoreHandle_t sem_;
+ BaseType_t higher_priority_task_woken;
+
+public:
+ BusEvent(CanDriver& can_driver)
+ {
+ (void)can_driver;
+ sem_ = xSemaphoreCreateBinary();
+ }
+
+ bool wait(uavcan::MonotonicDuration duration);
+
+ void signal();
+
+ void signalFromInterrupt();
+
+ void yieldFromISR();
+};
+
+class Mutex
+{
+ SemaphoreHandle_t mtx_;
+
+public:
+ Mutex(void)
+ {
+ mtx_ = xSemaphoreCreateMutex();
+ }
+ void lock();
+ void unlock();
+};
+
+#endif
+
+
+class MutexLocker
+{
+ Mutex& mutex_;
+
+public:
+ MutexLocker(Mutex& mutex)
+ : mutex_(mutex)
+ {
+ mutex_.lock();
+ }
+ ~MutexLocker()
+ {
+ mutex_.unlock();
+ }
+};
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,11 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include <uavcan/uavcan.hpp> + +#include <uavcan_stm32/thread.hpp> +#include <uavcan_stm32/clock.hpp> +#include <uavcan_stm32/can.hpp>
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/src/internal.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan_stm32/build_config.hpp>
+
+#if UAVCAN_STM32_CHIBIOS
+# include <hal.h>
+#elif UAVCAN_STM32_NUTTX
+# include <nuttx/arch.h>
+# include <arch/board/board.h>
+# include <chip/stm32_tim.h>
+# include <syslog.h>
+#elif UAVCAN_STM32_BAREMETAL
+#include <chip.h> // See http://uavcan.org/Implementations/Libuavcan/Platforms/STM32/
+#elif UAVCAN_STM32_FREERTOS
+# include <chip.h>
+# include <cmsis_os.h>
+#else
+# error "Unknown OS"
+#endif
+
+/**
+ * Debug output
+ */
+#ifndef UAVCAN_STM32_LOG
+// syslog() crashes the system in this context
+// # if UAVCAN_STM32_NUTTX && CONFIG_ARCH_LOWPUTC
+# if 0
+# define UAVCAN_STM32_LOG(fmt, ...) syslog("uavcan_stm32: " fmt "\n", ##__VA_ARGS__)
+# else
+# define UAVCAN_STM32_LOG(...) ((void)0)
+# endif
+#endif
+
+/**
+ * IRQ handler macros
+ */
+#if UAVCAN_STM32_CHIBIOS
+# define UAVCAN_STM32_IRQ_HANDLER(id) CH_IRQ_HANDLER(id)
+# define UAVCAN_STM32_IRQ_PROLOGUE() CH_IRQ_PROLOGUE()
+# define UAVCAN_STM32_IRQ_EPILOGUE() CH_IRQ_EPILOGUE()
+#elif UAVCAN_STM32_NUTTX
+# define UAVCAN_STM32_IRQ_HANDLER(id) int id(int irq, FAR void* context)
+# define UAVCAN_STM32_IRQ_PROLOGUE()
+# define UAVCAN_STM32_IRQ_EPILOGUE() return 0;
+#else
+# define UAVCAN_STM32_IRQ_HANDLER(id) void id(void)
+# define UAVCAN_STM32_IRQ_PROLOGUE()
+# define UAVCAN_STM32_IRQ_EPILOGUE()
+#endif
+
+#if UAVCAN_STM32_CHIBIOS
+/**
+ * Priority mask for timer and CAN interrupts.
+ */
+# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
+# if (CH_KERNEL_MAJOR == 2)
+# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY)
+# else // ChibiOS 3+
+# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_MAX_KERNEL_PRIORITY
+# endif
+# endif
+#endif
+
+#if UAVCAN_STM32_BAREMETAL
+/**
+ * Priority mask for timer and CAN interrupts.
+ */
+# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
+# define UAVCAN_STM32_IRQ_PRIORITY_MASK 0
+# endif
+#endif
+
+#if UAVCAN_STM32_FREERTOS
+/**
+ * Priority mask for timer and CAN interrupts.
+ */
+# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
+# define UAVCAN_STM32_IRQ_PRIORITY_MASK configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY
+# endif
+#endif
+
+/**
+ * Glue macros
+ */
+#define UAVCAN_STM32_GLUE2_(A, B) A##B
+#define UAVCAN_STM32_GLUE2(A, B) UAVCAN_STM32_GLUE2_(A, B)
+
+#define UAVCAN_STM32_GLUE3_(A, B, C) A##B##C
+#define UAVCAN_STM32_GLUE3(A, B, C) UAVCAN_STM32_GLUE3_(A, B, C)
+
+namespace uavcan_stm32
+{
+#if UAVCAN_STM32_CHIBIOS
+
+struct CriticalSectionLocker
+{
+ CriticalSectionLocker() { chSysSuspend(); }
+ ~CriticalSectionLocker() { chSysEnable(); }
+};
+
+#elif UAVCAN_STM32_NUTTX
+
+struct CriticalSectionLocker
+{
+ const irqstate_t flags_;
+
+ CriticalSectionLocker()
+ : flags_(enter_critical_section())
+ { }
+
+ ~CriticalSectionLocker()
+ {
+ leave_critical_section(flags_);
+ }
+};
+
+#elif UAVCAN_STM32_BAREMETAL
+
+struct CriticalSectionLocker
+{
+
+ CriticalSectionLocker()
+ {
+ __disable_irq();
+ }
+
+ ~CriticalSectionLocker()
+ {
+ __enable_irq();
+ }
+};
+
+#elif UAVCAN_STM32_FREERTOS
+
+struct CriticalSectionLocker
+{
+
+ CriticalSectionLocker()
+ {
+ taskENTER_CRITICAL();
+ }
+
+ ~CriticalSectionLocker()
+ {
+ taskEXIT_CRITICAL();
+ }
+};
+
+#endif
+
+namespace clock
+{
+uavcan::uint64_t getUtcUSecFromCanInterrupt();
+}
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,1231 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <cstring>
+#include <uavcan_stm32/can.hpp>
+#include <uavcan_stm32/clock.hpp>
+#include "internal.hpp"
+
+#if UAVCAN_STM32_CHIBIOS
+# include <hal.h>
+#elif UAVCAN_STM32_NUTTX
+# include <nuttx/arch.h>
+# include <nuttx/irq.h>
+# include <arch/board/board.h>
+#elif UAVCAN_STM32_BAREMETAL
+#include <chip.h> // See http://uavcan.org/Implementations/Libuavcan/Platforms/STM32/
+#elif UAVCAN_STM32_FREERTOS
+#else
+# error "Unknown OS"
+#endif
+
+#if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL
+# if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F3XX) || defined(STM32F4XX))
+// IRQ numbers
+# define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn
+# define CAN1_TX_IRQn USB_HP_CAN1_TX_IRQn
+// IRQ vectors
+# if !defined(CAN1_RX0_IRQHandler) || !defined(CAN1_TX_IRQHandler)
+# define CAN1_TX_IRQHandler USB_HP_CAN1_TX_IRQHandler
+# define CAN1_RX0_IRQHandler USB_LP_CAN1_RX0_IRQHandler
+# endif
+# endif
+#endif
+
+#if (UAVCAN_STM32_CHIBIOS && (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5))
+#define CAN1_TX_IRQHandler STM32_CAN1_TX_HANDLER
+#define CAN1_RX0_IRQHandler STM32_CAN1_RX0_HANDLER
+#define CAN1_RX1_IRQHandler STM32_CAN1_RX1_HANDLER
+#define CAN2_TX_IRQHandler STM32_CAN2_TX_HANDLER
+#define CAN2_RX0_IRQHandler STM32_CAN2_RX0_HANDLER
+#define CAN2_RX1_IRQHandler STM32_CAN2_RX1_HANDLER
+#endif
+
+#if UAVCAN_STM32_NUTTX
+# if !defined(STM32_IRQ_CAN1TX) && !defined(STM32_IRQ_CAN1RX0)
+# define STM32_IRQ_CAN1TX STM32_IRQ_USBHPCANTX
+# define STM32_IRQ_CAN1RX0 STM32_IRQ_USBLPCANRX0
+# endif
+extern "C"
+{
+static int can1_irq(const int irq, void*);
+#if UAVCAN_STM32_NUM_IFACES > 1
+static int can2_irq(const int irq, void*);
+#endif
+}
+#endif
+
+/* STM32F3's only CAN inteface does not have a number. */
+#if defined(STM32F3XX)
+#define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN
+#define RCC_APB1RSTR_CAN1RST RCC_APB1RSTR_CANRST
+#define CAN1_TX_IRQn CAN_TX_IRQn
+#define CAN1_RX0_IRQn CAN_RX0_IRQn
+#define CAN1_RX1_IRQn CAN_RX1_IRQn
+# if UAVCAN_STM32_BAREMETAL
+# define CAN1_TX_IRQHandler CAN_TX_IRQHandler
+# define CAN1_RX0_IRQHandler CAN_RX0_IRQHandler
+# define CAN1_RX1_IRQHandler CAN_RX1_IRQHandler
+# endif
+#endif
+
+
+namespace uavcan_stm32
+{
+namespace
+{
+
+CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] =
+{
+ UAVCAN_NULLPTR
+#if UAVCAN_STM32_NUM_IFACES > 1
+ , UAVCAN_NULLPTR
+#endif
+};
+
+inline void handleTxInterrupt(uavcan::uint8_t iface_index)
+{
+ UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES);
+ uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
+ if (utc_usec > 0)
+ {
+ utc_usec--;
+ }
+ if (ifaces[iface_index] != UAVCAN_NULLPTR)
+ {
+ ifaces[iface_index]->handleTxInterrupt(utc_usec);
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+}
+
+inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index)
+{
+ UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES);
+ uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
+ if (utc_usec > 0)
+ {
+ utc_usec--;
+ }
+ if (ifaces[iface_index] != UAVCAN_NULLPTR)
+ {
+ ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec);
+ }
+ else
+ {
+ UAVCAN_ASSERT(0);
+ }
+}
+
+} // namespace
+
+/*
+ * CanIface::RxQueue
+ */
+void CanIface::RxQueue::registerOverflow()
+{
+ if (overflow_cnt_ < 0xFFFFFFFF)
+ {
+ overflow_cnt_++;
+ }
+}
+
+void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags)
+{
+ buf_[in_].frame = frame;
+ buf_[in_].utc_usec = utc_usec;
+ buf_[in_].flags = flags;
+ in_++;
+ if (in_ >= capacity_)
+ {
+ in_ = 0;
+ }
+ len_++;
+ if (len_ > capacity_)
+ {
+ len_ = capacity_;
+ registerOverflow();
+ out_++;
+ if (out_ >= capacity_)
+ {
+ out_ = 0;
+ }
+ }
+}
+
+void CanIface::RxQueue::pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags)
+{
+ if (len_ > 0)
+ {
+ out_frame = buf_[out_].frame;
+ out_utc_usec = buf_[out_].utc_usec;
+ out_flags = buf_[out_].flags;
+ out_++;
+ if (out_ >= capacity_)
+ {
+ out_ = 0;
+ }
+ len_--;
+ }
+ else { UAVCAN_ASSERT(0); }
+}
+
+void CanIface::RxQueue::reset()
+{
+ in_ = 0;
+ out_ = 0;
+ len_ = 0;
+ overflow_cnt_ = 0;
+}
+
+/*
+ * CanIface
+ */
+const uavcan::uint32_t CanIface::TSR_ABRQx[CanIface::NumTxMailboxes] =
+{
+ bxcan::TSR_ABRQ0,
+ bxcan::TSR_ABRQ1,
+ bxcan::TSR_ABRQ2
+};
+
+int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings)
+{
+ if (target_bitrate < 1)
+ {
+ return -ErrInvalidBitRate;
+ }
+
+ /*
+ * Hardware configuration
+ */
+#if UAVCAN_STM32_BAREMETAL
+ const uavcan::uint32_t pclk = STM32_PCLK1;
+#elif UAVCAN_STM32_CHIBIOS
+ const uavcan::uint32_t pclk = STM32_PCLK1;
+#elif UAVCAN_STM32_NUTTX
+ const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY;
+#elif UAVCAN_STM32_FREERTOS
+ const uavcan::uint32_t pclk = HAL_RCC_GetPCLK1Freq();
+#else
+# error "Unknown OS"
+#endif
+
+ static const int MaxBS1 = 16;
+ static const int MaxBS2 = 8;
+
+ /*
+ * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
+ * CAN in Automation, 2003
+ *
+ * According to the source, optimal quanta per bit are:
+ * Bitrate Optimal Maximum
+ * 1000 kbps 8 10
+ * 500 kbps 16 17
+ * 250 kbps 16 17
+ * 125 kbps 16 17
+ */
+ const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
+
+ UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2));
+
+ static const int MaxSamplePointLocation = 900;
+
+ /*
+ * Computing (prescaler * BS):
+ * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual
+ * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified
+ * let:
+ * BS = 1 + BS1 + BS2 -- Number of time quanta per bit
+ * PRESCALER_BS = PRESCALER * BS
+ * ==>
+ * PRESCALER_BS = PCLK / BITRATE
+ */
+ const uavcan::uint32_t prescaler_bs = pclk / target_bitrate;
+
+ /*
+ * Searching for such prescaler value so that the number of quanta per bit is highest.
+ */
+ uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1);
+
+ while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0)
+ {
+ if (bs1_bs2_sum <= 2)
+ {
+ return -ErrInvalidBitRate; // No solution
+ }
+ bs1_bs2_sum--;
+ }
+
+ const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
+ if ((prescaler < 1U) || (prescaler > 1024U))
+ {
+ return -ErrInvalidBitRate; // No solution
+ }
+
+ /*
+ * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
+ * We need to find the values so that the sample point is as close as possible to the optimal value.
+ *
+ * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *)
+ * {{bs2 -> (1 + bs1)/7}}
+ *
+ * Hence:
+ * bs2 = (1 + bs1) / 7
+ * bs1 = (7 * bs1_bs2_sum - 1) / 8
+ *
+ * Sample point location can be computed as follows:
+ * Sample point location = (1 + bs1) / (1 + bs1 + bs2)
+ *
+ * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
+ * - With rounding to nearest
+ * - With rounding to zero
+ */
+ struct BsPair
+ {
+ uavcan::uint8_t bs1;
+ uavcan::uint8_t bs2;
+ uavcan::uint16_t sample_point_permill;
+
+ BsPair() :
+ bs1(0),
+ bs2(0),
+ sample_point_permill(0)
+ { }
+
+ BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) :
+ bs1(arg_bs1),
+ bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)),
+ sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
+ {
+ UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1);
+ }
+
+ bool isValid() const { return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); }
+ };
+
+ // First attempt with rounding to nearest
+ BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8));
+
+ if (solution.sample_point_permill > MaxSamplePointLocation)
+ {
+ // Second attempt with rounding to zero
+ solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8));
+ }
+
+ /*
+ * Final validation
+ * Helpful Python:
+ * def sample_point_from_btr(x):
+ * assert 0b0011110010000000111111000000000 & x == 0
+ * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511
+ * return (1+ts1+1)/(1+ts1+1+ts2+1)
+ *
+ */
+ if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid())
+ {
+ UAVCAN_ASSERT(0);
+ return -ErrLogic;
+ }
+
+ UAVCAN_STM32_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%",
+ int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F);
+
+ out_timings.prescaler = uavcan::uint16_t(prescaler - 1U);
+ out_timings.sjw = 0; // Which means one
+ out_timings.bs1 = uavcan::uint8_t(solution.bs1 - 1);
+ out_timings.bs2 = uavcan::uint8_t(solution.bs2 - 1);
+ return 0;
+}
+
+uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+ uavcan::CanIOFlags flags)
+{
+ if (frame.isErrorFrame() || frame.dlc > 8)
+ {
+ return -ErrUnsupportedFrame;
+ }
+
+ /*
+ * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because
+ * it is possible that the highest-priority frame between select() and send() could have been
+ * replaced with a lower priority one due to TX timeout. But we don't do this check because:
+ *
+ * - It is a highly unlikely scenario.
+ *
+ * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new
+ * frame can only have higher priority, which doesn't break the logic.
+ *
+ * - If high-priority frames are timing out in the TX queue, there's probably a lot of other
+ * issues to take care of before this one becomes relevant.
+ *
+ * - It takes CPU time. Not just CPU time, but critical section time, which is expensive.
+ */
+ CriticalSectionLocker lock;
+
+ /*
+ * Seeking for an empty slot
+ */
+ uavcan::uint8_t txmailbox = 0xFF;
+ if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0)
+ {
+ txmailbox = 0;
+ }
+ else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1)
+ {
+ txmailbox = 1;
+ }
+ else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2)
+ {
+ txmailbox = 2;
+ }
+ else
+ {
+ return 0; // No transmission for you.
+ }
+
+ peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics
+
+ /*
+ * Setting up the mailbox
+ */
+ bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox];
+ if (frame.isExtended())
+ {
+ mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
+ }
+ else
+ {
+ mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21);
+ }
+
+ if (frame.isRemoteTransmissionRequest())
+ {
+ mb.TIR |= bxcan::TIR_RTR;
+ }
+
+ mb.TDTR = frame.dlc;
+
+ mb.TDHR = (uavcan::uint32_t(frame.data[7]) << 24) |
+ (uavcan::uint32_t(frame.data[6]) << 16) |
+ (uavcan::uint32_t(frame.data[5]) << 8) |
+ (uavcan::uint32_t(frame.data[4]) << 0);
+ mb.TDLR = (uavcan::uint32_t(frame.data[3]) << 24) |
+ (uavcan::uint32_t(frame.data[2]) << 16) |
+ (uavcan::uint32_t(frame.data[1]) << 8) |
+ (uavcan::uint32_t(frame.data[0]) << 0);
+
+ mb.TIR |= bxcan::TIR_TXRQ; // Go.
+
+ /*
+ * Registering the pending transmission so we can track its deadline and loopback it as needed
+ */
+ TxItem& txi = pending_tx_[txmailbox];
+ txi.deadline = tx_deadline;
+ txi.frame = frame;
+ txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0;
+ txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
+ txi.pending = true;
+ return 1;
+}
+
+uavcan::int16_t CanIface::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+ uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
+{
+ out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps
+ uavcan::uint64_t utc_usec = 0;
+ {
+ CriticalSectionLocker lock;
+ if (rx_queue_.getLength() == 0)
+ {
+ return 0;
+ }
+ rx_queue_.pop(out_frame, utc_usec, out_flags);
+ }
+ out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec);
+ return 1;
+}
+
+uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs,
+ uavcan::uint16_t num_configs)
+{
+ if (num_configs <= NumFilters)
+ {
+ CriticalSectionLocker lock;
+
+ can_->FMR |= bxcan::FMR_FINIT;
+
+ // Slave (CAN2) gets half of the filters
+ can_->FMR &= ~0x00003F00UL;
+ can_->FMR |= static_cast<uint32_t>(NumFilters) << 8;
+
+ can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters
+ can_->FM1R = 0; // Identifier Mask mode
+ can_->FS1R = 0x7ffffff; // Single 32-bit for all
+
+ const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters;
+
+ if (num_configs == 0)
+ {
+ can_->FilterRegister[filter_start_index].FR1 = 0;
+ can_->FilterRegister[filter_start_index].FR2 = 0;
+ // We can't directly overwrite FA1R because that breaks the other CAN interface
+ can_->FA1R |= 1U << filter_start_index; // Other filters may still be enabled, we don't care
+ }
+ else
+ {
+ for (uint8_t i = 0; i < NumFilters; i++)
+ {
+ if (i < num_configs)
+ {
+ uint32_t id = 0;
+ uint32_t mask = 0;
+
+ const uavcan::CanFilterConfig* const cfg = filter_configs + i;
+
+ if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF))
+ {
+ id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3;
+ mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3;
+ id |= bxcan::RIR_IDE;
+ }
+ else
+ {
+ id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21; // Regular std frames, nothing fancy.
+ mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21; // Boring.
+ }
+
+ if (cfg->id & uavcan::CanFrame::FlagRTR)
+ {
+ id |= bxcan::RIR_RTR;
+ }
+
+ if (cfg->mask & uavcan::CanFrame::FlagEFF)
+ {
+ mask |= bxcan::RIR_IDE;
+ }
+
+ if (cfg->mask & uavcan::CanFrame::FlagRTR)
+ {
+ mask |= bxcan::RIR_RTR;
+ }
+
+ can_->FilterRegister[filter_start_index + i].FR1 = id;
+ can_->FilterRegister[filter_start_index + i].FR2 = mask;
+
+ can_->FA1R |= (1 << (filter_start_index + i));
+ }
+ else
+ {
+ can_->FA1R &= ~(1 << (filter_start_index + i));
+ }
+ }
+ }
+
+ can_->FMR &= ~bxcan::FMR_FINIT;
+
+ return 0;
+ }
+
+ return -ErrFilterNumConfigs;
+}
+
+bool CanIface::waitMsrINakBitStateChange(bool target_state)
+{
+#if UAVCAN_STM32_NUTTX || UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_FREERTOS
+ const unsigned Timeout = 1000;
+#else
+ const unsigned Timeout = 2000000;
+#endif
+ for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++)
+ {
+ const bool state = (can_->MSR & bxcan::MSR_INAK) != 0;
+ if (state == target_state)
+ {
+ return true;
+ }
+#if UAVCAN_STM32_NUTTX
+ ::usleep(1000);
+#endif
+#if UAVCAN_STM32_CHIBIOS
+ ::chThdSleep(MS2ST(1));
+#endif
+#if UAVCAN_STM32_FREERTOS
+ ::osDelay(1);
+#endif
+ }
+ return false;
+}
+
+int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
+{
+ /*
+ * We need to silence the controller in the first order, otherwise it may interfere with the following operations.
+ */
+ {
+ CriticalSectionLocker lock;
+
+ can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode
+ can_->MCR |= bxcan::MCR_INRQ; // Request init
+
+ can_->IER = 0; // Disable interrupts while initialization is in progress
+ }
+
+ if (!waitMsrINakBitStateChange(true))
+ {
+ UAVCAN_STM32_LOG("MSR INAK not set");
+ can_->MCR = bxcan::MCR_RESET;
+ return -ErrMsrInakNotSet;
+ }
+
+ /*
+ * Object state - interrupts are disabled, so it's safe to modify it now
+ */
+ rx_queue_.reset();
+ error_cnt_ = 0;
+ served_aborts_cnt_ = 0;
+ uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem());
+ peak_tx_mailbox_index_ = 0;
+ had_activity_ = false;
+
+ /*
+ * CAN timings for this bitrate
+ */
+ Timings timings;
+ const int timings_res = computeTimings(bitrate, timings);
+ if (timings_res < 0)
+ {
+ can_->MCR = bxcan::MCR_RESET;
+ return timings_res;
+ }
+ UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u",
+ unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));
+
+ /*
+ * Hardware initialization (the hardware has already confirmed initialization mode, see above)
+ */
+ can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648
+
+ can_->BTR = ((timings.sjw & 3U) << 24) |
+ ((timings.bs1 & 15U) << 16) |
+ ((timings.bs2 & 7U) << 20) |
+ (timings.prescaler & 1023U) |
+ ((mode == SilentMode) ? bxcan::BTR_SILM : 0);
+
+ can_->IER = bxcan::IER_TMEIE | // TX mailbox empty
+ bxcan::IER_FMPIE0 | // RX FIFO 0 is not empty
+ bxcan::IER_FMPIE1; // RX FIFO 1 is not empty
+
+ can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode
+
+ if (!waitMsrINakBitStateChange(false))
+ {
+ UAVCAN_STM32_LOG("MSR INAK not cleared");
+ can_->MCR = bxcan::MCR_RESET;
+ return -ErrMsrInakNotCleared;
+ }
+
+ /*
+ * Default filter configuration
+ */
+ if (self_index_ == 0)
+ {
+ can_->FMR |= bxcan::FMR_FINIT;
+
+ can_->FMR &= 0xFFFFC0F1;
+ can_->FMR |= static_cast<uavcan::uint32_t>(NumFilters) << 8; // Slave (CAN2) gets half of the filters
+
+ can_->FFA1R = 0; // All assigned to FIFO0 by default
+ can_->FM1R = 0; // Indentifier Mask mode
+
+#if UAVCAN_STM32_NUM_IFACES > 1
+ can_->FS1R = 0x7ffffff; // Single 32-bit for all
+ can_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything
+ can_->FilterRegister[0].FR2 = 0;
+ can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything
+ can_->FilterRegister[NumFilters].FR2 = 0;
+ can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface
+#else
+ can_->FS1R = 0x1fff;
+ can_->FilterRegister[0].FR1 = 0;
+ can_->FilterRegister[0].FR2 = 0;
+ can_->FA1R = 1;
+#endif
+
+ can_->FMR &= ~bxcan::FMR_FINIT;
+ }
+
+ return 0;
+}
+
+void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec)
+{
+ UAVCAN_ASSERT(mailbox_index < NumTxMailboxes);
+
+ had_activity_ = had_activity_ || txok;
+
+ TxItem& txi = pending_tx_[mailbox_index];
+
+ if (txi.loopback && txok && txi.pending)
+ {
+ rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback);
+ }
+
+ txi.pending = false;
+}
+
+void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec)
+{
+ // TXOK == false means that there was a hardware failure
+ if (can_->TSR & bxcan::TSR_RQCP0)
+ {
+ const bool txok = can_->TSR & bxcan::TSR_TXOK0;
+ can_->TSR = bxcan::TSR_RQCP0;
+ handleTxMailboxInterrupt(0, txok, utc_usec);
+ }
+ if (can_->TSR & bxcan::TSR_RQCP1)
+ {
+ const bool txok = can_->TSR & bxcan::TSR_TXOK1;
+ can_->TSR = bxcan::TSR_RQCP1;
+ handleTxMailboxInterrupt(1, txok, utc_usec);
+ }
+ if (can_->TSR & bxcan::TSR_RQCP2)
+ {
+ const bool txok = can_->TSR & bxcan::TSR_TXOK2;
+ can_->TSR = bxcan::TSR_RQCP2;
+ handleTxMailboxInterrupt(2, txok, utc_usec);
+ }
+ update_event_.signalFromInterrupt();
+
+ pollErrorFlagsFromISR();
+
+ #if UAVCAN_STM32_FREERTOS
+ update_event_.yieldFromISR();
+ #endif
+}
+
+void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
+{
+ UAVCAN_ASSERT(fifo_index < 2);
+
+ volatile uavcan::uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
+ if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0)
+ {
+ UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read
+ return;
+ }
+
+ /*
+ * Register overflow as a hardware error
+ */
+ if ((*rfr_reg & bxcan::RFR_FOVR) != 0)
+ {
+ error_cnt_++;
+ }
+
+ /*
+ * Read the frame contents
+ */
+ uavcan::CanFrame frame;
+ const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index];
+
+ if ((rf.RIR & bxcan::RIR_IDE) == 0)
+ {
+ frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21);
+ }
+ else
+ {
+ frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3);
+ frame.id |= uavcan::CanFrame::FlagEFF;
+ }
+
+ if ((rf.RIR & bxcan::RIR_RTR) != 0)
+ {
+ frame.id |= uavcan::CanFrame::FlagRTR;
+ }
+
+ frame.dlc = rf.RDTR & 15;
+
+ frame.data[0] = uavcan::uint8_t(0xFF & (rf.RDLR >> 0));
+ frame.data[1] = uavcan::uint8_t(0xFF & (rf.RDLR >> 8));
+ frame.data[2] = uavcan::uint8_t(0xFF & (rf.RDLR >> 16));
+ frame.data[3] = uavcan::uint8_t(0xFF & (rf.RDLR >> 24));
+ frame.data[4] = uavcan::uint8_t(0xFF & (rf.RDHR >> 0));
+ frame.data[5] = uavcan::uint8_t(0xFF & (rf.RDHR >> 8));
+ frame.data[6] = uavcan::uint8_t(0xFF & (rf.RDHR >> 16));
+ frame.data[7] = uavcan::uint8_t(0xFF & (rf.RDHR >> 24));
+
+ *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read
+
+ /*
+ * Store with timeout into the FIFO buffer and signal update event
+ */
+ rx_queue_.push(frame, utc_usec, 0);
+ had_activity_ = true;
+ update_event_.signalFromInterrupt();
+
+ pollErrorFlagsFromISR();
+
+ #if UAVCAN_STM32_FREERTOS
+ update_event_.yieldFromISR();
+ #endif
+}
+
+void CanIface::pollErrorFlagsFromISR()
+{
+ const uavcan::uint8_t lec = uavcan::uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
+ if (lec != 0)
+ {
+ can_->ESR = 0;
+ error_cnt_++;
+
+ // Serving abort requests
+ for (int i = 0; i < NumTxMailboxes; i++) // Dear compiler, may I suggest you to unroll this loop please.
+ {
+ TxItem& txi = pending_tx_[i];
+ if (txi.pending && txi.abort_on_error)
+ {
+ can_->TSR = TSR_ABRQx[i];
+ txi.pending = false;
+ served_aborts_cnt_++;
+ }
+ }
+ }
+}
+
+void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
+{
+ CriticalSectionLocker lock;
+ for (int i = 0; i < NumTxMailboxes; i++)
+ {
+ TxItem& txi = pending_tx_[i];
+ if (txi.pending && txi.deadline < current_time)
+ {
+ can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission
+ txi.pending = false;
+ error_cnt_++;
+ }
+ }
+}
+
+bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame& frame) const
+{
+ /*
+ * We can accept more frames only if the following conditions are satisfied:
+ * - There is at least one TX mailbox free (obvious enough);
+ * - The priority of the new frame is higher than priority of all TX mailboxes.
+ */
+ {
+ static const uavcan::uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2;
+ const uavcan::uint32_t tme = can_->TSR & TME;
+
+ if (tme == TME) // All TX mailboxes are free (as in freedom).
+ {
+ return true;
+ }
+
+ if (tme == 0) // All TX mailboxes are busy transmitting.
+ {
+ return false;
+ }
+ }
+
+ /*
+ * The second condition requires a critical section.
+ */
+ CriticalSectionLocker lock;
+
+ for (int mbx = 0; mbx < NumTxMailboxes; mbx++)
+ {
+ if (pending_tx_[mbx].pending && !frame.priorityHigherThan(pending_tx_[mbx].frame))
+ {
+ return false; // There's a mailbox whose priority is higher or equal the priority of the new frame.
+ }
+ }
+
+ return true; // This new frame will be added to a free TX mailbox in the next @ref send().
+}
+
+bool CanIface::isRxBufferEmpty() const
+{
+ CriticalSectionLocker lock;
+ return rx_queue_.getLength() == 0;
+}
+
+uavcan::uint64_t CanIface::getErrorCount() const
+{
+ CriticalSectionLocker lock;
+ return error_cnt_ + rx_queue_.getOverflowCount();
+}
+
+unsigned CanIface::getRxQueueLength() const
+{
+ CriticalSectionLocker lock;
+ return rx_queue_.getLength();
+}
+
+bool CanIface::hadActivity()
+{
+ CriticalSectionLocker lock;
+ const bool ret = had_activity_;
+ had_activity_ = false;
+ return ret;
+}
+
+/*
+ * CanDriver
+ */
+uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const
+{
+ uavcan::CanSelectMasks msk;
+
+ // Iface 0
+ msk.read = if0_.isRxBufferEmpty() ? 0 : 1;
+
+ if (pending_tx[0] != UAVCAN_NULLPTR)
+ {
+ msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0;
+ }
+
+ // Iface 1
+#if UAVCAN_STM32_NUM_IFACES > 1
+ if (!if1_.isRxBufferEmpty())
+ {
+ msk.read |= 1 << 1;
+ }
+
+ if (pending_tx[1] != UAVCAN_NULLPTR)
+ {
+ if (if1_.canAcceptNewTxFrame(*pending_tx[1]))
+ {
+ msk.write |= 1 << 1;
+ }
+ }
+#endif
+ return msk;
+}
+
+bool CanDriver::hasReadableInterfaces() const
+{
+#if UAVCAN_STM32_NUM_IFACES == 1
+ return !if0_.isRxBufferEmpty();
+#elif UAVCAN_STM32_NUM_IFACES == 2
+ return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty();
+#else
+# error UAVCAN_STM32_NUM_IFACES
+#endif
+}
+
+uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
+ const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
+ const uavcan::MonotonicTime blocking_deadline)
+{
+ const uavcan::CanSelectMasks in_masks = inout_masks;
+ const uavcan::MonotonicTime time = clock::getMonotonic();
+
+ if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots
+ {
+ CriticalSectionLocker cs_locker;
+ if0_.pollErrorFlagsFromISR();
+ }
+
+#if UAVCAN_STM32_NUM_IFACES > 1
+ if1_.discardTimedOutTxMailboxes(time);
+ {
+ CriticalSectionLocker cs_locker;
+ if1_.pollErrorFlagsFromISR();
+ }
+#endif
+
+ inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events
+ if ((inout_masks.read & in_masks.read) != 0 ||
+ (inout_masks.write & in_masks.write) != 0)
+ {
+ return 1;
+ }
+
+ (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates
+ inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set
+ return 1; // Return value doesn't matter as long as it is non-negative
+}
+
+
+#if UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+
+static void nvicEnableVector(IRQn_Type irq, uint8_t prio)
+{
+ #if !defined (USE_HAL_DRIVER)
+ NVIC_InitTypeDef NVIC_InitStructure;
+ NVIC_InitStructure.NVIC_IRQChannel = irq;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ #else
+ HAL_NVIC_SetPriority(irq, prio, 0);
+ HAL_NVIC_EnableIRQ(irq);
+ #endif
+}
+
+#endif
+
+void CanDriver::initOnce()
+{
+ /*
+ * CAN1, CAN2
+ */
+ {
+ CriticalSectionLocker lock;
+#if UAVCAN_STM32_NUTTX
+ modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN);
+ modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN1RST);
+ modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN1RST, 0);
+# if UAVCAN_STM32_NUM_IFACES > 1
+ modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN);
+ modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST);
+ modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0);
+# endif
+#else
+ RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+ RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST;
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
+# if UAVCAN_STM32_NUM_IFACES > 1
+ RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
+ RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST;
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
+# endif
+#endif
+ }
+
+ /*
+ * IRQ
+ */
+#if UAVCAN_STM32_NUTTX
+# define IRQ_ATTACH(irq, handler) \
+ { \
+ const int res = irq_attach(irq, handler); \
+ (void)res; \
+ assert(res >= 0); \
+ up_enable_irq(irq); \
+ }
+ IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq);
+ IRQ_ATTACH(STM32_IRQ_CAN1RX0, can1_irq);
+ IRQ_ATTACH(STM32_IRQ_CAN1RX1, can1_irq);
+# if UAVCAN_STM32_NUM_IFACES > 1
+ IRQ_ATTACH(STM32_IRQ_CAN2TX, can2_irq);
+ IRQ_ATTACH(STM32_IRQ_CAN2RX0, can2_irq);
+ IRQ_ATTACH(STM32_IRQ_CAN2RX1, can2_irq);
+# endif
+# undef IRQ_ATTACH
+#elif UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+ {
+ CriticalSectionLocker lock;
+ nvicEnableVector(CAN1_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+ nvicEnableVector(CAN1_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+ nvicEnableVector(CAN1_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+# if UAVCAN_STM32_NUM_IFACES > 1
+ nvicEnableVector(CAN2_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+ nvicEnableVector(CAN2_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+ nvicEnableVector(CAN2_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+# endif
+ }
+#endif
+}
+
+int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
+{
+ int res = 0;
+
+ UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
+
+ static bool initialized_once = false;
+ if (!initialized_once)
+ {
+ initialized_once = true;
+ UAVCAN_STM32_LOG("First initialization");
+ initOnce();
+ }
+
+ /*
+ * CAN1
+ */
+ UAVCAN_STM32_LOG("Initing iface 0...");
+ ifaces[0] = &if0_; // This link must be initialized first,
+ res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet;
+ if (res < 0) // a typical race condition.
+ {
+ UAVCAN_STM32_LOG("Iface 0 init failed %i", res);
+ ifaces[0] = UAVCAN_NULLPTR;
+ goto fail;
+ }
+
+ /*
+ * CAN2
+ */
+#if UAVCAN_STM32_NUM_IFACES > 1
+ UAVCAN_STM32_LOG("Initing iface 1...");
+ ifaces[1] = &if1_; // Same thing here.
+ res = if1_.init(bitrate, mode);
+ if (res < 0)
+ {
+ UAVCAN_STM32_LOG("Iface 1 init failed %i", res);
+ ifaces[1] = UAVCAN_NULLPTR;
+ goto fail;
+ }
+#endif
+
+ UAVCAN_STM32_LOG("CAN drv init OK");
+ UAVCAN_ASSERT(res >= 0);
+ return res;
+
+fail:
+ UAVCAN_STM32_LOG("CAN drv init failed %i", res);
+ UAVCAN_ASSERT(res < 0);
+ return res;
+}
+
+CanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
+{
+ if (iface_index < UAVCAN_STM32_NUM_IFACES)
+ {
+ return ifaces[iface_index];
+ }
+ return UAVCAN_NULLPTR;
+}
+
+bool CanDriver::hadActivity()
+{
+ bool ret = if0_.hadActivity();
+#if UAVCAN_STM32_NUM_IFACES > 1
+ ret |= if1_.hadActivity();
+#endif
+ return ret;
+}
+
+} // namespace uavcan_stm32
+
+/*
+ * Interrupt handlers
+ */
+extern "C"
+{
+
+#if UAVCAN_STM32_NUTTX
+
+static int can1_irq(const int irq, void*)
+{
+ if (irq == STM32_IRQ_CAN1TX)
+ {
+ uavcan_stm32::handleTxInterrupt(0);
+ }
+ else if (irq == STM32_IRQ_CAN1RX0)
+ {
+ uavcan_stm32::handleRxInterrupt(0, 0);
+ }
+ else if (irq == STM32_IRQ_CAN1RX1)
+ {
+ uavcan_stm32::handleRxInterrupt(0, 1);
+ }
+ else
+ {
+ PANIC();
+ }
+ return 0;
+}
+
+# if UAVCAN_STM32_NUM_IFACES > 1
+
+static int can2_irq(const int irq, void*)
+{
+ if (irq == STM32_IRQ_CAN2TX)
+ {
+ uavcan_stm32::handleTxInterrupt(1);
+ }
+ else if (irq == STM32_IRQ_CAN2RX0)
+ {
+ uavcan_stm32::handleRxInterrupt(1, 0);
+ }
+ else if (irq == STM32_IRQ_CAN2RX1)
+ {
+ uavcan_stm32::handleRxInterrupt(1, 1);
+ }
+ else
+ {
+ PANIC();
+ }
+ return 0;
+}
+
+# endif
+
+#else // UAVCAN_STM32_NUTTX
+
+#if !defined(CAN1_TX_IRQHandler) ||\
+ !defined(CAN1_RX0_IRQHandler) ||\
+ !defined(CAN1_RX1_IRQHandler)
+# error "Misconfigured build"
+#endif
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler);
+UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+ uavcan_stm32::handleTxInterrupt(0);
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler);
+UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+ uavcan_stm32::handleRxInterrupt(0, 0);
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler);
+UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+ uavcan_stm32::handleRxInterrupt(0, 1);
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+# if UAVCAN_STM32_NUM_IFACES > 1
+
+#if !defined(CAN2_TX_IRQHandler) ||\
+ !defined(CAN2_RX0_IRQHandler) ||\
+ !defined(CAN2_RX1_IRQHandler)
+# error "Misconfigured build"
+#endif
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler);
+UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+ uavcan_stm32::handleTxInterrupt(1);
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler);
+UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+ uavcan_stm32::handleRxInterrupt(1, 0);
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler);
+UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+ uavcan_stm32::handleRxInterrupt(1, 1);
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+# endif
+#endif // UAVCAN_STM32_NUTTX
+
+} // extern "C"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,496 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_stm32/clock.hpp>
+#include <uavcan_stm32/thread.hpp>
+#include "internal.hpp"
+
+#if UAVCAN_STM32_TIMER_NUMBER
+
+#include <cassert>
+#include <cmath>
+
+/*
+ * Timer instance
+ */
+# if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+# define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
+# define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
+# define TIMX_INPUT_CLOCK STM32_TIMCLK1
+# endif
+
+# if (UAVCAN_STM32_CHIBIOS && (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5))
+# define TIMX UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER)
+# define TIMX_IRQn UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER)
+# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER)
+# define TIMX_INPUT_CLOCK STM32_TIMCLK1
+# else
+# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
+# endif
+
+# if UAVCAN_STM32_NUTTX
+# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
+# define TMR_REG(o) (TIMX + (o))
+# define TIMX_INPUT_CLOCK UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
+
+# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
+# endif
+
+# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
+# define TIMX_RCC_ENR RCC->APB1ENR
+# define TIMX_RCC_RSTR RCC->APB1RSTR
+# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
+# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
+# else
+# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
+# endif
+
+/**
+ * UAVCAN_STM32_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
+ * This is useful at least with certain versions of ChibiOS which do not support the bit
+ * RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
+ * the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
+ * Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
+ * A normal way to use the override feature is to provide an alternative macro, e.g.:
+ *
+ * -DUAVCAN_STM32_TIMX_INPUT_CLOCK=STM32_HCLK
+ *
+ * Alternatively, the new clock rate can be specified directly.
+ */
+# ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
+# undef TIMX_INPUT_CLOCK
+# define TIMX_INPUT_CLOCK UAVCAN_STM32_TIMX_INPUT_CLOCK
+# endif
+
+extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
+
+namespace uavcan_stm32
+{
+namespace clock
+{
+namespace
+{
+
+const uavcan::uint32_t USecPerOverflow = 65536;
+
+Mutex mutex;
+
+bool initialized = false;
+
+bool utc_set = false;
+bool utc_locked = false;
+uavcan::uint32_t utc_jump_cnt = 0;
+UtcSyncParams utc_sync_params;
+float utc_prev_adj = 0;
+float utc_rel_rate_ppm = 0;
+float utc_rel_rate_error_integral = 0;
+uavcan::int32_t utc_accumulated_correction_nsec = 0;
+uavcan::int32_t utc_correction_nsec_per_overflow = 0;
+uavcan::MonotonicTime prev_utc_adj_at;
+
+uavcan::uint64_t time_mono = 0;
+uavcan::uint64_t time_utc = 0;
+
+}
+
+#if UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+
+static void nvicEnableVector(IRQn_Type irq, uint8_t prio)
+{
+ #if !defined (USE_HAL_DRIVER)
+ NVIC_InitTypeDef NVIC_InitStructure;
+ NVIC_InitStructure.NVIC_IRQChannel = irq;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ #else
+ HAL_NVIC_SetPriority(irq, prio, 0);
+ HAL_NVIC_EnableIRQ(irq);
+ #endif
+
+}
+
+#endif
+
+void init()
+{
+ CriticalSectionLocker lock;
+ if (initialized)
+ {
+ return;
+ }
+ initialized = true;
+
+
+# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+ // Power-on and reset
+ TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
+ TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK;
+ TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK;
+
+ // Enable IRQ
+ nvicEnableVector(TIMX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+
+# if (TIMX_INPUT_CLOCK % 1000000) != 0
+# error "No way, timer clock must be divisible by 1e6. FIXME!"
+# endif
+
+ // Start the timer
+ TIMX->ARR = 0xFFFF;
+ TIMX->PSC = (TIMX_INPUT_CLOCK / 1000000) - 1; // 1 tick == 1 microsecond
+ TIMX->CR1 = TIM_CR1_URS;
+ TIMX->SR = 0;
+ TIMX->EGR = TIM_EGR_UG; // Reload immediately
+ TIMX->DIER = TIM_DIER_UIE;
+ TIMX->CR1 = TIM_CR1_CEN; // Start
+
+# endif
+
+# if UAVCAN_STM32_NUTTX
+
+ // Attach IRQ
+ irq_attach(TIMX_IRQn, &TIMX_IRQHandler);
+
+ // Power-on and reset
+ modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
+ modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
+ modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
+
+
+ // Start the timer
+ putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
+ putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET));
+ putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
+ putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
+ putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
+ putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
+ putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
+
+ // Prioritize and Enable IRQ
+// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
+// need to investigate
+// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
+ up_enable_irq(TIMX_IRQn);
+
+# endif
+}
+
+void setUtc(uavcan::UtcTime time)
+{
+ MutexLocker mlocker(mutex);
+ UAVCAN_ASSERT(initialized);
+
+ {
+ CriticalSectionLocker locker;
+ time_utc = time.toUSec();
+ }
+
+ utc_set = true;
+ utc_locked = false;
+ utc_jump_cnt++;
+ utc_prev_adj = 0;
+ utc_rel_rate_ppm = 0;
+}
+
+static uavcan::uint64_t sampleUtcFromCriticalSection()
+{
+# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+ UAVCAN_ASSERT(initialized);
+ UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);
+
+ volatile uavcan::uint64_t time = time_utc;
+ volatile uavcan::uint32_t cnt = TIMX->CNT;
+
+ if (TIMX->SR & TIM_SR_UIF)
+ {
+ cnt = TIMX->CNT;
+ const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
+ (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
+ time = uavcan::uint64_t(uavcan::int64_t(time) + add);
+ }
+ return time + cnt;
+# endif
+
+# if UAVCAN_STM32_NUTTX
+
+ UAVCAN_ASSERT(initialized);
+ UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
+
+ volatile uavcan::uint64_t time = time_utc;
+ volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
+
+ if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
+ {
+ cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
+ const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
+ (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
+ time = uavcan::uint64_t(uavcan::int64_t(time) + add);
+ }
+ return time + cnt;
+# endif
+}
+
+uavcan::uint64_t getUtcUSecFromCanInterrupt()
+{
+ return utc_set ? sampleUtcFromCriticalSection() : 0;
+}
+
+uavcan::MonotonicTime getMonotonic()
+{
+ uavcan::uint64_t usec = 0;
+ // Scope Critical section
+ {
+ CriticalSectionLocker locker;
+
+ volatile uavcan::uint64_t time = time_mono;
+
+# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+
+ volatile uavcan::uint32_t cnt = TIMX->CNT;
+ if (TIMX->SR & TIM_SR_UIF)
+ {
+ cnt = TIMX->CNT;
+# endif
+
+# if UAVCAN_STM32_NUTTX
+
+ volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
+
+ if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
+ {
+ cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
+# endif
+ time += USecPerOverflow;
+ }
+ usec = time + cnt;
+
+# ifndef NDEBUG
+ static uavcan::uint64_t prev_usec = 0; // Self-test
+ UAVCAN_ASSERT(prev_usec <= usec);
+ (void)prev_usec;
+ prev_usec = usec;
+# endif
+ } // End Scope Critical section
+
+ return uavcan::MonotonicTime::fromUSec(usec);
+}
+
+uavcan::UtcTime getUtc()
+{
+ if (utc_set)
+ {
+ uavcan::uint64_t usec = 0;
+ {
+ CriticalSectionLocker locker;
+ usec = sampleUtcFromCriticalSection();
+ }
+ return uavcan::UtcTime::fromUSec(usec);
+ }
+ return uavcan::UtcTime();
+}
+
+static float lowpass(float xold, float xnew, float corner, float dt)
+{
+ const float tau = 1.F / corner;
+ return (dt * xnew + tau * xold) / (dt + tau);
+}
+
+static void updateRatePID(uavcan::UtcDuration adjustment)
+{
+ const uavcan::MonotonicTime ts = getMonotonic();
+ const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
+ prev_utc_adj_at = ts;
+ const float adj_usec = float(adjustment.toUSec());
+
+ /*
+ * Target relative rate in PPM
+ * Positive to go faster
+ */
+ const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
+
+ /*
+ * Current relative rate in PPM
+ * Positive if the local clock is faster
+ */
+ const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
+ utc_prev_adj = adj_usec;
+ utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
+
+ const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
+
+ if (dt > 10)
+ {
+ utc_rel_rate_error_integral = 0;
+ }
+ else
+ {
+ utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
+ utc_rel_rate_error_integral =
+ uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
+ utc_rel_rate_error_integral =
+ uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
+ }
+
+ /*
+ * Rate controller
+ */
+ float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
+ total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
+ total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
+
+ utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
+
+// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
+// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
+// total_rate_correction_ppm);
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+ MutexLocker mlocker(mutex);
+ UAVCAN_ASSERT(initialized);
+
+ if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
+ {
+ const uavcan::int64_t adj_usec = adjustment.toUSec();
+
+ {
+ CriticalSectionLocker locker;
+ if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
+ {
+ time_utc = 1;
+ }
+ else
+ {
+ time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
+ }
+ }
+
+ utc_set = true;
+ utc_locked = false;
+ utc_jump_cnt++;
+ utc_prev_adj = 0;
+ utc_rel_rate_ppm = 0;
+ }
+ else
+ {
+ updateRatePID(adjustment);
+
+ if (!utc_locked)
+ {
+ utc_locked =
+ (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
+ (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
+ }
+ }
+}
+
+float getUtcRateCorrectionPPM()
+{
+ MutexLocker mlocker(mutex);
+ const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
+ return 1e6F * rate_correction_mult;
+}
+
+uavcan::uint32_t getUtcJumpCount()
+{
+ MutexLocker mlocker(mutex);
+ return utc_jump_cnt;
+}
+
+bool isUtcLocked()
+{
+ MutexLocker mlocker(mutex);
+ return utc_locked;
+}
+
+UtcSyncParams getUtcSyncParams()
+{
+ MutexLocker mlocker(mutex);
+ return utc_sync_params;
+}
+
+void setUtcSyncParams(const UtcSyncParams& params)
+{
+ MutexLocker mlocker(mutex);
+ // Add some sanity check
+ utc_sync_params = params;
+}
+
+} // namespace clock
+
+SystemClock& SystemClock::instance()
+{
+ static union SystemClockStorage
+ {
+ uavcan::uint8_t buffer[sizeof(SystemClock)];
+ long long _aligner_1;
+ long double _aligner_2;
+ } storage;
+
+ SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
+
+ if (!clock::initialized)
+ {
+ MutexLocker mlocker(clock::mutex);
+ clock::init();
+ new (ptr)SystemClock();
+ }
+ return *ptr;
+}
+
+} // namespace uavcan_stm32
+
+
+/**
+ * Timer interrupt handler
+ */
+
+extern "C"
+UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
+{
+ UAVCAN_STM32_IRQ_PROLOGUE();
+
+# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS
+ TIMX->SR = 0;
+# endif
+# if UAVCAN_STM32_NUTTX
+ putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
+# endif
+
+ using namespace uavcan_stm32::clock;
+ UAVCAN_ASSERT(initialized);
+
+ time_mono += USecPerOverflow;
+
+ if (utc_set)
+ {
+ time_utc += USecPerOverflow;
+ utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
+ if (std::abs(utc_accumulated_correction_nsec) >= 1000)
+ {
+ time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
+ utc_accumulated_correction_nsec %= 1000;
+ }
+
+ // Correction decay - 1 nsec per 65536 usec
+ if (utc_correction_nsec_per_overflow > 0)
+ {
+ utc_correction_nsec_per_overflow--;
+ }
+ else if (utc_correction_nsec_per_overflow < 0)
+ {
+ utc_correction_nsec_per_overflow++;
+ }
+ else
+ {
+ ; // Zero
+ }
+ }
+
+ UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,284 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_stm32/thread.hpp>
+#include <uavcan_stm32/clock.hpp>
+#include <uavcan_stm32/can.hpp>
+#include "internal.hpp"
+
+
+namespace uavcan_stm32
+{
+
+#if UAVCAN_STM32_CHIBIOS
+/*
+ * BusEvent
+ */
+bool BusEvent::wait(uavcan::MonotonicDuration duration)
+{
+ static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
+
+ const uavcan::int64_t msec = duration.toMSec();
+ msg_t ret = msg_t();
+
+ if (msec <= 0)
+ {
+# if (CH_KERNEL_MAJOR == 2)
+ ret = sem_.waitTimeout(TIME_IMMEDIATE);
+# else // ChibiOS 3+
+ ret = sem_.wait(TIME_IMMEDIATE);
+# endif
+ }
+ else
+ {
+# if (CH_KERNEL_MAJOR == 2)
+ ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
+# else // ChibiOS 3+
+ ret = sem_.wait((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
+# endif
+ }
+# if (CH_KERNEL_MAJOR == 2)
+ return ret == RDY_OK;
+# else // ChibiOS 3+
+ return ret == MSG_OK;
+# endif
+}
+
+void BusEvent::signal()
+{
+ sem_.signal();
+}
+
+void BusEvent::signalFromInterrupt()
+{
+# if (CH_KERNEL_MAJOR == 2)
+ chSysLockFromIsr();
+ sem_.signalI();
+ chSysUnlockFromIsr();
+# else // ChibiOS 3+
+ chSysLockFromISR();
+ sem_.signalI();
+ chSysUnlockFromISR();
+# endif
+}
+
+/*
+ * Mutex
+ */
+void Mutex::lock()
+{
+ mtx_.lock();
+}
+
+void Mutex::unlock()
+{
+# if (CH_KERNEL_MAJOR == 2)
+ chibios_rt::BaseThread::unlockMutex();
+# else // ChibiOS 3+
+ mtx_.unlock();
+# endif
+}
+
+
+#elif UAVCAN_STM32_FREERTOS
+
+bool BusEvent::wait(uavcan::MonotonicDuration duration)
+{
+ static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
+
+ const uavcan::int64_t msec = duration.toMSec();
+
+ BaseType_t ret;
+
+ if (msec <= 0)
+ {
+ ret = xSemaphoreTake( sem_, ( TickType_t ) 0 );
+ }
+ else
+ {
+ ret = xSemaphoreTake( sem_, (msec > MaxDelayMSec) ? (MaxDelayMSec/portTICK_RATE_MS) : (msec/portTICK_RATE_MS));
+ }
+ return ret == pdTRUE;
+}
+
+void BusEvent::signal()
+{
+ xSemaphoreGive( sem_ );
+}
+
+void BusEvent::signalFromInterrupt()
+{
+ higher_priority_task_woken = pdFALSE;
+
+ xSemaphoreGiveFromISR( sem_, &higher_priority_task_woken );
+}
+
+void BusEvent::yieldFromISR()
+{
+ portYIELD_FROM_ISR( higher_priority_task_woken );
+}
+
+/*
+ * Mutex
+ */
+void Mutex::lock()
+{
+ xSemaphoreTake( mtx_, portMAX_DELAY );
+}
+
+void Mutex::unlock()
+{
+ xSemaphoreGive( mtx_ );
+}
+
+
+#elif UAVCAN_STM32_NUTTX
+
+const unsigned BusEvent::MaxPollWaiters;
+const char* const BusEvent::DevName = "/dev/uavcan/busevent";
+
+int BusEvent::openTrampoline(::file* filp)
+{
+ return static_cast<BusEvent*>(filp->f_inode->i_private)->open(filp);
+}
+
+int BusEvent::closeTrampoline(::file* filp)
+{
+ return static_cast<BusEvent*>(filp->f_inode->i_private)->close(filp);
+}
+
+int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup)
+{
+ return static_cast<BusEvent*>(filp->f_inode->i_private)->poll(filp, fds, setup);
+}
+
+int BusEvent::open(::file* filp)
+{
+ (void)filp;
+ return 0;
+}
+
+int BusEvent::close(::file* filp)
+{
+ (void)filp;
+ return 0;
+}
+
+int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup)
+{
+ CriticalSectionLocker locker;
+ int ret = -1;
+
+ if (setup)
+ {
+ ret = addPollWaiter(fds);
+ if (ret == 0)
+ {
+ /*
+ * Two events can be reported via POLLIN:
+ * - The RX queue is not empty. This event is level-triggered.
+ * - Transmission complete. This event is edge-triggered.
+ * FIXME Since TX event is edge-triggered, it can be lost between poll() calls.
+ */
+ fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0);
+ if (fds->revents != 0)
+ {
+ (void)sem_post(fds->sem);
+ }
+ }
+ }
+ else
+ {
+ ret = removePollWaiter(fds);
+ }
+
+ return ret;
+}
+
+int BusEvent::addPollWaiter(::pollfd* fds)
+{
+ for (unsigned i = 0; i < MaxPollWaiters; i++)
+ {
+ if (pollset_[i] == UAVCAN_NULLPTR)
+ {
+ pollset_[i] = fds;
+ return 0;
+ }
+ }
+ return -ENOMEM;
+}
+
+int BusEvent::removePollWaiter(::pollfd* fds)
+{
+ for (unsigned i = 0; i < MaxPollWaiters; i++)
+ {
+ if (fds == pollset_[i])
+ {
+ pollset_[i] = UAVCAN_NULLPTR;
+ return 0;
+ }
+ }
+ return -EINVAL;
+}
+
+BusEvent::BusEvent(CanDriver& can_driver)
+ : can_driver_(can_driver)
+ , signal_(false)
+{
+ std::memset(&file_ops_, 0, sizeof(file_ops_));
+ std::memset(pollset_, 0, sizeof(pollset_));
+ file_ops_.open = &BusEvent::openTrampoline;
+ file_ops_.close = &BusEvent::closeTrampoline;
+ file_ops_.poll = &BusEvent::pollTrampoline;
+ // TODO: move to init(), add proper error handling
+ if (register_driver(DevName, &file_ops_, 0666, static_cast<void*>(this)) != 0)
+ {
+ std::abort();
+ }
+}
+
+BusEvent::~BusEvent()
+{
+ (void)unregister_driver(DevName);
+}
+
+bool BusEvent::wait(uavcan::MonotonicDuration duration)
+{
+ // TODO blocking wait
+ const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration;
+ while (clock::getMonotonic() < deadline)
+ {
+ {
+ CriticalSectionLocker locker;
+ if (signal_)
+ {
+ signal_ = false;
+ return true;
+ }
+ }
+ ::usleep(1000);
+ }
+ return false;
+}
+
+void BusEvent::signalFromInterrupt()
+{
+ signal_ = true; // HACK
+ for (unsigned i = 0; i < MaxPollWaiters; i++)
+ {
+ ::pollfd* const fd = pollset_[i];
+ if (fd != UAVCAN_NULLPTR)
+ {
+ fd->revents |= fd->events & POLLIN;
+ if ((fd->revents != 0) && (fd->sem->semcount <= 0))
+ {
+ (void)sem_post(fd->sem);
+ }
+ }
+ }
+}
+
+#endif
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/.gitignore Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,2 @@ +# Nested repository, we don't need to submodule it +zubax_chibios
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/Makefile Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,61 @@ +# +# Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> +# + +PROJECT = uavcan_test_stm32f107 + +# +# Test application +# + +MAIN ?= main.cpp + +CPPSRC = src/$(MAIN) \ + src/dummy.cpp \ + src/board/board.cpp + +# +# UAVCAN library +# + +export LIBUAVCAN_REPO_ROOT := $(abspath ../../..) + +UDEFS = -DUAVCAN_STM32_CHIBIOS=1 \ + -DUAVCAN_STM32_TIMER_NUMBER=6 \ + -DUAVCAN_TINY=1 \ + -DUAVCAN_STM32_NUM_IFACES=2 \ + -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 + +include $(LIBUAVCAN_REPO_ROOT)/libuavcan/include.mk +CPPSRC += $(LIBUAVCAN_SRC) +UINCDIR += $(LIBUAVCAN_INC) + +include $(LIBUAVCAN_REPO_ROOT)/libuavcan_drivers/stm32/driver/include.mk +CPPSRC += $(LIBUAVCAN_STM32_SRC) +UINCDIR += $(LIBUAVCAN_STM32_INC) + +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +UINCDIR += dsdlc_generated + +# +# Git commit hash +# + +GIT_HASH := $(shell git rev-parse --short HEAD) +UDEFS += -DGIT_HASH=0x$(GIT_HASH) + +# +# Platform +# + +UINCDIR += src/sys + +SERIAL_CLI_PORT_NUMBER = 2 + +CPPWARN := -Wundef -Wno-error=undef + +RELEASE_OPT = -Os -fomit-frame-pointer +DEBUG_OPT = -Os -g3 +#USE_OPT = -flto + +include zubax_chibios/rules_stm32f105_107.mk
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/README.md Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,4 @@ +UAVCAN test project for STM32 +----------------------------- + +Please checkout/symlink https://github.com/Zubax/zubax_chibios, branch `stable_v1`, into subdirectory `zubax_chibios`; then follow instructions in `zubax_chibios/README.md`.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,99 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "board.hpp"
+#include <cstring>
+#include <unistd.h>
+#include <ch.h>
+#include <ch.hpp>
+#include <hal.h>
+#include <zubax_chibios/sys/sys.h>
+
+/**
+ * GPIO config for ChibiOS PAL driver
+ */
+const PALConfig pal_default_config =
+{
+ { VAL_GPIOAODR, VAL_GPIOACRL, VAL_GPIOACRH },
+ { VAL_GPIOBODR, VAL_GPIOBCRL, VAL_GPIOBCRH },
+ { VAL_GPIOCODR, VAL_GPIOCCRL, VAL_GPIOCCRH },
+ { VAL_GPIODODR, VAL_GPIODCRL, VAL_GPIODCRH },
+ { VAL_GPIOEODR, VAL_GPIOECRL, VAL_GPIOECRH }
+};
+
+namespace board
+{
+
+void init()
+{
+ halInit();
+
+ chibios_rt::System::init();
+
+ sdStart(&STDOUT_SD, nullptr);
+}
+
+__attribute__((noreturn))
+void die(int error)
+{
+ lowsyslog("Fatal error %i\n", error);
+ while (1)
+ {
+ setLed(false);
+ ::sleep(1);
+ setLed(true);
+ ::sleep(1);
+ }
+}
+
+void setLed(bool state)
+{
+ palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state);
+}
+
+void restart()
+{
+ NVIC_SystemReset();
+}
+
+void readUniqueID(std::uint8_t bytes[UniqueIDSize])
+{
+ std::memcpy(bytes, reinterpret_cast<const void*>(0x1FFFF7E8), UniqueIDSize);
+}
+
+}
+
+/*
+ * Early init from ChibiOS
+ */
+extern "C"
+{
+
+void __early_init(void)
+{
+ stm32_clock_init();
+}
+
+void boardInit(void)
+{
+ AFIO->MAPR |=
+ AFIO_MAPR_CAN_REMAP_REMAP3 |
+ AFIO_MAPR_CAN2_REMAP |
+ AFIO_MAPR_USART2_REMAP;
+
+ /*
+ * Enabling the CAN controllers, then configuring GPIO functions for CAN_TX.
+ * Order matters, otherwise the CAN_TX pins will twitch, disturbing the CAN bus.
+ * This is why we can't perform this initialization using ChibiOS GPIO configuration.
+ */
+ RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+ palSetPadMode(GPIOD, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+
+#if UAVCAN_STM32_NUM_IFACES > 1
+ RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
+ palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+#endif
+}
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/test_stm32f107/src/board/board.hpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdint>
+
+namespace board
+{
+
+void init();
+
+__attribute__((noreturn))
+void die(int error);
+
+void setLed(bool state);
+
+void restart();
+
+constexpr unsigned UniqueIDSize = 12;
+
+void readUniqueID(std::uint8_t bytes[UniqueIDSize]);
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,215 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <unistd.h>
+#include <zubax_chibios/sys/sys.h>
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "board/board.hpp"
+
+namespace app
+{
+namespace
+{
+
+uavcan_stm32::CanInitHelper<128> can;
+
+constexpr unsigned NodePoolSize = 16384;
+
+uavcan::Node<NodePoolSize>& getNode()
+{
+ static uavcan::Node<NodePoolSize> node(can.driver, uavcan_stm32::SystemClock::instance());
+ return node;
+}
+
+void init()
+{
+ board::init();
+
+ /*
+ * CAN auto bit rate detection.
+ * Automatic bit rate detection requires that the bus has at least one other CAN node publishing some
+ * frames periodically.
+ * Auto bit rate detection can be bypassed byif the desired bit rate is passed directly to can.init(), e.g.:
+ * can.init(1000000);
+ */
+ int res = 0;
+ do
+ {
+ ::sleep(1);
+ ::lowsyslog("CAN auto bit rate detection...\n");
+
+ std::uint32_t bitrate = 0;
+ res = can.init([]() { ::usleep(can.getRecommendedListeningDelay().toUSec()); }, bitrate);
+ if (res >= 0)
+ {
+ ::lowsyslog("CAN inited at %u bps\n", unsigned(bitrate));
+ }
+ }
+ while (res < 0);
+}
+
+class : public chibios_rt::BaseStaticThread<8192>
+{
+ void configureNodeInfo()
+ {
+ getNode().setName("org.uavcan.stm32_test_stm32f107");
+
+ /*
+ * Software version
+ * TODO: Fill other fields too
+ */
+ uavcan::protocol::SoftwareVersion swver;
+
+ swver.vcs_commit = GIT_HASH;
+ swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
+
+ getNode().setSoftwareVersion(swver);
+
+ lowsyslog("Git commit hash: 0x%08x\n", GIT_HASH);
+
+ /*
+ * Hardware version
+ * TODO: Fill other fields too
+ */
+ uavcan::protocol::HardwareVersion hwver;
+
+ std::uint8_t uid[board::UniqueIDSize] = {};
+ board::readUniqueID(uid);
+ std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
+
+ getNode().setHardwareVersion(hwver);
+
+ lowsyslog("UDID:");
+ for (auto b : hwver.unique_id)
+ {
+ lowsyslog(" %02x", unsigned(b));
+ }
+ lowsyslog("\n");
+ }
+
+ void performDynamicNodeIDAllocation()
+ {
+ uavcan::DynamicNodeIDClient client(getNode());
+
+ const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
+ if (client_start_res < 0)
+ {
+ board::die(client_start_res);
+ }
+
+ lowsyslog("Waiting for dynamic node ID allocation...\n");
+ while (!client.isAllocationComplete())
+ {
+ const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
+ if (spin_res < 0)
+ {
+ lowsyslog("Spin failure: %i\n", spin_res);
+ }
+ }
+
+ lowsyslog("Dynamic node ID %d allocated by %d\n",
+ int(client.getAllocatedNodeID().get()),
+ int(client.getAllocatorNodeID().get()));
+
+ getNode().setNodeID(client.getAllocatedNodeID());
+ }
+
+public:
+ msg_t main()
+ {
+ /*
+ * Setting up the node parameters
+ */
+ configureNodeInfo();
+
+ /*
+ * Initializing the UAVCAN node
+ */
+ const int node_init_res = getNode().start();
+ if (node_init_res < 0)
+ {
+ board::die(node_init_res);
+ }
+
+ /*
+ * Waiting for a dynamic node ID allocation
+ */
+ performDynamicNodeIDAllocation();
+
+ /*
+ * Time synchronizer
+ */
+ static uavcan::GlobalTimeSyncSlave time_sync_slave(getNode());
+ {
+ const int res = time_sync_slave.start();
+ if (res < 0)
+ {
+ board::die(res);
+ }
+ }
+
+ /*
+ * Main loop
+ */
+ lowsyslog("UAVCAN node started\n");
+ getNode().setModeOperational();
+ while (true)
+ {
+ const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(5000));
+ if (spin_res < 0)
+ {
+ lowsyslog("Spin failure: %i\n", spin_res);
+ }
+
+ lowsyslog("Time sync master: %u\n", unsigned(time_sync_slave.getMasterNodeID().get()));
+
+ lowsyslog("Memory usage: free=%u used=%u worst=%u\n",
+ getNode().getAllocator().getNumFreeBlocks(),
+ getNode().getAllocator().getNumUsedBlocks(),
+ getNode().getAllocator().getPeakNumUsedBlocks());
+
+ lowsyslog("CAN errors: %lu %lu\n",
+ static_cast<unsigned long>(can.driver.getIface(0)->getErrorCount()),
+ static_cast<unsigned long>(can.driver.getIface(1)->getErrorCount()));
+
+#if !UAVCAN_TINY
+ node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::INFO);
+ node.logInfo("app", "UTC %* sec, %* corr, %* jumps",
+ uavcan_stm32::clock::getUtc().toMSec() / 1000,
+ uavcan_stm32::clock::getUtcSpeedCorrectionPPM(),
+ uavcan_stm32::clock::getUtcAjdustmentJumpCount());
+#endif
+ }
+ return msg_t();
+ }
+} uavcan_node_thread;
+
+}
+}
+
+int main()
+{
+ app::init();
+
+ lowsyslog("Starting the UAVCAN thread\n");
+ app::uavcan_node_thread.start(LOWPRIO);
+
+ while (true)
+ {
+ for (int i = 0; i < 200; i++)
+ {
+ board::setLed(app::can.driver.hadActivity());
+ ::usleep(25000);
+ }
+
+ const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
+ lowsyslog("UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n",
+ static_cast<unsigned long>(utc.toMSec() / 1000),
+ uavcan_stm32::clock::getUtcRateCorrectionPPM(),
+ uavcan_stm32::clock::getUtcJumpCount(),
+ int(uavcan_stm32::clock::isUtcLocked()));
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+/// Assert is needed for STM32 SPL (if it is being used, that is)
+#include <assert.h>
+#define assert_param(x) assert(x)
+
+#define STM32_LSECLK 32768
+#define STM32_HSECLK 25000000
+
+#define STM32F10X_CL
+
+/*
+ * GPIO
+ */
+// LED
+#define GPIO_PORT_LED GPIOB
+#define GPIO_PIN_LED 9
+
+// GPIOD 10 is configured as OUTPUT, it is used as board reboot monitor.
+
+/*
+ * I/O ports initial setup, this configuration is established soon after reset
+ * in the initialization code.
+ *
+ * The digits have the following meaning:
+ * 0 - Analog input.
+ * 1 - Push Pull output 10MHz.
+ * 2 - Push Pull output 2MHz.
+ * 3 - Push Pull output 50MHz.
+ * 4 - Digital input.
+ * 5 - Open Drain output 10MHz.
+ * 6 - Open Drain output 2MHz.
+ * 7 - Open Drain output 50MHz.
+ * 8 - Digital input with PullUp or PullDown resistor depending on ODR.
+ * 9 - Alternate Push Pull output 10MHz.
+ * A - Alternate Push Pull output 2MHz.
+ * B - Alternate Push Pull output 50MHz.
+ * C - Reserved.
+ * D - Alternate Open Drain output 10MHz.
+ * E - Alternate Open Drain output 2MHz.
+ * F - Alternate Open Drain output 50MHz.
+ * Please refer to the STM32 Reference Manual for details.
+ */
+
+#define VAL_GPIOACRL 0x88888888 // 7..0
+#define VAL_GPIOACRH 0x88888888 // 15..8
+#define VAL_GPIOAODR 0x00000000
+
+#define VAL_GPIOBCRL 0x84488888 // CAN2 TX initialized as INPUT, it must be configured later!
+#define VAL_GPIOBCRH 0x88888828
+#define VAL_GPIOBODR 0x00000000
+
+#define VAL_GPIOCCRL 0x88888888
+#define VAL_GPIOCCRH 0x88888888
+#define VAL_GPIOCODR 0x00000000
+
+#define VAL_GPIODCRL 0x88b88844 // CAN1 TX initialized as INPUT, it must be configured later!
+#define VAL_GPIODCRH 0x88888288
+#define VAL_GPIODODR ((1 << 10))
+
+#define VAL_GPIOECRL 0x88888888
+#define VAL_GPIOECRH 0x88888888
+#define VAL_GPIOEODR 0x00000000
+
+#if !defined(_FROM_ASM_)
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void boardInit(void);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FROM_ASM_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/src/sys/chconf.h Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#define CH_FREQUENCY 1000 + +#define CH_USE_HEAP TRUE +#define CH_USE_DYNAMIC FALSE + +#if defined(DEBUG_BUILD) && DEBUG_BUILD +# define CH_OPTIMIZE_SPEED FALSE +# define CH_DBG_SYSTEM_STATE_CHECK TRUE +# define CH_DBG_ENABLE_CHECKS TRUE +# define CH_DBG_ENABLE_ASSERTS TRUE +# define CH_DBG_ENABLE_STACK_CHECK TRUE +# define CH_DBG_FILL_THREADS TRUE +# define CH_DBG_THREADS_PROFILING TRUE +#elif defined(RELEASE_BUILD) && RELEASE_BUILD +# define CH_DBG_THREADS_PROFILING FALSE +#else +# error "Invalid configuration: Either DEBUG_BUILD or RELEASE_BUILD must be true" +#endif + +#define PORT_IDLE_THREAD_STACK_SIZE 64 +#define PORT_INT_REQUIRED_STACK 256 + +#include <zubax_chibios/sys/chconf_tail.h>
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/src/sys/halconf.h Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include "mcuconf.h" + +#define HAL_USE_TM TRUE +#define HAL_USE_PAL TRUE +#define HAL_USE_ADC FALSE +#define HAL_USE_CAN FALSE +#define HAL_USE_DAC FALSE +#define HAL_USE_EXT FALSE +#define HAL_USE_GPT FALSE +#define HAL_USE_I2C FALSE +#define HAL_USE_ICU FALSE +#define HAL_USE_MAC FALSE +#define HAL_USE_MMC_SPI FALSE +#define HAL_USE_PWM FALSE +#define HAL_USE_RTC FALSE +#define HAL_USE_SDC FALSE +#define HAL_USE_SERIAL TRUE +#define HAL_USE_SERIAL_USB FALSE +#define HAL_USE_SPI FALSE +#define HAL_USE_UART FALSE +#define HAL_USE_USB FALSE + +#define SERIAL_DEFAULT_BITRATE 115200 +#define SERIAL_BUFFERS_SIZE 128 + +#include <zubax_chibios/sys/halconf_tail.h>
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/src/sys/mcuconf.h Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,191 @@ +/* + * STM32F107 drivers configuration. + * The following settings override the default settings present in + * the various device driver implementation headers. + * Note that the settings for each driver only have effect if the whole + * driver is enabled in halconf.h. + * + * IRQ priorities: + * 15...0 Lowest...Highest. + * + * DMA priorities: + * 0...3 Lowest...Highest. + */ + +#define STM32F107_MCUCONF + +/* + * HAL driver system settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_HSI_ENABLED TRUE +#define STM32_LSI_ENABLED TRUE +#define STM32_HSE_ENABLED TRUE +#define STM32_LSE_ENABLED FALSE +#define STM32_SW STM32_SW_PLL +#define STM32_PLLSRC STM32_PLLSRC_PREDIV1 +#define STM32_PREDIV1SRC STM32_PREDIV1SRC_PLL2 +#define STM32_PREDIV1_VALUE 5 +#define STM32_PLLMUL_VALUE 9 +#define STM32_PREDIV2_VALUE 5 +#define STM32_PLL2MUL_VALUE 8 +#define STM32_PLL3MUL_VALUE 10 +#define STM32_HPRE STM32_HPRE_DIV1 +#define STM32_PPRE1 STM32_PPRE1_DIV2 +#define STM32_PPRE2 STM32_PPRE2_DIV2 +#define STM32_ADCPRE STM32_ADCPRE_DIV4 +#define STM32_OTG_CLOCK_REQUIRED FALSE +#define STM32_OTGFSPRE STM32_OTGFSPRE_DIV3 +#define STM32_I2S_CLOCK_REQUIRED FALSE +#define STM32_MCOSEL STM32_MCOSEL_PLL3 +#define STM32_RTCSEL STM32_RTCSEL_HSEDIV +#define STM32_PVD_ENABLE FALSE +#define STM32_PLS STM32_PLS_LEV0 + +/* + * ADC driver system settings. + */ +#define STM32_ADC_USE_ADC1 FALSE +#define STM32_ADC_ADC1_DMA_PRIORITY 2 +#define STM32_ADC_ADC1_IRQ_PRIORITY 6 + +/* + * CAN driver system settings. + */ +#define STM32_CAN_USE_CAN1 FALSE +#define STM32_CAN_USE_CAN2 FALSE +#define STM32_CAN_CAN1_IRQ_PRIORITY 4 +#define STM32_CAN_CAN2_IRQ_PRIORITY 4 + +/* + * EXT driver system settings. + */ +#define STM32_EXT_EXTI0_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI1_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI2_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI3_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI4_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI5_9_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI10_15_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI16_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI17_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI18_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI19_IRQ_PRIORITY 6 + +/* + * GPT driver system settings. + */ +#define STM32_GPT_USE_TIM1 FALSE +#define STM32_GPT_USE_TIM2 FALSE +#define STM32_GPT_USE_TIM3 FALSE +#define STM32_GPT_USE_TIM4 FALSE +#define STM32_GPT_USE_TIM5 FALSE +#define STM32_GPT_USE_TIM8 FALSE +#define STM32_GPT_TIM1_IRQ_PRIORITY 7 +#define STM32_GPT_TIM2_IRQ_PRIORITY 7 +#define STM32_GPT_TIM3_IRQ_PRIORITY 7 +#define STM32_GPT_TIM4_IRQ_PRIORITY 7 +#define STM32_GPT_TIM5_IRQ_PRIORITY 7 +#define STM32_GPT_TIM8_IRQ_PRIORITY 7 + +/* + * I2C driver system settings. + */ +#define STM32_I2C_USE_I2C1 FALSE +#define STM32_I2C_USE_I2C2 FALSE +#define STM32_I2C_I2C1_IRQ_PRIORITY 5 +#define STM32_I2C_I2C2_IRQ_PRIORITY 5 +#define STM32_I2C_I2C1_DMA_PRIORITY 3 +#define STM32_I2C_I2C2_DMA_PRIORITY 3 +#define STM32_I2C_I2C1_DMA_ERROR_HOOK() chSysHalt() +#define STM32_I2C_I2C2_DMA_ERROR_HOOK() chSysHalt() + +/* + * ICU driver system settings. + */ +#define STM32_ICU_USE_TIM1 FALSE +#define STM32_ICU_USE_TIM2 FALSE +#define STM32_ICU_USE_TIM3 FALSE +#define STM32_ICU_USE_TIM4 FALSE +#define STM32_ICU_USE_TIM5 FALSE +#define STM32_ICU_USE_TIM8 FALSE +#define STM32_ICU_TIM1_IRQ_PRIORITY 7 +#define STM32_ICU_TIM2_IRQ_PRIORITY 7 +#define STM32_ICU_TIM3_IRQ_PRIORITY 7 +#define STM32_ICU_TIM4_IRQ_PRIORITY 7 +#define STM32_ICU_TIM5_IRQ_PRIORITY 7 +#define STM32_ICU_TIM8_IRQ_PRIORITY 7 + +/* + * PWM driver system settings. + */ +#define STM32_PWM_USE_ADVANCED FALSE +#define STM32_PWM_USE_TIM1 TRUE +#define STM32_PWM_USE_TIM2 FALSE +#define STM32_PWM_USE_TIM3 FALSE +#define STM32_PWM_USE_TIM4 FALSE +#define STM32_PWM_USE_TIM5 FALSE +#define STM32_PWM_USE_TIM8 FALSE +#define STM32_PWM_TIM1_IRQ_PRIORITY 7 +#define STM32_PWM_TIM2_IRQ_PRIORITY 7 +#define STM32_PWM_TIM3_IRQ_PRIORITY 7 +#define STM32_PWM_TIM4_IRQ_PRIORITY 7 +#define STM32_PWM_TIM5_IRQ_PRIORITY 7 +#define STM32_PWM_TIM8_IRQ_PRIORITY 7 + +/* + * RTC driver system settings. + */ +#define STM32_RTC_IRQ_PRIORITY 15 + +/* + * SERIAL driver system settings. + */ +#define STM32_SERIAL_USE_USART1 FALSE +#define STM32_SERIAL_USE_USART2 TRUE +#define STM32_SERIAL_USE_USART3 FALSE +#define STM32_SERIAL_USE_UART4 FALSE +#define STM32_SERIAL_USE_UART5 FALSE +#define STM32_SERIAL_USART1_PRIORITY 12 +#define STM32_SERIAL_USART2_PRIORITY 12 +#define STM32_SERIAL_USART3_PRIORITY 12 +#define STM32_SERIAL_UART4_PRIORITY 12 +#define STM32_SERIAL_UART5_PRIORITY 12 + +/* + * SPI driver system settings. + */ +#define STM32_SPI_USE_SPI1 FALSE +#define STM32_SPI_USE_SPI2 FALSE +#define STM32_SPI_USE_SPI3 FALSE +#define STM32_SPI_SPI1_DMA_PRIORITY 1 +#define STM32_SPI_SPI2_DMA_PRIORITY 1 +#define STM32_SPI_SPI3_DMA_PRIORITY 1 +#define STM32_SPI_SPI1_IRQ_PRIORITY 10 +#define STM32_SPI_SPI2_IRQ_PRIORITY 10 +#define STM32_SPI_SPI3_IRQ_PRIORITY 10 +#define STM32_SPI_DMA_ERROR_HOOK(spip) chSysHalt() + +/* + * UART driver system settings. + */ +#define STM32_UART_USE_USART1 FALSE +#define STM32_UART_USE_USART2 FALSE +#define STM32_UART_USE_USART3 FALSE +#define STM32_UART_USART1_IRQ_PRIORITY 12 +#define STM32_UART_USART2_IRQ_PRIORITY 12 +#define STM32_UART_USART3_IRQ_PRIORITY 12 +#define STM32_UART_USART1_DMA_PRIORITY 0 +#define STM32_UART_USART2_DMA_PRIORITY 0 +#define STM32_UART_USART3_DMA_PRIORITY 0 +#define STM32_UART_DMA_ERROR_HOOK(uartp) chSysHalt() + +/* + * USB driver system settings. + */ +#define STM32_USB_USE_OTG1 FALSE +#define STM32_USB_OTG1_IRQ_PRIORITY 14 +#define STM32_USB_OTG1_RX_FIFO_SIZE 512 +#define STM32_USB_OTG_THREAD_PRIO LOWPRIO +#define STM32_USB_OTG_THREAD_STACK_SIZE 128 +#define STM32_USB_OTGFIFO_FILL_BASEPRI 0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tools/scitools_understand_buildspy.sh Sat Apr 14 10:25:32 2018 +0000
@@ -0,0 +1,26 @@
+#!/bin/sh
+#
+# This script generates project file for SciTools Understand via buildspy.
+#
+
+buildspy_dir="$1"
+uavcan_dir="$(readlink -f $(dirname $0)/..)"
+
+function die() { echo $1; exit 1; }
+
+[ -z "$buildspy_dir" ] && die "Path to buildspy directory expected, e.g. ~/opt/scitools/bin/linux64/buildspy/"
+
+compiler="$buildspy_dir/g++wrapper"
+buildspy="$buildspy_dir/buildspy"
+
+echo "Pathes:"
+echo "compiler: $compiler"
+echo "buildspy: $buildspy"
+echo "uavcan: $uavcan_dir"
+
+read -p "Looks good? (y/N) " confirm
+[[ $confirm == "y" ]] || die "Bye"
+
+cmake "$uavcan_dir" -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_COMPILER="$compiler" || exit 1
+
+$buildspy -db uavcan.udb -cmd make