*

ODrive-fw-v0.5.6/Firmware/fibre-cpp/.gitignore

@@ -0,0 +1,4 @@
+/third_party
+build/
+build-*/
+/.tup

ODrive-fw-v0.5.6/Firmware/fibre-cpp/Dockerfile

@@ -0,0 +1,66 @@
+FROM archlinux:base-devel
+
+# Set up package manager
+RUN echo "[custom]" >> /etc/pacman.conf && \
+    echo "SigLevel = Required TrustedOnly" >> /etc/pacman.conf && \
+    echo "Server = https://innovation-labs.appinstall.ch/archlinux/\$repo/os/\$arch" >> /etc/pacman.conf && \
+    pacman-key --init && \
+    pacman-key --recv-keys 0CB4116A1A3A789937D6DEFB506F27823D2B7B33 && \
+    pacman-key --lsign-key 0CB4116A1A3A789937D6DEFB506F27823D2B7B33 && \
+    pacman -Syu --noconfirm
+
+# Install prerequisites for the following targets:
+#  - Linux (AMD64)
+#  - Linux (ARM)
+#  - Windows (AMD64)
+#  - macOS (x86_32/AMD64)
+#  - WebAssembly
+RUN pacman -S --noconfirm tup clang gcc binutils wget && \
+    pacman -S --noconfirm arm-linux-gnueabihf-gcc arm-linux-gnueabihf-binutils && \
+    pacman -S --noconfirm mingw-w64-gcc mingw-w64-binutils p7zip && \
+    pacman -S --noconfirm apple-darwin-osxcross && \
+    pacman -S --noconfirm emscripten
+
+ENV PATH=${PATH}:/opt/osxcross/bin
+ENV PATH=${PATH}:/usr/lib/emscripten
+
+COPY get_dependencies.sh /get_dependencies.sh
+
+# Download and compile dependencies
+RUN /get_dependencies.sh download_deb_pkg libusb-dev-amd64 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.23-2build1_amd64.deb" && \
+    /get_dependencies.sh download_deb_pkg libusb-amd64 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.23-2build1_amd64.deb" && \
+    /get_dependencies.sh download_deb_pkg libusb-i386 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.23-2build1_i386.deb" && \
+    /get_dependencies.sh download_deb_pkg libusb-dev-i386 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.23-2build1_i386.deb" && \
+    /get_dependencies.sh download_deb_pkg libusb-armhf "http://mirrordirector.raspbian.org/raspbian/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.24-2_armhf.deb" && \
+    /get_dependencies.sh download_deb_pkg libusb-dev-armhf "http://mirrordirector.raspbian.org/raspbian/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.24-2_armhf.deb" && \
+    /get_dependencies.sh download_deb_pkg libstdc++-linux-armhf "http://mirrors.kernel.org/ubuntu/pool/universe/g/gcc-10-cross/libstdc++-10-dev-armhf-cross_10-20200411-0ubuntu1cross1_all.deb"
+
+
+RUN /get_dependencies.sh patch_macos_sdk && \
+    CC='/opt/osxcross/bin/o64-clang' LD_LIBRARY_PATH="/opt/osxcross/lib" CFLAGS='-I/opt/osxcross/SDK/MacOSX10.13.sdk/usr/include -arch i386 -arch x86_64' MACOSX_DEPLOYMENT_TARGET='10.9' /get_dependencies.sh compile_libusb 'macos-amd64' 'x86_64-apple-darwin17'
+
+RUN mkdir -p "third_party/libusb-windows" && \
+    pushd "third_party/libusb-windows" > /dev/null && \
+    wget "https://github.com/libusb/libusb/releases/download/v1.0.23/libusb-1.0.23.7z" && \
+    7z x -o"libusb-1.0.23" "libusb-1.0.23.7z"
+
+# Make Emscripten build its standard libraries for the WebAssembly target
+RUN echo "void test() {}" | em++ -x c - -o /tmp/a.out
+
+# Install dependencies for interface_generator.py
+RUN pacman -S --noconfirm python-yaml python-jinja python-jsonschema
+
+ENV THIRD_PARTY=/
+
+# Set up entrypoint
+RUN echo "#!/bin/bash" > /entrypoint.sh && \
+    echo "set -euo pipefail" >> /entrypoint.sh && \
+    echo "rm -rdf build/*" >> /entrypoint.sh && \
+    echo "echo building \$@" >> /entrypoint.sh && \
+    echo "tup generate --config \$@ /tmp/build.sh" >> /entrypoint.sh && \
+    echo "exec /usr/bin/bash -x -e /tmp/build.sh" >> /entrypoint.sh && \
+    chmod +x /entrypoint.sh && \
+    mkdir /build
+
+WORKDIR /build
+ENTRYPOINT ["/entrypoint.sh"]

ODrive-fw-v0.5.6/Firmware/fibre-cpp/Makefile

@@ -0,0 +1,6 @@
+
+all:
+	tup --no-environ-check build-local
+	tup --no-environ-check build-wasm
+	cp build-local/libfibre-* ../python/fibre/
+	cp build-wasm/libfibre-* ../js/

ODrive-fw-v0.5.6/Firmware/fibre-cpp/README.md

@@ -0,0 +1,90 @@
+# fibre-cpp
+
+This directory provides the C++ reference implementation of [Fibre](https://github.com/samuelsadok/fibre). Its home is located [here](https://github.com/samuelsadok/fibre/tree/master/cpp). There's also a standalone repository for this directory [here](https://github.com/samuelsadok/fibre-cpp).
+
+## Overview
+
+There are two approaches to include Fibre in your project:
+
+  1. **Embedding fibre-cpp:** Your application's build process includes the source code files of fibre-cpp. Your application uses Fibre's C++ API to interact with Fibre. This is the recommended approach for embedded systems.
+  2. **Linking to libfibre:** Your application links to a separately compiled library `libfibre` and uses Fibre's C API to interact with this library. You can obtain precompiled binaries on the main project's [release page](https://github.com/samuelsadok/fibre/releases). This is the recommended approach for desktop systems, where you want all backends enabled, because you can avoid the burden of collecting build dependencies. _Note:_ currently only the client role is supported with this approach. That means you can use it to discover and access remote objects but you cannot use it to expose local objects yet.
+
+## Configuring fibre-cpp
+
+Various preprocessor defines can be used to customize Fibre:
+
+ - `FIBRE_ENABLE_SERVER={0|1}` (_default 0_): Enable support for exposing objects to remote peers.
+ - `FIBRE_ENABLE_CLIENT={0|1}` (_default 0_): Enable support for discovering and using objects exposed by remote peers.
+ - `FIBRE_ENABLE_EVENT_LOOP={0|1}` (_default 0_): Enable the builtin event loop implementation. Not supported on all platforms.
+ - `FIBRE_ALLOW_HEAP={0|1}` (_default 0_): Allow Fibre to allocate memory on the heap using `malloc` and `free`. If this option is disabled only one Fibre instance can be opened. Currently `FIBRE_ENABLE_CLIENT` (and several other options) cannot be used together with this option.
+ - `FIBRE_MAX_LOG_VERBOSITY={0...5}` (_default 5_): The maximum log verbosity that will be compiled into the binary. In embedded systems it's recommended to set this to 0 to reduce binary size. On platforms that support environment variables the actual run time log verbosity can be changed by setting the environment variable `FIBRE_LOG={0...5}`.
+ - `FIBRE_DEFAULT_LOG_VERBOSITY={0...5}` (_default 5_): The default log verbosity that will be used unless overridden by other means (for instance through the environment variables).
+ - `FIBRE_ENABLE_LIBUSB_BACKEND={0|1}` (_default 0_): Enable libusb backend for host side USB support. This requires `FIBRE_ALLOC_HEAP=1`.
+ - `FIBRE_ENABLE_TCP_CLIENT_BACKEND={0|1}` (_default 0_): Enable TCP client backend. This requires `FIBRE_ALLOC_HEAP=1`.
+ - `FIBRE_ENABLE_TCP_SERVER_BACKEND={0|1}` (_default 0_): Enable TCP server backend. This requires `FIBRE_ALLOC_HEAP=1`.
+
+## Adding fibre-cpp to your application's build process
+
+If your application uses [tup](http://gittup.org/tup/) as build system you can directly call the function `get_fibre_package()` in [package.lua](package.lua) as part of your build process. This function spits out a list of code files and compiler flags needed to compile fibre-cpp for a given configuration. Refer to [package.lua](package.lua) for more details.
+
+If your application doesn't use tup, you have to manually check which code files you need.
+
+## Using fibre-cpp
+
+Currently there's no nice walkthrough for this but here are two applications that you can use as an example:
+
+ - The [ODrive Firmware](https://github.com/madcowswe/ODrive/tree/devel/Firmware)
+ - The [test server](https://github.com/samuelsadok/fibre/blob/devel/test/test_server.cpp)
+
+## Configuring `libfibre`
+
+A file called tup.config can be placed in this directory to customize the build. See [configs](configs/) for examples.
+
+## Compiling `libfibre`
+
+Before you compile libfibre yourself consider if the [official releases](https://github.com/samuelsadok/fibre/releases) may be suitable for you instead.
+
+The recommended way for compiling libfibre is using Docker. You can use the same docker container to cross-compile for all supported targets.
+
+However if you're actively developing fibre you may want to compile natively for faster compile times.
+
+### Docker
+
+The following example compiles libfibre for the `linux-amd64` target. Refer to the "configs/" folder for a list of supported targets. You can also add new targets there but you may need to modify the Dockerfile to include tooling for your new target.
+
+```
+docker build -t fibre-compiler .
+docker run -it -v /tmp/build:/build/cpp/build --entrypoint bash fibre-compiler -c "rm -rd /build/cpp/build/*"
+docker run -it -v "$(pwd)":/build -v /tmp/build:/build/build -w /build fibre-compiler configs/linux-amd64.config
+```
+
+The output file is now located under `/tmp/build/libfibre-linux-amd64.so` on your host system.
+
+If something fails you can enter the container interactively with `docker run -it -v "$(pwd)":/build -v /tmp/build:/build/build -w /build --entrypoint bash fibre-compiler`.
+
+### Windows
+  1. Download MinGW from [here](https://sourceforge.net/projects/mingw-w64/files/Toolchains%20targetting%20Win32/Personal%20Builds/mingw-builds/installer/mingw-w64-install.exe/download) and install it.
+  2. Add `C:\Program Files\mingw-w64\x86_64-8.1.0-posix-seh-rt_v6-rev0\mingw64\bin` (or similar) to your `PATH` environment variable.
+  3. Download the libusb binaries from [here](`https://github.com/libusb/libusb/releases/download/v1.0.23/libusb-1.0.23.7z`) and unpack them to `third_party/libusb-windows` (such that the file `third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a` exists).
+  4. Navigate to this directory and run `make`
+
+### Ubuntu
+  1. `sudo apt-get install libusb-1.0-0-dev`
+  2. Navigate to this directory and run `make`
+
+### macOS
+  1. `brew install libusb`
+  2. Navigate to this directory and run `make`
+
+## Using `libfibre`
+
+The API is documented in [libfibre.h](include/fibre/libfibre.h).
+
+To compile your application you need to link against the libfibre binary (`-L/path/to/libfibre.so`) and add "libfibre.h" to your include path under a folder named "fibre", e.g. `-I/path/to/fibre-cpp/include`.
+
+
+## Notes for Contributors
+
+ - Fibre currently targets C++11 to maximize compatibility with other projects
+ - Notes on platform independent programming:
+   - Don't use the keyword `interface` (defined as a macro on Windows in `rpc.h`)

ODrive-fw-v0.5.6/Firmware/fibre-cpp/Tupfile.lua

@@ -0,0 +1,144 @@
+
+-- Projects that include fibre-cpp and also use tup can place a Tuprules.lua file
+-- into their root directory with the line `no_libfibre = true` to prevent
+-- libfibre from building.
+if no_libfibre == true then
+    return
+end
+
+tup.include('package.lua')
+
+CFLAGS = {'-fPIC -std=c++11 -DFIBRE_COMPILE -Wall'}
+LDFLAGS = {'-static-libstdc++'}
+
+
+if tup.getconfig("CC") == "" then
+    CXX = 'clang++'
+    LINKER = 'clang++'
+else
+    CXX = tup.getconfig("CC")
+    LINKER = tup.getconfig("CC")
+end
+
+function get_bool_config(name, default)
+    if tup.getconfig(name) == "" then
+        return default
+    elseif tup.getconfig(name) == "true" then
+        return true
+    elseif tup.getconfig(name) == "false" then
+        return false
+    else
+        error(name.." ("..tup.getconfig(name)..") must be 'true' or 'false'.")
+    end
+end
+
+CFLAGS += tup.getconfig("CFLAGS")
+LDFLAGS += tup.getconfig("LDFLAGS")
+DEBUG = get_bool_config("DEBUG", true)
+STRICT = get_bool_config("STRICT", false)
+
+machine = fibre_run_now(CXX..' -dumpmachine') -- works with both clang and GCC
+
+BUILD_TYPE='-shared'
+
+if string.find(machine, "x86_64.*%-linux%-.*") then
+    outname = 'libfibre-linux-amd64.so'
+    LDFLAGS += '-lpthread -Wl,--version-script=libfibre.version -Wl,--gc-sections'
+    STRIP = not DEBUG
+elseif string.find(machine, "arm.*%-linux%-.*") then
+	outname = 'libfibre-linux-armhf.so'
+    LDFLAGS += '-lpthread -Wl,--version-script=libfibre.version -Wl,--gc-sections'
+    STRIP = false
+elseif string.find(machine, "x86_64.*-mingw.*") then
+    outname = 'libfibre-windows-amd64.dll'
+    LDFLAGS += '-lpthread -Wl,--version-script=libfibre.version'
+    STRIP = not DEBUG
+elseif string.find(machine, "x86_64.*-apple-.*") then
+    outname = 'libfibre-macos-x86.dylib'
+    STRIP = false
+elseif string.find(machine, "wasm.*") then
+    outname = 'libfibre-wasm.js'
+    STRIP = false
+    BUILD_TYPE = ''
+else
+    error('unknown machine identifier '..machine)
+end
+
+LDFLAGS += BUILD_TYPE
+
+if DEBUG then
+    CFLAGS += '-O1 -g'
+else
+    CFLAGS += '-O3' -- TODO: add back -lfto
+end
+
+if STRICT then
+    CFLAGS += '-Werror'
+end
+
+function compile(src_file)
+    obj_file = 'build/'..tup.file(src_file)..'.o'
+    tup.frule{
+        inputs={src_file},
+        command='^co^ '..CXX..' -c %f '..tostring(CFLAGS)..' -o %o',
+        outputs={obj_file}
+    }
+    return obj_file
+end
+
+pkg = get_fibre_package({
+    enable_server=false,
+    enable_client=true,
+    enable_tcp_server_backend=get_bool_config("ENABLE_TCP_SERVER_BACKEND", true),
+    enable_tcp_client_backend=get_bool_config("ENABLE_TCP_CLIENT_BACKEND", true),
+    enable_libusb_backend=get_bool_config("ENABLE_LIBUSB_BACKEND", true),
+    allow_heap=true,
+    pkgconf=(tup.getconfig("USE_PKGCONF") != "") and tup.getconfig("USE_PKGCONF") or nil
+})
+
+CFLAGS += pkg.cflags
+LDFLAGS += pkg.ldflags
+
+for _, inc in pairs(pkg.include_dirs) do
+    CFLAGS += '-I./'..inc
+end
+
+for _, src_file in pairs(pkg.code_files) do
+    object_files += compile(src_file)
+end
+object_files += compile('libfibre.cpp')
+
+outname = 'build/'..outname
+
+if not STRIP then
+    compile_outname=outname
+else
+    compile_outname=outname..'.fat'
+end
+
+if tup.ext(outname) == 'js' then
+    extra_outputs = {tup.base(compile_outname)..'.wasm'}
+else
+    extra_outputs = {}
+end
+
+tup.frule{
+    inputs=object_files,
+    command='^c^ '..LINKER..' %f '..tostring(CFLAGS)..' '..tostring(LDFLAGS)..' -o %o',
+    outputs={compile_outname, extra_outputs=extra_outputs}
+}
+
+if STRIP then
+    tup.frule{
+        inputs={compile_outname},
+        command='strip --strip-all --discard-all %f -o %o',
+        outputs={outname}
+    }
+end
+
+if string.find(machine, "x86_64.*-apple-.*") then
+    tup.frule{
+        inputs=outname,
+        command='^c^ chmod 644 %f',
+    }
+end

ODrive-fw-v0.5.6/Firmware/fibre-cpp/channel_discoverer.cpp

@@ -0,0 +1,47 @@
+
+#include <fibre/channel_discoverer.hpp>
+#include <string.h>
+#include <stdio.h>
+#include <algorithm>
+
+using namespace fibre;
+
+bool ChannelDiscoverer::try_parse_key(const char* begin, const char* end, const char* key, const char** val_begin, const char** val_end) {
+    ssize_t keylen = strlen(key);
+
+    while (begin != end) {
+        const char* next_delim = std::find(begin, end, ',');
+        
+        if ((next_delim - begin >= keylen) && (memcmp(begin, key, keylen) == 0)) {
+            if (next_delim - begin == keylen) {
+                // The key exists but has no value
+                *val_begin = *val_end = next_delim;
+                return true;
+            } else if (begin[keylen] == '=') {
+                *val_begin = begin + keylen + 1;
+                *val_end = next_delim;
+                return true;
+            }
+        }
+
+        begin = std::min(next_delim + 1, end);
+    }
+
+    return false; // key not found
+}
+
+bool ChannelDiscoverer::try_parse_key(const char* begin, const char* end, const char* key, int* val) {
+    const char* val_begin;
+    const char* val_end;
+    if (!try_parse_key(begin, end, key, &val_begin, &val_end)) {
+        return false;
+    }
+
+    // Copy value to a null-terminated buffer
+    char buf[val_end - val_begin + 1];
+    memcpy(buf, val_begin, val_end - val_begin);
+    buf[val_end - val_begin] = 0;
+
+    return sscanf(buf, "0x%x", val) == 1
+        || sscanf(buf, "%d", val) == 1;
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/configs/linux-amd64.config

@@ -0,0 +1,6 @@
+CONFIG_DEBUG=false
+CONFIG_STRICT=true
+CONFIG_CC="clang++"
+CONFIG_CFLAGS="-I$THIRD_PARTY./third_party/libusb-dev-armhf/usr/include/libusb-1.0"
+CONFIG_LDFLAGS="$THIRD_PARTY./third_party/libusb-amd64/lib/x86_64-linux-gnu/libusb-1.0.so.0.2.0"
+CONFIG_USE_PKGCONF=false

ODrive-fw-v0.5.6/Firmware/fibre-cpp/configs/linux-armhf.config

@@ -0,0 +1,6 @@
+CONFIG_DEBUG=false
+CONFIG_STRICT=true
+CONFIG_CC="arm-linux-gnueabihf-g++"
+CONFIG_CFLAGS="-I$THIRD_PARTY./third_party/libusb-dev-armhf/usr/include/libusb-1.0"
+CONFIG_LDFLAGS="-L$THIRD_PARTY./third_party/libstdc++-linux-armhf/usr/lib/gcc-cross/arm-linux-gnueabihf/10 $THIRD_PARTY./third_party/libusb-armhf/usr/lib/arm-linux-gnueabihf/libusb-1.0.so.0"
+CONFIG_USE_PKGCONF=false

ODrive-fw-v0.5.6/Firmware/fibre-cpp/configs/macos-x86.config

@@ -0,0 +1,10 @@
+CONFIG_DEBUG=false
+CONFIG_STRICT=true
+CONFIG_CC="LD_LIBRARY_PATH=/opt/osxcross/lib MACOSX_DEPLOYMENT_TARGET=10.9 /opt/osxcross/bin/o64-clang++"
+CONFIG_CFLAGS="-I$THIRD_PARTY./third_party/libusb-1.0.23/libusb -arch i386 -arch x86_64"
+CONFIG_LDFLAGS="$THIRD_PARTY./third_party/libusb-1.0.23/build-macos-amd64/libusb/.libs/libusb-1.0.a -framework CoreFoundation -framework IOKit"
+# not supported yet
+CONFIG_ENABLE_TCP_SERVER_BACKEND=false
+# not supported yet
+CONFIG_ENABLE_TCP_CLIENT_BACKEND=false
+CONFIG_USE_PKGCONF=false

ODrive-fw-v0.5.6/Firmware/fibre-cpp/configs/wasm.config

@@ -0,0 +1,9 @@
+CONFIG_DEBUG=true
+CONFIG_STRICT=true
+CONFIG_CC=/usr/lib/emscripten/em++
+CONFIG_CFLAGS=-include emscripten.h -DFIBRE_PUBLIC=EMSCRIPTEN_KEEPALIVE -s RESERVED_FUNCTION_POINTERS=1
+CONFIG_LDFLAGS=-s EXPORT_ES6=1 -s MODULARIZE=1 -s USE_ES6_IMPORT_META=0 -s 'EXTRA_EXPORTED_RUNTIME_METHODS=[addFunction, stringToUTF8Array, UTF8ArrayToString, ENV]'
+CONFIG_USE_PKGCONF=false
+CONFIG_ENABLE_LIBUSB_BACKEND=false
+CONFIG_ENABLE_TCP_SERVER_BACKEND=false
+CONFIG_ENABLE_TCP_CLIENT_BACKEND=false

ODrive-fw-v0.5.6/Firmware/fibre-cpp/configs/windows-amd64.config

@@ -0,0 +1,10 @@
+CONFIG_DEBUG=false
+CONFIG_STRICT=true
+CONFIG_CC="x86_64-w64-mingw32-g++"
+CONFIG_CFLAGS="-I$THIRD_PARTY./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0"
+CONFIG_LDFLAGS="-static-libgcc $THIRD_PARTY./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a"
+# not supported yet
+CONFIG_ENABLE_TCP_SERVER_BACKEND=false
+# not supported yet
+CONFIG_ENABLE_TCP_CLIENT_BACKEND=false
+CONFIG_USE_PKGCONF=false

ODrive-fw-v0.5.6/Firmware/fibre-cpp/crc.hpp

@@ -0,0 +1,56 @@
+#ifndef __CRC_HPP
+#define __CRC_HPP
+
+#include <stdint.h>
+#include <limits.h>
+
+// Calculates an arbitrary CRC for one byte.
+// Adapted from https://barrgroup.com/Embedded-Systems/How-To/CRC-Calculation-C-Code
+template<typename T, unsigned POLYNOMIAL>
+static T calc_crc(T remainder, uint8_t value) {
+    constexpr T BIT_WIDTH = (CHAR_BIT * sizeof(T));
+    constexpr T TOPBIT = ((T)1 << (BIT_WIDTH - 1));
+    
+    // Bring the next byte into the remainder.
+    remainder ^= (value << (BIT_WIDTH - 8));
+
+    // Perform modulo-2 division, a bit at a time.
+    for (uint8_t bit = 8; bit; --bit) {
+        if (remainder & TOPBIT) {
+            remainder = (remainder << 1) ^ POLYNOMIAL;
+        } else {
+            remainder = (remainder << 1);
+        }
+    }
+
+    return remainder;
+}
+
+template<typename T, unsigned POLYNOMIAL>
+static T calc_crc(T remainder, const uint8_t* buffer, size_t length) {
+    while (length--)
+        remainder = calc_crc<T, POLYNOMIAL>(remainder, *(buffer++));
+    return remainder;
+}
+
+template<unsigned POLYNOMIAL>
+static uint8_t calc_crc8(uint8_t remainder, uint8_t value) {
+    return calc_crc<uint8_t, POLYNOMIAL>(remainder, value);
+}
+
+template<unsigned POLYNOMIAL>
+static uint16_t calc_crc16(uint16_t remainder, uint8_t value) {
+    return calc_crc<uint16_t, POLYNOMIAL>(remainder, value);
+}
+
+template<unsigned POLYNOMIAL>
+static uint8_t calc_crc8(uint8_t remainder, const uint8_t* buffer, size_t length) {
+    return calc_crc<uint8_t, POLYNOMIAL>(remainder, buffer, length);
+}
+
+template<unsigned POLYNOMIAL>
+static uint16_t calc_crc16(uint16_t remainder, const uint8_t* buffer, size_t length) {
+    return calc_crc<uint16_t, POLYNOMIAL>(remainder, buffer, length);
+}
+
+#endif /* __CRC_HPP */

ODrive-fw-v0.5.6/Firmware/fibre-cpp/endpoints_template.j2

@@ -0,0 +1,92 @@
+/*[# This is the original template, thus the warning below does not apply to this file #]
+ * ============================ WARNING ============================
+ * ==== This is an autogenerated file.                          ====
+ * ==== Any changes to this file will be lost when recompiling. ====
+ * =================================================================
+ *
+ * This file contains the toplevel handler for Fibre v0.1 endpoint operations.
+ *
+ * This endpoint-oriented approach will be deprecated in Fibre v0.2 in favor of
+ * a function-oriented approach and a more powerful object model.
+ *
+ */
+#ifndef __FIBRE_ENDPOINTS_HPP
+#define __FIBRE_ENDPOINTS_HPP
+
+#include <fibre/introspection.hpp>
+#include <fibre/../../legacy_protocol.hpp>
+#include <fibre/../../crc.hpp>
+
+// Note: with -Og the functions with large switch statements reserves a huge amount
+// of stack space because they reserves separate space for the stack frame of each
+// of the inlined functions.
+// The minimum known set of flags to prevent this is `-O1 -fipa-sra`.
+// `-O2`, `-O3` and `-Os` are supersets of this.
+
+#pragma GCC push_options
+#pragma GCC optimize ("s")
+
+namespace fibre {
+
+const unsigned char embedded_json[] = [[embedded_endpoint_definitions | to_c_string]];
+const size_t embedded_json_length = sizeof(embedded_json) - 1;
+const uint16_t json_crc_ = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(PROTOCOL_VERSION, embedded_json, embedded_json_length);
+const uint32_t json_version_id_ = (json_crc_ << 16) | calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(json_crc_, embedded_json, embedded_json_length);
+
+static void get_property(Introspectable& result, size_t idx) {
+    switch (idx) {
+[%- for endpoint in endpoints %]
+[%- if endpoint.function.name == 'exchange' and endpoint.in_bindings | list == ['obj'] %]
+        case [[endpoint.id]]: { [[(endpoint.in_bindings['obj'] + '$') | replace(')$', ', &result.storage_)')]]; result.type_info_ = &FibrePropertyTypeInfo<[[endpoint.function.in['obj'].type.c_name]]>::singleton; } break;
+[%- endif %]
+[%- endfor %]
+        default: break;
+    }
+}
+
+
+bool endpoint_handler(int idx, cbufptr_t* input_buffer, bufptr_t* output_buffer) {
+    //Introspectable property = get_property(idx);
+    //if property.is_valid()
+
+    switch (idx) {
+[%- for endpoint in endpoints %]
+[%- if (endpoint.function.name == 'exchange' or endpoint.function.name == 'read') and endpoint.in_bindings | list == ['obj'] %]
+        case [[endpoint.id]]: { return [[endpoint.function.fullname | to_snake_case]]([% for k, arg in endpoint.function.in.items() %][% if k in endpoint.in_bindings %]static_cast<[[arg.type.c_name]]>([[endpoint.in_bindings[k]]])[% else %]std::nullopt[% endif %], [% endfor %][% for k, arg in endpoint.function.out.items() %][% if k in endpoint.out_bindings %]static_cast<[[arg.type.c_name]]*>([[endpoint.out_bindings[k]]])[% else %]nullptr[% endif %], [% endfor %]input_buffer, output_buffer); } break;
+[%- else %]
+        case [[endpoint.id]]: { return [[endpoint.function.fullname | to_snake_case]]([% for k, arg in endpoint.function.in.items() %][% if k in endpoint.in_bindings %]static_cast<[[arg.type.c_name]]>([[endpoint.in_bindings[k]]])[% else %]std::nullopt[% endif %], [% endfor %][% for k, arg in endpoint.function.out.items() %][% if k in endpoint.out_bindings %]static_cast<[[arg.type.c_name]]*>([[endpoint.out_bindings[k]]])[% else %]nullptr[% endif %], [% endfor %]input_buffer, output_buffer); } break;
+[%- endif %]
+[%- endfor %]
+        default: return false;
+    }
+}
+
+bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref) {
+    if (endpoint_ref.json_crc != json_crc_) {
+        return false;
+    }
+
+    switch (endpoint_ref.endpoint_id) {
+[%- for endpoint in endpoints %]
+        case [[endpoint.id]]: return true;
+[%- endfor %]
+        default: return false;
+    }
+}
+
+bool set_endpoint_from_float(endpoint_ref_t endpoint_ref, float value) {
+    if (endpoint_ref.json_crc != json_crc_) {
+        return false;
+    }
+
+    Introspectable property{};
+    get_property(property, endpoint_ref.endpoint_id);
+    const FloatSettableTypeInfo* type_info = dynamic_cast<const FloatSettableTypeInfo*>(property.get_type_info());
+    return type_info && type_info->set_float(property, value);
+}
+
+}
+
+#pragma GCC pop_options
+
+#endif // __FIBRE_ENDPOINTS_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/fibre.cpp

@@ -0,0 +1,270 @@
+
+#include <fibre/fibre.hpp>
+#include "logging.hpp"
+#include <fibre/channel_discoverer.hpp>
+#include "legacy_protocol.hpp"
+#include "print_utils.hpp"
+#include <memory>
+#include <algorithm>
+#include <array>
+
+#if FIBRE_ALLOW_HEAP
+#include <unordered_map>
+#include <string>
+#endif
+
+DEFINE_LOG_TOPIC(FIBRE);
+USE_LOG_TOPIC(FIBRE);
+
+#if FIBRE_ENABLE_EVENT_LOOP
+#  ifdef __linux__
+#    include "platform_support/epoll_event_loop.hpp"
+using EventLoopImpl = fibre::EpollEventLoop;
+#  else
+#    error "No event loop implementation available for this operating system."
+#  endif
+#endif
+
+using namespace fibre;
+
+struct DiscoveryContext {
+};
+
+#if FIBRE_ALLOW_HEAP
+
+template<typename T>
+T* my_alloc() {
+    return new T{};
+}
+
+template<typename T>
+void my_free(T* ctx) {
+    delete ctx;
+}
+
+#else
+
+template<typename T>
+struct TheInstance {
+    static T instance;
+    static bool in_use;
+};
+
+template<typename T> T TheInstance<T>::instance{};
+template<typename T> bool TheInstance<T>::in_use = false;
+
+template<typename T>
+T* my_alloc() {
+    if (!TheInstance<T>::in_use) {
+        TheInstance<T>::in_use = true;
+        return &TheInstance<T>::instance;
+    } else {
+        return nullptr;
+    }
+}
+
+template<typename T>
+void my_free(T* ctx) {
+    if (ctx == &TheInstance<T>::instance) {
+        TheInstance<T>::in_use = false;
+    } else {
+        FIBRE_LOG(E) << "bad instance";
+    }
+}
+
+#endif
+
+bool fibre::launch_event_loop(Callback<void, EventLoop*> on_started) {
+#if FIBRE_ENABLE_EVENT_LOOP
+    EventLoopImpl* event_loop = my_alloc<EventLoopImpl>(); // TODO: free
+    return event_loop->start([&](){ on_started.invoke(event_loop); });
+#else
+    return false;
+#endif
+}
+
+struct BackendInitializer {
+    template<typename T>
+    bool operator()(T& backend) {
+        if (!backend.init(ctx->event_loop)) {
+            return false;
+        }
+        ctx->register_backend(backend.get_name(), &backend);
+        return true;
+    }
+    Context* ctx;
+};
+struct BackendDeinitializer {
+    template<typename T>
+    bool operator()(T& backend) {
+        ctx->deregister_backend(backend.get_name());
+        return backend.deinit();
+    }
+    Context* ctx;
+};
+
+template<typename ... T, size_t ... Is>
+bool all(std::tuple<T...> args, std::index_sequence<Is...>) {
+    std::array<bool, sizeof...(T)> arr = { std::get<Is>(args) ... };
+    return std::all_of(arr.begin(), arr.end(), [](bool val) { return val; });
+}
+
+template<typename ... T>
+bool all(std::tuple<T...> args) {
+    return all(args, std::make_index_sequence<sizeof...(T)>());
+}
+
+Context* fibre::open(EventLoop* event_loop) {
+    Context* ctx = my_alloc<Context>();
+    if (!ctx) {
+        FIBRE_LOG(E) << "already opened";
+        return nullptr;
+    }
+
+    ctx->event_loop = event_loop;
+    auto static_backends_good = for_each_in_tuple(BackendInitializer{ctx},
+            ctx->static_backends);
+
+    if (!all(static_backends_good)) {
+        // TODO: shutdown backends
+        FIBRE_LOG(E) << "some backends failed to initialize";
+        return nullptr;
+    }
+
+    return ctx;
+}
+
+void fibre::close(Context* ctx) {
+    if (ctx->n_domains) {
+        FIBRE_LOG(W) <<ctx->n_domains << " domains are still open";
+    }
+
+    for_each_in_tuple(BackendDeinitializer{ctx},
+            ctx->static_backends);
+
+    my_free<Context>(ctx);
+}
+
+#if FIBRE_ALLOW_HEAP
+Domain* Context::create_domain(std::string specs) {
+    FIBRE_LOG(D) << "creating domain with path \"" << specs << "\"";
+
+    Domain* domain = new Domain(); // deleted in close_domain
+    domain->ctx = this;
+
+    std::string::iterator prev_delim = specs.begin();
+    while (prev_delim < specs.end()) {
+        auto next_delim = std::find(prev_delim, specs.end(), ';');
+        auto colon = std::find(prev_delim, next_delim, ':');
+        auto colon_end = std::min(colon + 1, next_delim);
+
+        std::string name{prev_delim, colon};
+        auto it = discoverers.find(name);
+
+        if (it == discoverers.end()) {
+            FIBRE_LOG(W) << "transport layer \"" << name << "\" not implemented";
+        } else {
+            domain->channel_discovery_handles[name] = nullptr;
+            it->second->start_channel_discovery(domain,
+                    &*colon_end, next_delim - colon_end,
+                    &domain->channel_discovery_handles[name]);
+        }
+
+        prev_delim = std::min(next_delim + 1, specs.end());
+    }
+
+    n_domains++;
+    return domain;
+}
+
+void Context::close_domain(Domain* domain) {
+    for (auto& it: domain->channel_discovery_handles) {
+        discoverers[it.first]->stop_channel_discovery(it.second);
+    }
+    domain->channel_discovery_handles.clear();
+    delete domain;
+    n_domains--;
+}
+
+void Context::register_backend(std::string name, ChannelDiscoverer* backend) {
+    if (discoverers.find(name) != discoverers.end()) {
+        FIBRE_LOG(W) << "Discoverer " << name << " already registered";
+        return; // TODO: report status
+    }
+    
+    discoverers[name] = backend;
+}
+
+void Context::deregister_backend(std::string name) {
+    auto it = discoverers.find(name);
+    if (it == discoverers.end()) {
+        FIBRE_LOG(W) << "Discoverer " << name << " not registered";
+        return; // TODO: report status
+    }
+    
+    discoverers.erase(it);
+}
+#endif
+
+#if FIBRE_ENABLE_CLIENT
+void Domain::start_discovery(Callback<void, Object*, Interface*> on_found_object, Callback<void, Object*> on_lost_object) {
+    on_found_object_ = on_found_object;
+    on_lost_object_ = on_lost_object;
+    for (auto& it: root_objects_) {
+        on_found_object_.invoke(it.first, it.second);
+    }
+}
+
+void Domain::stop_discovery() {
+    auto on_lost_object = on_lost_object_;
+    on_found_object_ = nullptr;
+    on_lost_object_ = nullptr;
+    for (auto& it: root_objects_) {
+        on_lost_object.invoke(it.first);
+    }
+}
+#endif
+
+void Domain::add_channels(ChannelDiscoveryResult result) {
+    FIBRE_LOG(D) << "found channels!";
+
+    if (result.status != kFibreOk) {
+        FIBRE_LOG(W) << "discoverer stopped";
+        return;
+    }
+
+    if (!result.rx_channel || !result.tx_channel) {
+        FIBRE_LOG(W) << "unidirectional operation not supported yet";
+        return;
+    }
+
+#if FIBRE_ENABLE_CLIENT || FIBRE_ENABLE_SERVER
+    // Deleted during on_stopped()
+    auto protocol = new fibre::LegacyProtocolPacketBased(result.rx_channel, result.tx_channel, result.mtu);
+#if FIBRE_ENABLE_CLIENT
+    protocol->start(MEMBER_CB(this, on_found_root_object), MEMBER_CB(this, on_lost_root_object), MEMBER_CB(this, on_stopped));
+#else
+    protocol->start(MEMBER_CB(this, on_stopped));
+#endif
+#endif
+}
+
+#if FIBRE_ENABLE_CLIENT
+void Domain::on_found_root_object(LegacyObjectClient* obj_client, std::shared_ptr<LegacyObject> obj) {
+    Object* root_object = reinterpret_cast<Object*>(obj.get());
+    Interface* root_intf = reinterpret_cast<Interface*>(obj->intf.get());
+    root_objects_[root_object] = root_intf;
+    on_found_object_.invoke(root_object, root_intf);
+}
+
+void Domain::on_lost_root_object(LegacyObjectClient* obj_client, std::shared_ptr<LegacyObject> obj) {
+    Object* root_object = reinterpret_cast<Object*>(obj.get());
+    auto it = root_objects_.find(root_object);
+    root_objects_.erase(it);
+    on_lost_object_.invoke(root_object);
+}
+#endif
+
+void Domain::on_stopped(LegacyProtocolPacketBased* protocol, StreamStatus status) {
+    delete protocol;
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/function_stubs_template.j2

@@ -0,0 +1,40 @@
+/*[# This is the original template, thus the warning below does not apply to this file #]
+ * ============================ WARNING ============================
+ * ==== This is an autogenerated file.                          ====
+ * ==== Any changes to this file will be lost when recompiling. ====
+ * =================================================================
+ *
+ * This file contains serializing/deserializing stubs for the functions defined
+ * in your interface file.
+ *
+ */
+
+#include <fibre/bufptr.hpp>
+
+[% for intf in interfaces.values() %]
+[% for func in intf.functions.values() %]
+static inline bool [[func.fullname | to_snake_case]]([% for arg in func.in.values() %]std::optional<[[arg.type.c_name]]> in_[[arg.name]], [% endfor %][% for arg in func.out.values() %][[arg.type.c_name]]* out_[[arg.name]], [% endfor %]fibre::cbufptr_t* input_buffer, fibre::bufptr_t* output_buffer) {
+[%- if func.in %]
+    bool success = [% for arg in func.in.values() %](in_[[arg.name]].has_value() || (in_[[arg.name]] = fibre::Codec<[[arg.type.c_name]]>::decode(input_buffer)).has_value()[% if arg.optional %] || true[% endif %])[% if not loop.last %]
+                && [% endif %][% endfor %];
+[%- else %]
+    bool success = true;
+[%- endif %]
+    if (!success) {
+        return false;
+    }
+[%- if func.implementation %]
+    [% if func.out %]std::tuple<[% for arg in func.out.values() %][[arg.type.c_name]][[', ' if not loop.last]][% endfor %]> ret = [% endif %][[func.implementation]]([% for arg in func.in.values() %](*in_[[arg.name]][% if not arg.optional %])[% endif %][[', ' if not loop.last]][% endfor %]);
+[%- else %]
+    [% if func.out %]std::tuple<[% for arg in func.out.values() %][[arg.type.c_name]][[', ' if not loop.last]][% endfor %]> ret = [% endif %](*in_[[(func.in.values() | first).name]])->[[func.name]]([% for arg in func.in.values() | skip_first %][% if not arg.optional %]*[% endif %]in_[[arg.name]][[', ' if not loop.last]][% endfor %]);
+[%- endif %]
+[%- if func.out %]
+    return [% for arg in func.out.values() %]((out_[[arg.name]] && ((*out_[[arg.name]] = std::get<[[loop.index0]]>(ret)), true)) || fibre::Codec<[[arg.type.c_name]]>::encode(std::get<[[loop.index0]]>(ret), output_buffer))[% if not loop.last %]
+        && [% endif %][% endfor %];
+[%- else %]
+    return true;
+[%- endif %]
+}
+[% endfor %]
+[% endfor %]
+

ODrive-fw-v0.5.6/Firmware/fibre-cpp/get_dependencies.sh

@@ -0,0 +1,77 @@
+#!/bin/bash
+set -euo pipefail
+
+# Usage: download_deb_pkg destination-dir url
+function download_deb_pkg() {
+    mkdir -p third_party
+    dir="$1"
+    url="$2"
+    file="$(sed 's|^.*/\([^/]*\)$|\1|' <<< "$url")"
+
+    pushd third_party > /dev/null
+    if ! [ -f "${file}" ]; then
+        wget "${url}"
+    fi
+    if ! [ -d "${dir}/usr" ]; then
+        ar x "${file}" "data.tar.xz"
+        mkdir -p "${dir}"
+        tar -xvf "data.tar.xz" -C "${dir}"
+    fi
+    popd > /dev/null
+}
+
+# Usage: compile_libusb arch-name arch
+function compile_libusb() {
+    arch_name="$1"
+    arch="$2"
+    libusb_version=1.0.23
+
+    pushd third_party > /dev/null
+    if ! [ -f "libusb-${libusb_version}.tar.bz2" ]; then
+        wget "https://github.com/libusb/libusb/releases/download/v${libusb_version}/libusb-${libusb_version}.tar.bz2"
+    fi
+    if ! [ -d "libusb-${libusb_version}" ]; then
+        tar -xvf "libusb-${libusb_version}.tar.bz2"
+    fi
+
+    mkdir -p "libusb-${libusb_version}/build-${arch_name}"
+    pushd "libusb-${libusb_version}/build-${arch_name}" > /dev/null
+    unset LDFLAGS
+    if ! [ -f "libusb/.libs/libusb-1.0.a" ]; then
+        ../configure --host="$arch" \
+                    --enable-static \
+                    --prefix=/opt/osxcross/ \
+                    --disable-dependency-tracking
+        # They broke parallel building in libusb 1.20
+        make
+    fi
+    popd > /dev/null
+    popd > /dev/null
+}
+
+function patch_macos_sdk() {
+    # Link are broken:
+    # …ions/Current/Headers $ ls -l IOReturn.h 
+    # lrwxrwxrwx 1 root root 189 Dec 26  2019 IOReturn.h -> Users/phracker/Documents/Xcode-beta.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk/System/Library/Frameworks/Kernel.framework/Versions/A/Headers/IOKit/IOReturn.h
+    # Fix with:
+    # sudo ln -sf /opt/osxcross/SDK/MacOSX10.13.sdk/System/Library/Frameworks/Kernel.framework/Versions/A/Headers/IOKit/IOReturn.h IOReturn.h
+
+    oldprefix="Users/phracker/Documents/Xcode-beta.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk"
+    newprefix="/opt/osxcross/SDK/MacOSX10.13.sdk"
+    while IFS= read -r link; do
+        destination="$(readlink "$link")"
+        pruned_destination="${destination#"$oldprefix"}"
+        if [ "${oldprefix}${pruned_destination}" == "${destination}" ]; then
+            sudo mv -T "${newprefix}${pruned_destination}" "$link"
+        fi
+    done <<< "$(find /opt/osxcross/SDK/MacOSX10.13.sdk/System/Library/Frameworks/IOKit.framework -xtype l)"
+}
+
+cmd="$1"
+shift
+case "$cmd" in
+    download_deb_pkg) download_deb_pkg $@ ;;
+    compile_libusb) compile_libusb $@ ;;
+    patch_macos_sdk) patch_macos_sdk $@ ;;
+    *) echo "unknown command" && false ;;
+esac

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/async_stream.hpp

@@ -0,0 +1,145 @@
+#ifndef __FIBRE_ASYNC_STREAM_HPP
+#define __FIBRE_ASYNC_STREAM_HPP
+
+#include <fibre/bufptr.hpp>
+#include <fibre/callback.hpp>
+#include <stdint.h>
+
+namespace fibre {
+
+enum StreamStatus {
+    kStreamOk,
+    kStreamCancelled,
+    kStreamClosed,
+    kStreamError
+};
+
+
+struct ReadResult {
+    StreamStatus status;
+
+    /**
+     * @brief The pointer to one position after the last byte that was
+     * transferred.
+     * This must always be in [buffer.begin(), buffer.end()], even if the
+     * transfer was not succesful.
+     * If the status is kStreamError or kStreamCancelled then the accuracy
+     * of this field is not guaranteed.
+     */
+    unsigned char* end;
+};
+
+struct WriteResult {
+    StreamStatus status;
+
+    /**
+     * @brief The pointer to one position after the last byte that was
+     * transferred.
+     * This must always be in [buffer.begin(), buffer.end()], even if the
+     * transfer was not succesful.
+     * If the status is kStreamError or kStreamCancelled then the accuracy
+     * of this field is not guaranteed.
+     */
+    const unsigned char* end;
+};
+
+
+using TransferHandle = uintptr_t;
+
+/**
+ * @brief Base class for asynchronous stream sources.
+ */
+class AsyncStreamSource {
+public:
+    /**
+     * @brief Starts a read operation. Once the read operation completes,
+     * on_finished.complete() is called.
+     *
+     * Most implementations only allow one transfer to be active at a time.
+     *
+     * TODO: specify if `completer` can be called directly within this function.
+     *
+     * @param buffer: The buffer where the data to be written shall be fetched from.
+     *        Must remain valid until `completer` is satisfied.
+     * @param handle: The variable pointed to by this argument is set to an
+     *        opaque transfer handle that can be passed to cancel_read() as
+     *        long as the operation has not yet completed.
+     *        If the completer is invoked directly from start_read() then the
+     *        handle is not modified after this invokation. That means it's safe
+     *        for the completion handler to reuse the handle variable.
+     * @param completer: The completer that will be completed once the operation
+     *        finishes, whether successful or not.
+     *        Must remain valid until it is satisfied.
+     */
+    virtual void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) = 0;
+
+    /**
+     * @brief Cancels an operation that was previously started with start_read().
+     *
+     * The transfer is cancelled asynchronously and the associated completer
+     * will eventually be completed with kStreamCancelled. Until then the
+     * transfer must be considered still in progress and associated resources
+     * must not be freed.
+     * 
+     * TODO: specify if an implementation is allowed to return something other
+     * than kStreamCancelled when the transfer was cancelled.
+     *
+     * This function must not be called once the stream has started to invoke
+     * the associated completion handler. It must also not be called twice for
+     * the same transfer.
+     */
+    virtual void cancel_read(TransferHandle transfer_handle) = 0;
+};
+
+/**
+ * @brief Base class for asynchronous stream sources.
+ *
+ * Thread-safety: Implementations are generally not required to provide thread
+ * safety. Users should only call the functions of this class on the same thread
+ * as the event loop on which the stream runs.
+ */
+class AsyncStreamSink {
+public:
+    /**
+     * @brief Starts a write operation. Once the write operation completes,
+     * on_finished.complete() is called.
+     *
+     * Most implementations only allow one transfer to be active at a time.
+     *
+     * TODO: specify if `completer` can be called directly within this function.
+     *
+     * @param buffer: The buffer where the data to be written shall be fetched from.
+     *        Must remain valid until `completer` is satisfied.
+     * @param handle: The variable pointed to by this argument is set to an
+     *        opaque transfer handle that can be passed to cancel_write() as
+     *        long as the operation has not yet completed.
+     *        If the completer is invoked directly from start_write() then the
+     *        handle is not modified after this invokation. That means it's safe
+     *        for the completion handler to reuse the handle variable.
+     * @param completer: The completer that will be completed once the operation
+     *        finishes, whether successful or not.
+     *        Must remain valid until it is satisfied.
+     */
+    virtual void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) = 0;
+
+    /**
+     * @brief Cancels an operation that was previously started with start_write().
+     *
+     * The transfer is cancelled asynchronously and the associated completer
+     * will eventually be completed with kStreamCancelled. Until then the
+     * transfer must be considered still in progress and associated resources
+     * must not be freed.
+     * 
+     * TODO: specify if an implementation is allowed to return something other
+     * than kStreamCancelled when the transfer was cancelled.
+     *
+     * This function must not be called once the stream has started to invoke
+     * the associated completion handler. It must also not be called twice for
+     * the same transfer.
+     */
+    virtual void cancel_write(TransferHandle transfer_handle) = 0;
+};
+
+}
+
+#endif // __FIBRE_ASYNC_STREAM_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/bufptr.hpp

@@ -0,0 +1,100 @@
+#ifndef __FIBRE_BUFPTR_HPP
+#define __FIBRE_BUFPTR_HPP
+
+#include <stdlib.h>
+#include <vector>
+
+namespace fibre {
+
+static inline bool soft_assert(bool expr) { return expr; } // TODO: implement
+
+/**
+ * @brief Holds a reference to a buffer and a length.
+ * Since this class implements begin() and end(), you can use it with many
+ * standard algorithms that operate on iterable objects.
+ */
+template<typename T>
+struct generic_bufptr_t {
+    using iterator = T*;
+    using const_iterator = const T*;
+
+    generic_bufptr_t(T* begin, size_t length) : begin_(begin), end_(begin + length) {}
+
+    generic_bufptr_t(T* begin, T* end) : begin_(begin), end_(end) {}
+
+    generic_bufptr_t() : begin_(nullptr), end_(nullptr) {}
+
+    template<size_t I>
+    generic_bufptr_t(T (&begin)[I]) : generic_bufptr_t(begin, I) {}
+
+    generic_bufptr_t(std::vector<typename std::remove_const<T>::type>& vector)
+        : generic_bufptr_t(vector.data(), vector.size()) {}
+
+    generic_bufptr_t(const std::vector<typename std::remove_const<T>::type>& vector)
+        : generic_bufptr_t(vector.data(), vector.size()) {}
+
+    generic_bufptr_t(const generic_bufptr_t<typename std::remove_const<T>::type>& other)
+        : generic_bufptr_t(other.begin(), other.end()) {}
+
+    generic_bufptr_t& operator+=(size_t num) {
+        if (!soft_assert(num <= size())) {
+            num = size();
+        }
+        begin_ += num;
+        return *this;
+    }
+
+    generic_bufptr_t operator++(int) {
+        generic_bufptr_t result = *this;
+        *this += 1;
+        return result;
+    }
+
+    T& operator*() {
+        return *begin_;
+    }
+
+    generic_bufptr_t take(size_t num) const {
+        if (!soft_assert(num <= size())) {
+            num = size();
+        }
+        generic_bufptr_t result = {begin_, num};
+        return result;
+    }
+
+    generic_bufptr_t skip(size_t num, size_t* processed_bytes = nullptr) const {
+        if (!soft_assert(num <= size())) {
+            num = size();
+        }
+        if (processed_bytes)
+            (*processed_bytes) += num;
+        return {begin_ + num, end_};
+    }
+
+    size_t size() const {
+        return end_ - begin_;
+    }
+
+    bool empty() const {
+        return size() == 0;
+    }
+
+    T*& begin() { return begin_; }
+    T*& end() { return end_; }
+    T* const & begin() const { return begin_; }
+    T* const & end() const { return end_; }
+    T& front() const { return *begin(); }
+    T& back() const { return *(end() - 1); }
+    T& operator[](size_t idx) { return *(begin() + idx); }
+
+private:
+    T* begin_;
+    T* end_;
+};
+
+using cbufptr_t = generic_bufptr_t<const unsigned char>;
+using bufptr_t = generic_bufptr_t<unsigned char>;
+
+}
+
+#endif // __FIBRE_BUFPTR_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/callback.hpp

@@ -0,0 +1,126 @@
+#ifndef __CALLBACK_HPP
+#define __CALLBACK_HPP
+
+#include <stdlib.h>
+#include <typeinfo>
+#include <tuple>
+#include <functional>
+#include <type_traits>
+
+namespace fibre {
+
+namespace detail {
+template<typename T> struct get_default { static T val() { return {}; } };
+template<> struct get_default<void> { static void val() {} };
+}
+
+template<typename TRet, typename ... TArgs>
+class Callback {
+public:
+    Callback() : cb_(nullptr), ctx_(nullptr) {}
+    Callback(std::nullptr_t) : cb_(nullptr), ctx_(nullptr) {}
+    Callback(TRet(*callback)(void*, TArgs...), void* ctx) :
+        cb_(callback), ctx_(ctx) {}
+
+    /**
+     * @brief Creates a copy of another Callback instance.
+     * 
+     * This is only works it the other callback has identical template parameters.
+     * This constructor is templated so that construction from an incompatible
+     * callback gives a useful error message.
+     */
+    //template<typename TRetOther, typename ... TArgsOther>
+    //Callback(const Callback<TRetOther, TArgsOther...>& other) : cb_(other.cb_), ctx_(other.ctx_) {
+    //    static_assert(std::is_same<Callback<TRetOther, TArgsOther...>, Callback>::value, "incompatible callback type");
+    //}
+
+    Callback(const Callback& other) : cb_(other.cb_), ctx_(other.ctx_) {}
+
+    // If you get a compile error "[...] invokes a deleted function" that points
+    // here then you're probably trying to assign a Callback with incompatible
+    // template arguments to another Callback.
+    template<typename TRetOther, typename ... TArgsOther>
+    Callback(const Callback<TRetOther, TArgsOther...>& other) = delete;
+
+    /**
+     * @brief Constructs a callback object from a functor. The functor must
+     * remain allocated throughout the lifetime of the Callback.
+     */
+    template<typename TFunc>
+    Callback(const TFunc& func) :
+        cb_([](void* ctx, TArgs...args){
+            return (*(const TFunc*)ctx)(args...);
+        }), ctx_((void*)&func) {}
+
+    operator bool() {
+        return cb_;
+    }
+    
+    TRet invoke(TArgs ... arg) const {
+        if (cb_) {
+            return (*cb_)(ctx_, arg...);
+        }
+        return detail::get_default<TRet>::val();
+    }
+
+    TRet invoke_and_clear(TArgs ... arg) {
+        void* ctx = ctx_;
+        auto cb = cb_;
+        ctx_ = nullptr;
+        cb_ = nullptr;
+        if (cb) {
+            return (*cb)(ctx, arg...);
+        }
+        return detail::get_default<TRet>::val();
+    }
+
+    typedef TRet(*cb_t)(void*, TArgs...);
+    cb_t get_ptr() { return cb_; }
+    void* get_ctx() { return ctx_; }
+
+private:
+    TRet(*cb_)(void*, TArgs...);
+    void* ctx_;
+};
+
+template<typename _TRet, typename _TObj, typename ... _TArgs>
+struct function_traits {
+    using TRet = _TRet;
+    using TArgs = std::tuple<_TArgs...>;
+    using TObj = _TObj;
+};
+
+template<typename _TRet, typename _TObj, typename ... _TArgs>
+function_traits<_TRet, _TObj, _TArgs...> make_function_traits(_TRet (_TObj::*)(_TArgs...)) {
+    return {};
+}
+
+template<typename T1, T1 T2, typename T3, typename T4, typename T5>
+struct MemberCallback;
+
+template<typename T, T func, typename TObj, typename TRes, typename ... TArgs>
+struct MemberCallback<T, func, TObj, TRes, std::tuple<TArgs...>> {
+    using cb_t = Callback<TRes, TArgs...>;
+    static cb_t with(TObj* obj) {
+        return cb_t{[](void* obj, TArgs... arg) {
+            return (((TObj*)obj)->*func)(arg...);
+        }, obj};
+    }
+};
+
+template<typename T, T func,
+         typename TTraits = decltype(make_function_traits(func)),
+         typename MemCb = MemberCallback<T, func, typename TTraits::TObj, typename TTraits::TRet, typename TTraits::TArgs>>
+typename MemCb::cb_t make_callback(typename TTraits::TObj* obj) {
+    return MemCb::with(obj);
+}
+
+#define MEMBER_CB(obj, func) \
+    fibre::make_callback< \
+        decltype(&std::remove_reference_t<decltype(*obj)>::func), \
+        &std::remove_reference_t<decltype(*obj)>::func \
+    >(obj)
+
+}
+
+#endif // __CALLBACK_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/channel_discoverer.hpp

@@ -0,0 +1,38 @@
+#ifndef __FIBRE_CHANNEL_DISCOVERER
+#define __FIBRE_CHANNEL_DISCOVERER
+
+#include "async_stream.hpp"
+#include <fibre/callback.hpp>
+#include <fibre/status.hpp>
+
+namespace fibre {
+
+struct ChannelDiscoveryResult {
+    Status status;
+    AsyncStreamSource* rx_channel;
+    AsyncStreamSink* tx_channel;
+    size_t mtu;
+};
+
+struct ChannelDiscoveryContext {};
+
+class Domain; // defined in fibre.hpp
+
+class ChannelDiscoverer {
+public:
+    // TODO: maybe we should remove "handle" because a discovery can also be
+    // uniquely identified by domain.
+    virtual void start_channel_discovery(
+        Domain* domain,
+        const char* specs, size_t specs_len,
+        ChannelDiscoveryContext** handle) = 0;
+    virtual int stop_channel_discovery(ChannelDiscoveryContext* handle) = 0;
+
+protected:
+    bool try_parse_key(const char* begin, const char* end, const char* key, const char** val_begin, const char** val_end);
+    bool try_parse_key(const char* begin, const char* end, const char* key, int* val);
+};
+
+}
+
+#endif // __FIBRE_CHANNEL_DISCOVERER

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/event_loop.hpp

@@ -0,0 +1,74 @@
+#ifndef __FIBRE_EVENT_LOOP_HPP
+#define __FIBRE_EVENT_LOOP_HPP
+
+#include "callback.hpp"
+#include <stdint.h>
+
+namespace fibre {
+
+struct EventLoopTimer;
+
+/**
+ * @brief Base class for event loops.
+ * 
+ * Thread-safety: The public functions of this class except for post() must not
+ * be assumed to be thread-safe.
+ * Generally the functions of an event loop are only safe to be called from the
+ * event loop's thread itself.
+ */
+class EventLoop {
+public:
+    /**
+     * @brief Registers a callback for immediate execution on the event loop
+     * thread.
+     * 
+     * This function must be thread-safe.
+     */
+    virtual bool post(Callback<void> callback) = 0;
+
+    /**
+     * @brief Registers the given file descriptor on this event loop.
+     * 
+     * This function is only implemented on Unix-like systems.
+     * 
+     * @param fd: A waitable Unix file descriptor on which to listen for events.
+     * @param events: A bitfield that specifies the events to listen for.
+     *        For instance EPOLLIN or EPOLLOUT.
+     * @param callback: The callback to invoke every time the event triggers.
+     *        A bitfield is passed to the callback to indicate which events were
+     *        triggered. This callback must remain valid until
+     *        deregister_event() is called for the same file descriptor.
+     */
+    virtual bool register_event(int fd, uint32_t events, Callback<void, uint32_t> callback) = 0;
+
+    /**
+     * @brief Deregisters the given event.
+     * 
+     * Once this function returns, the associated callback will no longer be
+     * invoked and its resources can be freed.
+     */
+    virtual bool deregister_event(int fd) = 0;
+
+    /**
+     * @brief Registers a callback to be called at a later point in time.
+     * 
+     * This returns an opaque handler which can be used to cancel the timer.
+     * 
+     * @param delay: The delay from now in seconds.
+     *               TOOD: specify if OS sleep time is counted in.
+     */
+    virtual struct EventLoopTimer* call_later(float delay, Callback<void> callback) = 0;
+
+    /**
+     * @brief Cancels a timer which was previously started by call_later().
+     * 
+     * Must not be called after invokation of the callback has started.
+     * This also means that cancel_timer() must not be called from within the
+     * callback of the timer itself.
+     */
+    virtual bool cancel_timer(EventLoopTimer* timer) = 0;
+};
+
+}
+
+#endif // __FIBRE_EVENT_LOOP_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/fibre.hpp

@@ -0,0 +1,155 @@
+#ifndef __FIBRE_HPP
+#define __FIBRE_HPP
+
+#include <fibre/callback.hpp>
+#include <fibre/bufptr.hpp>
+#include <fibre/cpp_utils.hpp>
+#include <fibre/event_loop.hpp>
+#include <fibre/channel_discoverer.hpp>
+#include <string>
+#include <memory>
+
+#if FIBRE_ENABLE_LIBUSB_BACKEND
+#include "../../platform_support/libusb_transport.hpp"
+#endif
+
+#if FIBRE_ENABLE_TCP_CLIENT_BACKEND
+#include "../../platform_support/posix_tcp_backend.hpp"
+#endif
+
+namespace fibre {
+
+struct CallBuffers {
+    Status status;
+    cbufptr_t tx_buf;
+    bufptr_t rx_buf;
+};
+
+struct CallBufferRelease {
+    Status status;
+    const uint8_t* tx_end;
+    uint8_t* rx_end;
+};
+
+struct Function {
+    virtual std::optional<CallBufferRelease>
+    call(void**, CallBuffers, Callback<std::optional<CallBuffers>, CallBufferRelease>) = 0;
+};
+
+struct Object;
+struct Interface;
+class Domain;
+
+template<typename T>
+struct StaticBackend {
+    std::string name;
+    T impl;
+};
+
+struct Context {
+    size_t n_domains = 0;
+    EventLoop* event_loop;
+
+    std::tuple<
+#if FIBRE_ENABLE_LIBUSB_BACKEND
+        LibusbDiscoverer
+#endif
+#if FIBRE_ENABLE_LIBUSB_BACKEND && FIBRE_ENABLE_TCP_CLIENT_BACKEND
+        , // TODO: find a less awkward way to do this
+#endif
+#if FIBRE_ENABLE_TCP_CLIENT_BACKEND
+        PosixTcpClientBackend
+#endif
+#if FIBRE_ENABLE_TCP_CLIENT_BACKEND && FIBRE_ENABLE_TCP_SERVER_BACKEND
+        , // TODO: find a less awkward way to do this
+#endif
+#if FIBRE_ENABLE_TCP_SERVER_BACKEND
+        PosixTcpServerBackend
+#endif
+    > static_backends;
+
+#if FIBRE_ALLOW_HEAP
+    std::unordered_map<std::string, ChannelDiscoverer*> discoverers;
+#endif
+
+    /**
+     * @brief Creates a domain on which objects can subsequently be published
+     * and discovered.
+     * 
+     * This potentially starts looking for channels on this domain.
+     */
+    Domain* create_domain(std::string specs);
+    void close_domain(Domain* domain);
+
+    void register_backend(std::string name, ChannelDiscoverer* backend);
+    void deregister_backend(std::string name);
+};
+
+// TODO: don't declare these types here
+struct LegacyProtocolPacketBased;
+class LegacyObjectClient;
+struct LegacyObject;
+
+class Domain {
+    friend struct Context;
+public:
+#if FIBRE_ENABLE_CLIENT
+    // TODO: add interface argument
+    // TODO: support multiple discovery instances
+    void start_discovery(Callback<void, Object*, Interface*> on_found_object, Callback<void, Object*> on_lost_object);
+    void stop_discovery();
+#endif
+
+    void add_channels(ChannelDiscoveryResult result);
+
+    Context* ctx;
+private:
+#if FIBRE_ENABLE_CLIENT
+    void on_found_root_object(LegacyObjectClient* obj_client, std::shared_ptr<LegacyObject> obj);
+    void on_lost_root_object(LegacyObjectClient* obj_client, std::shared_ptr<LegacyObject> obj);
+#endif
+    void on_stopped(LegacyProtocolPacketBased* protocol, StreamStatus status);
+
+#if FIBRE_ALLOW_HEAP
+    std::unordered_map<std::string, fibre::ChannelDiscoveryContext*> channel_discovery_handles;
+#endif
+#if FIBRE_ENABLE_CLIENT
+    Callback<void, Object*, Interface*> on_found_object_;
+    Callback<void, Object*> on_lost_object_;
+    std::unordered_map<Object*, Interface*> root_objects_;
+#endif
+};
+
+/**
+ * @brief Opens and initializes a Fibre context.
+ * 
+ * If FIBRE_ALLOW_HEAP=0 only one Fibre context can be open at a time.
+ * 
+ * @returns: A non-null pointer on success, null otherwise.
+ */
+Context* open(EventLoop* event_loop);
+
+void close(Context*);
+
+/**
+ * @brief Launches an event loop on the current thread.
+ * 
+ * This function returns when the event loop becomes empty.
+ * 
+ * If FIBRE_ALLOW_HEAP=0 only one event loop can be running at a time.
+ * 
+ * This function returns false if Fibre was compiled with
+ * FIBRE_ENABLE_EVENT_LOOP=0.
+ * 
+ * @param on_started: This function is the first event that is placed on the
+ *        event loop. This function usually creates further events, for instance
+ *        by calling open().
+ * @returns: true if the event loop ran to completion. False if this function is
+ *           not implemented on this operating system or if another error
+ *           occurred.
+ */
+bool launch_event_loop(Callback<void, EventLoop*> on_started);
+
+}
+
+#endif // __FIBRE_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/introspection.hpp

@@ -0,0 +1,219 @@
+#ifndef __FIBRE_INTROSPECTION_HPP
+#define __FIBRE_INTROSPECTION_HPP
+
+#include <stdlib.h>
+#include <algorithm>
+#include <cstring>
+
+#pragma GCC push_options
+#pragma GCC optimize ("s")
+
+class TypeInfo;
+class Introspectable;
+using introspectable_storage_t = std::aligned_storage<4 * sizeof(uintptr_t), sizeof(uintptr_t)>::type;
+
+struct PropertyInfo {
+    const char * name;
+    const TypeInfo* type_info;
+};
+
+/**
+ * @brief Contains runtime accessible type information.
+ * 
+ * Specifically, this information consists of a list of PropertyInfo items which
+ * enable accessing attributes of an object by a runtime string.
+ * 
+ * Typically, for each combination of C++ type and Fibre interface implemented
+ * by this type, one (static constant) TypeInfo object will exist.
+ */
+class TypeInfo {
+    friend class Introspectable;
+public:
+    TypeInfo(const PropertyInfo* property_table, size_t property_table_length)
+        : property_table_(property_table), property_table_length_(property_table_length) {}
+
+    virtual introspectable_storage_t get_child(introspectable_storage_t obj, size_t idx) const = 0;
+    Introspectable get_child(const Introspectable& obj, const char * name, size_t length) const;
+
+protected:
+
+    template<typename T> static T& as(Introspectable& obj);
+    template<typename T> static const T& as(const Introspectable& obj);
+    template<typename T> static Introspectable make_introspectable(T obj, const TypeInfo* type_info);
+
+private:
+    const PropertyInfo* property_table_;
+    size_t property_table_length_;
+};
+
+/**
+ * @brief Wraps a reference to an application object by attaching runtime
+ * accessible type information.
+ * 
+ * The reference that is wrapped is typically a pointer but can also be a small
+ * temporary, on-demand constructed object such as a fibre::Property<...> which
+ * contains multiple pointers.
+ */
+class Introspectable {
+    friend class TypeInfo;
+public:
+    Introspectable() {}
+
+    /**
+     * @brief Returns an Introspectable object for the attribute referenced by
+     * the specified attribute name.
+     * 
+     * The name can consist of multiple parts separated by dots.
+     * 
+     * If the attribute does not exist, an invalid Introspectable is returned.
+     * 
+     * @param path: The name or path of the attribute.
+     * @param length: The maximum length of the name.
+     */
+    Introspectable get_child(const char * path, size_t length) {
+        Introspectable current = *this;
+
+        const char * begin = path;
+        const char * end = std::find(begin, path + length, '\0');
+
+        while ((begin < end) && current.type_info_) {
+            const char * end_of_token = std::find(begin, end, '.');
+            current = current.get_direct_child(begin, end_of_token - begin);
+            begin = std::min(end, end_of_token + 1);
+        }
+
+        return current;
+    };
+
+    bool is_valid() {
+        return type_info_;
+    }
+
+    const TypeInfo* get_type_info() {
+        return type_info_;
+    }
+
+private:
+    Introspectable get_direct_child(const char * name, size_t length) const {
+        for (size_t i = 0; i < type_info_->property_table_length_; ++i) {
+            if (!strncmp(name, type_info_->property_table_[i].name, length) && (length == strlen(type_info_->property_table_[i].name))) {
+                Introspectable result;
+                result.storage_ = type_info_->get_child(storage_, i);
+                result.type_info_ = type_info_->property_table_[i].type_info;
+                return result;
+            }
+        }
+        return {};
+    }
+
+public: // these should technically be protected but are public for optimization reasons
+    // We use this storage to hold generic small objects. Usually that's a pointer
+    // but sometimes it's an on-demand constructed Property<...>.
+    // Caution: only put objects in here which are trivially copyable, movable
+    // and destructible as any custom operation wouldn't be called.
+    introspectable_storage_t storage_;
+    const TypeInfo* type_info_ = nullptr;
+};
+
+template<typename T> T& TypeInfo::as(Introspectable& obj) {
+    static_assert(sizeof(T) <= sizeof(obj.storage_), "invalid size");
+    return *(T*)&obj.storage_;
+}
+template<typename T> const T& TypeInfo::as(const Introspectable& obj) {
+    static_assert(sizeof(T) <= sizeof(obj.storage_), "invalid size");
+    return *(const T*)&obj.storage_;
+}
+template<typename T> Introspectable TypeInfo::make_introspectable(T obj, const TypeInfo* type_info) {
+    Introspectable introspectable;
+    as<T>(introspectable) = obj;
+    introspectable.type_info_ = type_info;
+    return introspectable;
+}
+
+
+// maybe_underlying_type_t<T> resolves to the underlying type of T if T is an enum type or otherwise to T itself.
+template<typename T, bool = std::is_enum<T>::value> struct maybe_underlying_type;
+template<typename T> struct maybe_underlying_type<T, true> { typedef std::underlying_type_t<T> type; };
+template<typename T> struct maybe_underlying_type<T, false> { typedef T type; };
+template<typename T> using maybe_underlying_type_t = typename maybe_underlying_type<T>::type;
+
+
+struct StringConvertibleTypeInfo {
+    virtual bool get_string(const Introspectable& obj, char* buffer, size_t length) const { return false; }
+    virtual bool set_string(const Introspectable& obj, char* buffer, size_t length) const { return false; }
+};
+
+struct FloatSettableTypeInfo {
+    //virtual bool get_float(const Introspectable& obj, float* val) const { return false; }
+    virtual bool set_float(const Introspectable& obj, float val) const { return false; }
+};
+
+/* Built-in type infos ********************************************************/
+
+template<typename T>
+struct FibrePropertyTypeInfo;
+
+// readonly property
+template<typename T>
+struct FibrePropertyTypeInfo<Property<const T>> : StringConvertibleTypeInfo, TypeInfo {
+    using TypeInfo::TypeInfo;
+    static const PropertyInfo property_table[];
+    static const FibrePropertyTypeInfo<Property<const T>> singleton;
+
+    introspectable_storage_t get_child(introspectable_storage_t obj, size_t idx) const override {
+        return {};
+    }
+
+    bool get_string(const Introspectable& obj, char* buffer, size_t length) const override {
+        return to_string(static_cast<maybe_underlying_type_t<T>>(as<const Property<const T>>(obj).read()), buffer, length, 0);
+    }
+};
+
+template<typename T>
+const PropertyInfo FibrePropertyTypeInfo<Property<const T>>::property_table[] = {};
+template<typename T>
+const FibrePropertyTypeInfo<Property<const T>> FibrePropertyTypeInfo<Property<const T>>::singleton{FibrePropertyTypeInfo<Property<const T>>::property_table, sizeof(FibrePropertyTypeInfo<Property<const T>>::property_table) / sizeof(FibrePropertyTypeInfo<Property<const T>>::property_table[0])};
+
+// readwrite property
+template<typename T>
+struct FibrePropertyTypeInfo<Property<T>> : FloatSettableTypeInfo, StringConvertibleTypeInfo, TypeInfo {
+    using TypeInfo::TypeInfo;
+    static const PropertyInfo property_table[];
+    static const FibrePropertyTypeInfo<Property<T>> singleton;
+    static const Introspectable make_introspectable(Property<T> obj) { return TypeInfo::make_introspectable(obj, &singleton); }
+
+    introspectable_storage_t get_child(introspectable_storage_t obj, size_t idx) const override {
+        return {};
+    }
+
+    bool get_string(const Introspectable& obj, char* buffer, size_t length) const override {
+        return to_string(static_cast<maybe_underlying_type_t<T>>(as<const Property<T>>(obj).read()), buffer, length, 0);
+    }
+
+    bool set_string(const Introspectable& obj, char* buffer, size_t length) const override {
+        maybe_underlying_type_t<T> value{};
+        if (!from_string(buffer, length, &value, 0)) {
+            return false;
+        }
+        as<const Property<T>>(obj).exchange(static_cast<T>(value));
+        return true;
+    }
+
+    bool set_float(const Introspectable& obj, float val) const override {
+        maybe_underlying_type_t<T> value{};
+        if (!conversion::set_from_float(val, &value)) {
+            return false;
+        }
+        as<const Property<T>>(obj).exchange(static_cast<T>(value));
+        return true;
+    }
+};
+
+template<typename T>
+const PropertyInfo FibrePropertyTypeInfo<Property<T>>::property_table[] = {};
+template<typename T>
+const FibrePropertyTypeInfo<Property<T>> FibrePropertyTypeInfo<Property<T>>::singleton{FibrePropertyTypeInfo<Property<T>>::property_table, sizeof(FibrePropertyTypeInfo<Property<T>>::property_table) / sizeof(FibrePropertyTypeInfo<Property<T>>::property_table[0])};
+
+#pragma GCC pop_options
+
+#endif // __FIBRE_INTROSPECTION_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/libfibre.h

@@ -0,0 +1,603 @@
+/**
+ * @brief Fibre C library
+ * 
+ * The library is fully asynchronous and runs on an application-managed event
+ * loop. This integration happens with the call to libfibre_open(), where the
+ * application must pass a couple of functions that libfibre will use to put
+ * tasks on the event loop.
+ * 
+ * Some general things to note:
+ *  - None of the library's functions are blocking.
+ *  - None of the library's functions can be expected to be thread-safe, they
+ *    should not be invoked from any other thread than the one that runs the
+ *    event loop.
+ *  - Callbacks that the user passes to a libfibre function are always executed
+ *    on the event loop thread.
+ *  - All of the library's functions can be expected reentry-safe. That means
+ *    you can call into any libfibre function from any callback handler that
+ *    libfibre invokes.
+ */
+
+#ifndef __LIBFIBRE_H
+#define __LIBFIBRE_H
+
+#include <stdint.h>
+#include <stdlib.h>
+
+#if defined(_MSC_VER)
+#   define DLL_EXPORT __declspec(dllexport)
+#   define DLL_IMPORT __declspec(dllimport)
+#elif defined(__GNUC__)
+#   define DLL_EXPORT __attribute__((visibility("default")))
+#   define DLL_IMPORT
+#   if __GNUC__ > 4
+#       define DLL_LOCAL __attribute__((visibility("hidden")))
+#   else
+#       define DLL_LOCAL
+#   endif
+#else
+#   error("Don't know how to export shared object libraries")
+#endif
+
+#ifdef FIBRE_COMPILE
+#   ifndef FIBRE_PUBLIC
+#       define FIBRE_PUBLIC DLL_EXPORT
+#   endif
+#   define FIBRE_PRIVATE DLL_LOCAL
+#else
+#   define FIBRE_PUBLIC DLL_IMPORT
+#endif
+
+
+#define FIBRE_PRIVATE DLL_LOCAL
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct LibFibreCtx;
+struct LibFibreDiscoveryCtx;
+struct LibFibreCallContext;
+struct LibFibreObject;
+struct LibFibreInterface;
+struct LibFibreFunction;
+struct LibFibreAttribute;
+struct LibFibreTxStream;
+struct LibFibreRxStream;
+struct LibFibreDomain;
+
+// This enum must remain identical to fibre::Status.
+enum LibFibreStatus {
+    kFibreOk,
+    kFibreBusy, //<! The request will complete asynchronously
+    kFibreCancelled, //!< The operation was cancelled due to a request by the application or the remote peer
+    kFibreClosed, //!< The operation has finished orderly or shall be finished orderly
+    kFibreInvalidArgument, //!< Bug in the application
+    kFibreInternalError, //!< Bug in the local fibre implementation
+    kFibreProtocolError, //!< A remote peer is misbehaving (indicates bug in the remote peer)
+    kFibreHostUnreachable, //!< The remote peer can no longer be reached
+    //kFibreInsufficientData, // maybe we will introduce this to tell the caller that the granularity of the data is too small
+};
+
+struct LibFibreVersion {
+    uint16_t major;
+    uint16_t minor;
+    uint16_t patch;
+};
+
+typedef int (*post_cb_t)(void (*callback)(void*), void* cb_ctx);
+typedef int (*register_event_cb_t)(int fd, uint32_t events, void (*callback)(void*, uint32_t), void* cb_ctx);
+typedef int (*deregister_event_cb_t)(int fd);
+typedef struct EventLoopTimer* (*call_later_cb_t)(float delay, void (*callback)(void*), void* cb_ctx);
+typedef int (*cancel_timer_cb_t)(struct EventLoopTimer* timer);
+
+struct LibFibreEventLoop {
+    /**
+     * @brief Called by libfibre when it wants the application to run a callback
+     * on the application's event loop.
+     * 
+     * This is the only callback that libfibre can invoke from a different
+     * thread than the event loop thread itself. The application must ensure
+     * that this callback is thread-safe.
+     * This allows libfibre to run other threads internally while keeping
+     * threading promises made to the application.
+     */
+    post_cb_t post;
+
+    /**
+     * @brief TODO: this is a Unix specific callback. Need to use IOCP on Windows.
+     */
+    register_event_cb_t register_event;
+
+    /**
+     * @brief TODO: this is a Unix specific callback. Need to use IOCP on Windows.
+     */
+    deregister_event_cb_t deregister_event;
+
+    /**
+     * @brief Called by libfibre to ask the application to call a certain
+     * callback after a certain amount of time.
+     * 
+     * The callback must be invoked on the same thread on which libfibre_open()
+     * was called. The application should return an opaque handle that
+     * libfibre can use to cancel the timer.
+     */
+    call_later_cb_t call_later;
+
+    /**
+     * @brief Called by libfibre to ask the application to cancel a callback
+     * timer previously enqueued with call_later().
+     */
+    cancel_timer_cb_t cancel_timer;
+};
+
+/**
+ * @brief on_start_discovery callback type for libfibre_register_backend().
+ * 
+ * For every channel pair that the application finds that matches the filter of
+ * this discoverer the application should call libfibre_add_channels().
+ * 
+ * @param discovery_handle: An opaque handle that libfibre will pass to the
+ *        corresponding on_stop_discovery callback to stop the discovery.
+ * @param specs, specs_length: The specs string that specifies discoverer-specific
+ *        filter parameters.
+ */
+typedef void (*on_start_discovery_cb_t)(void* ctx, LibFibreDomain* domain, const char* specs, size_t specs_length);
+typedef void (*on_stop_discovery_cb_t)(void* ctx, LibFibreDomain* domain);
+
+/**
+ * @brief on_found_object callback type for libfibre_start_discovery().
+ * @param obj: The object handle.
+ * @param intf: The interface handle. Valid for as long as any handle of an
+ *        object that implements it is valid.
+ */
+typedef void (*on_found_object_cb_t)(void*, LibFibreObject* obj, LibFibreInterface* intf);
+
+/**
+ * @brief on_lost_object callback type for libfibre_start_discovery().
+ */
+typedef void (*on_lost_object_cb_t)(void*, LibFibreObject* obj);
+
+typedef void (*on_stopped_cb_t)(void*, LibFibreStatus);
+
+typedef void (*on_attribute_added_cb_t)(void*, LibFibreAttribute*, const char* name, size_t name_length, LibFibreInterface*, const char* intf_name, size_t intf_name_length);
+typedef void (*on_attribute_removed_cb_t)(void*, LibFibreAttribute*);
+
+/**
+ * @brief on_function_added callback type for libfibre_subscribe_to_interface().
+ * 
+ * @param ctx: The user data that was passed to libfibre_subscribe_to_interface().
+ * @param func: A handle for the function. Remains valid until the corresponding
+ *        call to on_function_removed().
+ * @param name: The ASCII-encoded name of the function.
+ * @param name_length: Length in bytes of the name.
+ * @param input_names: A null-terminated list of null-terminated ASCII-encoded
+ *        strings. Each string corresponds to the name of one input argument.
+ *        The list and the string buffers are only valid for the duration of the
+ *        callback. They must not be freed by the application.
+ * @param input_codecs: A null-terminated list of null-terminated ASCII-encoded
+ *        strings. Each string names the codec of one input argument.
+ *        The list and the string buffers are only valid for the duration of the
+ *        callback. They must not be freed by the application.
+ * @param output_names: Analogous to input_names.
+ * @param output_codecs: Analogous to output names.
+ */
+typedef void (*on_function_added_cb_t)(void* ctx, LibFibreFunction* func, const char* name, size_t name_length, const char** input_names, const char** input_codecs, const char** output_names, const char** output_codecs);
+
+typedef void (*on_function_removed_cb_t)(void*, LibFibreFunction*);
+
+/**
+ * @brief Callback type for libfibre_call().
+ * 
+ * For an overview of the coroutine call control flow see libfibre_call().
+ * 
+ * @param ctx: The context pointer that was passed to libfibre_call().
+ * @param tx_end: End of the range of data that was accepted by libfibre. This
+ *        is always in the interval [tx_buf, tx_buf + tx_len] where `tx_buf` and
+ *        `tx_len` are the arguments of the corresponding libfibre_call() call.
+ * @param tx_end: End of the range of data that was returned by libfibre. This
+ *        is always in the interval [rx_buf, rx_buf + rx_len] where `rx_buf` and
+ *        `rx_len` are the arguments of the corresponding libfibre_call() call.
+ * @param tx_buf: The application should set this to the next buffer to
+ *        transmit. The buffer must remain valid until the next callback
+ *        invokation.
+ * @param tx_len: The length of tx_buf. Must be zero if tx_buf is NULL.
+ * @param rx_buf: The application should set this to the buffer into which data
+ *        should be written. The buffer must remain allocated until the next
+ *        callback invokation.
+ * @param rx_len: The length of rx_buf. Must be zero if rx_buf is NULL.
+ * 
+ * @retval kFibreOk: The application set tx_buf and rx_buf to valid or empty
+ *         buffers and libfibre should invoke the callback again when it has
+ *         made progress.
+ * @retval kFibreBusy: The application cannot provide a new tx_buf or rx_buf at
+ *         the moment. The application will eventually call libfibre_call() for
+ *         this coroutine call again.
+ * @retval kFibreClosed: The application may have returned non-empty buffers and
+ *         if libfibre manages to fully handle these buffers it shall consider
+ *         the call ended.
+ * @retval kFibreCancelled: The application did not set valid tx and rx buffers
+ *         and libfibre should consider the call cancelled. Libfibre will not
+ *         invoke the callback anymore.
+ */
+typedef LibFibreStatus (*libfibre_call_cb_t)(void* ctx,
+        LibFibreStatus status,
+        const unsigned char* tx_end, unsigned char* rx_end,
+        const unsigned char** tx_buf, size_t* tx_len,
+        unsigned char** rx_buf, size_t* rx_len);
+
+/**
+ * @brief TX completion callback type for libfibre_start_tx().
+ * 
+ * @param ctx: The user data that was passed to libfibre_start_tx().
+ * @param tx_stream: The TX stream on which the TX operation completed.
+ * @param status: The status of the last TX operation.
+ *         - kFibreOk: The indicated range of the TX buffer was successfully
+ *           transmitted and the stream might accept more data.
+ *         - kFibreClosed: The indicated range of the TX buffer was successfully
+ *           transmitted and the stream will no longer accept any data.
+ *         - Any other status: Successful transmission of the data cannot be
+ *           guaranteed and no more data can be sent on this stream.
+ * @param tx_end: Points to the address after the last byte read from the
+ *        TX buffer. This pointer always points to a valid position in the
+ *        buffer (or the end of the buffer), even if the transmission failed.
+ *        However if the status is something other than kFibreOk and
+ *        kFibreClosed then the pointer may not precisely indicate the
+ *        transmitted data range.
+ */
+typedef void (*on_tx_completed_cb_t)(void* ctx, LibFibreTxStream* tx_stream, LibFibreStatus status, const uint8_t* tx_end);
+
+/**
+ * @brief RX completion callback type for libfibre_start_rx().
+ * 
+ * @param ctx: The user data that was passed to libfibre_start_rx().
+ * @param rx_stream: The RX stream on which the RX operation completed.
+ * @param status: The status of the last RX operation.
+ *         - kFibreOk: The indicated range of the RX buffer was successfully
+ *           filled with received data and the stream might emit more data.
+ *         - kFibreClosed: The indicated range of the RX buffer was successfully
+ *           filled with received data and the stream will emit no more data.
+ *         - Any other status: Successful transmission of the data cannot be
+ *           guaranteed and no more data can be sent on this stream.
+ * @param rx_end: Points to the address after the last byte written to the
+ *        RX buffer. This pointer always points to a valid position in the
+ *        buffer (or the end of the buffer), even if the reception failed.
+ *        However if the status is something other than kFibreOk and
+ *        kFibreClosed then the pointer may not precisely indicate the
+ *        received data range.
+ */
+typedef void (*on_rx_completed_cb_t)(void* ctx, LibFibreRxStream* rx_stream, LibFibreStatus status, uint8_t* rx_end);
+
+/**
+ * @brief Returns the version of the libfibre library.
+ * 
+ * The returned struct must not be freed.
+ * 
+ * The version adheres to Semantic Versioning, that means breaking changes of
+ * the ABI can be detected by an increment of the major version number (unless
+ * it's zero).
+ * 
+ * Even if breaking changes are introduced, we promise to keep this function
+ * backwards compatible.
+ */
+FIBRE_PUBLIC const struct LibFibreVersion* libfibre_get_version();
+
+/**
+ * @brief Opens and initializes a Fibre context.
+ * 
+ * @param event_loop: The event loop on which libfibre will run. Some function
+          of the event loop can be left unimplemented (set to NULL) depending on
+          the platform and the backends used (TODO: elaborate).
+          The event loop must be single threaded and all calls to libfibre must
+          happen on the event loop thread.
+ */
+FIBRE_PUBLIC struct LibFibreCtx* libfibre_open(LibFibreEventLoop event_loop);
+
+/**
+ * @brief Closes a context that was previously opened with libfibre_open().
+ *
+ * This function must not be invoked before all ongoing discovery processes
+ * are stopped and all channels are closed.
+ */
+FIBRE_PUBLIC void libfibre_close(struct LibFibreCtx* ctx);
+
+/**
+ * @brief Registers an external channel provider.
+ * 
+ * Libfibre starts and stops the discoverer on demand as a result of calls
+ * to libfibre_start_discovery() and libfibre_stop_discovery().
+ * This can be used by applications to implement transport providers which are
+ * not supported natively in libfibre.
+ */
+FIBRE_PUBLIC void libfibre_register_backend(LibFibreCtx* ctx, const char* name,
+    size_t name_length, on_start_discovery_cb_t on_start_discovery,
+    on_stop_discovery_cb_t on_stop_discovery, void* cb_ctx);
+
+/**
+ * @brief Creates a communication domain from the specified spec string.
+ * 
+ * @param ctx: The libfibre context that was obtained from libfibre_open().
+ * @param specs: Pointer to an ASCII string encoding the channel specifications.
+ *        Must remain valid for the life time of the discovery.
+ *        See README of the main Fibre repository for details.
+ *        (https://github.com/samuelsadok/fibre/tree/devel).
+ * @returns: An opaque handle which can be passed to libfibre_start_discovery().
+ */
+FIBRE_PUBLIC LibFibreDomain* libfibre_open_domain(LibFibreCtx* ctx,
+    const char* specs, size_t specs_len);
+
+/**
+ * @brief Closes a domain that was previously opened with libfibre_open_domain().
+ */
+FIBRE_PUBLIC void libfibre_close_domain(LibFibreDomain* domain);
+
+/**
+ * @brief Adds new TX and RX channels to a domain.
+ * 
+ * The channels can be closed with libfibre_close_tx() and libfibre_close_rx().
+ */
+FIBRE_PUBLIC void libfibre_add_channels(LibFibreDomain* domain, LibFibreRxStream** tx_channel, LibFibreTxStream** rx_channel, size_t mtu);
+
+/**
+ * @brief Starts looking for Fibre objects that match the specifications.
+ *
+ * @param domain: The domain obtained from libfibre_open_domain() on which to
+ *        discover objects.
+ * @param on_found_object: Invoked for every matching object that is found.
+ *        The application must expect the same object handle to appear more than
+ *        once.
+ *        libfibre increments the internal reference count of the object before
+ *        this call and decrements it after the corresponding call to
+ *        on_lost_object. When the reference count reaches zero the application
+ *        must no longer use it. The reference count is always non-negative.
+ * @param on_lost_object: Invoked when an object is lost.
+ * @param on_stopped: Invoked when the discovery stops for any reason, including
+ *        a corresponding call to libfibre_stop_discovery().
+ * @param cb_ctx: Arbitrary user data passed to the callbacks.
+ * @returns: An opaque handle which should be passed to libfibre_stop_discovery().
+ */
+FIBRE_PUBLIC void libfibre_start_discovery(LibFibreDomain* domain,
+    LibFibreDiscoveryCtx** handle, on_found_object_cb_t on_found_object,
+    on_lost_object_cb_t on_lost_object,
+    on_stopped_cb_t on_stopped, void* cb_ctx);
+
+/**
+ * @brief Stops an ongoing discovery process that was previously started with
+ * libfibre_start_discovery().
+ *
+ * The discovery is stopped asynchronously. That means it must still be
+ * considered ongoing until the on_stopped callback which was passed to
+ * libfibre_start_discovery() is invoked. Once this callback is invoked,
+ * libfibre_stop_discovery() must no longer be called.
+ */
+FIBRE_PUBLIC void libfibre_stop_discovery(LibFibreDiscoveryCtx* handle);
+
+/**
+ * @brief Subscribes to changes on the interface.
+ * 
+ * All functions and attributes which are already part of the interface by the
+ * time this function is called are also announced to the subscriber.
+ * 
+ * @param interface: An interface handle that was obtained in the callback of
+ *        libfibre_start_discovery().
+ * @param on_attribute_added: Invoked when an attribute is added to the
+ *        interface.
+ * @param on_attribute_removed: Invoked when an attribute is removed from the
+ *        interface, including when the interface is being torn down. This is
+ *        called exactly once for every call to on_attribute_added().
+ * @param on_function_added: Invoked when a function is added to the
+ *        interface. The input_names, input_codecs, output_names and
+ *        output_codecs arguments are null terminated lists of null terminated
+ *        strings. The name buffer and the four lists are only valid for the
+ *        duration of the callback and must not be freed by the application.
+ *        The function handle remains valid until the corresponding call to
+ *        on_function_removed().
+ * @param on_function_removed: Invoked when a function is removed from the
+ *        interface, including when the interface is being torn down. This is
+ *        called exactly once for every call to on_function_added().
+ * @param cb_ctx: Arbitrary user data passed to the callbacks.
+ */
+FIBRE_PUBLIC void libfibre_subscribe_to_interface(LibFibreInterface* interface,
+    on_attribute_added_cb_t on_attribute_added,
+    on_attribute_removed_cb_t on_attribute_removed,
+    on_function_added_cb_t on_function_added,
+    on_function_removed_cb_t on_function_removed,
+    void* cb_ctx);
+
+/**
+ * @brief Returns the object that corresponds the the specified attribute of
+ * another object.
+ * 
+ * This function runs purely locally and therefore returns a result immediately.
+ * 
+ * TODO: it might be useful to allow this operation to go through to the remote
+ * device.
+ * TODO: Specify whether the returned object handle must be identical for
+ * repeated calls.
+ * 
+ * @param parent_obj: An object handle that was obtained in the callback of
+ *        libfibre_start_discovery() or from a previous call to
+ *        libfibre_get_attribute().
+ * @param attr: An attribute handle that was obtained in the on_attribute_added()
+ *        callback of libfibre_subscribe_to_interface().
+ * @param child_obj_ptr: If and only if the function succeeds, the variable that
+ *        this argument points to is set to the requested subobject. The returned
+ *        object handle is only guaranteed to remain valid for as long as the
+ *        parent object handle is valid.
+ * @returns: kFibreOk or kFibreInvalidArgument
+ */
+FIBRE_PUBLIC LibFibreStatus libfibre_get_attribute(LibFibreObject* parent_obj, LibFibreAttribute* attr, LibFibreObject** child_obj_ptr);
+
+/**
+ * @brief Starts a remote coroutine call or continues or cancels an ongoing call.
+ * 
+ * A remote coroutine call can be considered a continuous exchange of the
+ * following tuples:
+ * 
+ *  Client Application  ===== (tx_buf, rx_buf, status) ====> libfibre
+ *  Client Application  <==== (tx_end, rx_end, status) ===== libfibre
+ * 
+ * These tuples are exchanged through the input/output arguments of
+ * libfibre_call() or libfibre_call()'s callback.
+ * 
+ * If during an ongoing call either of the two parties is unable to respond
+ * immediately it responds with kFibreBusy and will thus get the responsibility
+ * to resume the call when able. kFibreCancelled can be issued by either party
+ * at any time.
+ * 
+ * Each party must make progress during every control transfer to the other
+ * party.
+ * 
+ * For the application this means for every call to libfibre_call() and every
+ * return from libfibre_call()'s callback the arguments passed from application
+ * to libfibre must satisfy at least one of the following:
+ * 
+ *  - The call handle is NULL
+ *  - tx_len is non-zero
+ *  - rx_len is non-zero
+ *  - The status is different from kFibreOk
+ * 
+ * For libfibre this means every for return from libfibre_call() and every
+ * call to libfibre_call()'s callback the arguments passed from libfibre to
+ * application satisfy at least one of the following:
+ * 
+ *  - tx_end is larger than the corresponding tx_buf
+ *  - rx_end is larger than the corresponding rx_buf
+ *  - The status is different from kFibreOk
+ * 
+ * @param func: A function handle that was obtained in the on_function_added()
+ *        callback of libfibre_subscribe_to_interface().
+ * @param handle: The variable being pointed to by this argument identifies the
+ *        coroutine call. If the variable is NULL it will be set to a new opaque
+ *        handle. If the variable is not NULL the active function call is
+ *        continued or cancelled (depending on status).
+ * @param tx_buf: The buffer to transmit. If libfibre_call() returns kFibreBusy
+ *        then this buffer must remain valid until `callback` is invoked.
+ *        Otherwise it can be freed immediately after this call.
+ * @param tx_len: Length of tx_buf. Must be zero if tx_buf is NULL.
+ * @param rx_buf: The buffer into which the received data should be written. If
+ *        libfibre_call() returns kFibreBusy then this buffer must remain
+ *        allocated until `callback` is invoked. Otherwise it can be freed
+ *        immediately after this call.
+ * @param rx_len: Length of rx_buf. Must be zero if rx_buf is NULL.
+ * @param tx_end: End of the range of data that was accepted by libfibre. This
+ *        is always in the interval [tx_buf, tx_buf + tx_len] unless
+ *        libfibre_call() returns kFibreBusy, in which case this is NULL.
+ *        This value does not give any delivery guarantees.
+ * @param rx_end: End of the range of data that was returned by libfibre. This
+ *        is always in the interval [rx_buf, rx_buf + rx_len] unless
+ *        libfibre_call() returns kFibreBusy, in which case this is NULL.
+ * @param callback: Will be invoked eventually if and only if libfibre_call()
+ *        returns kFibreBusy. This callback is never invoked from inside
+ *        libfibre_call().
+ * @param cb_ctx: An opaque application-defined handle that gets passed to
+ *        `callback`.
+ * 
+ * @retval kFibreOk: libfibre accepted some or all of the tx_buf or filled some
+ *         or all of the rx_buf with data and can immediately accept more TX
+ *         data or provide more RX data.
+ * @retval kFibreBusy: libfibre will complete the request asynchronously by
+ *         calling `callback`. If this value is returned, then the application
+ *         must not invoke libfibre_call() on the same call handle again until
+ *         `callback` is invoked except for cancelling the call with a status
+ *         of `kFibreCancelled`.
+ * @retval kFibreClosed: the remote server completed the call and will not
+ *         accept or return any more data on this call. The application must not
+ *         pass the closed call context handle to libfibre_call() anymore.
+ * @retval kFibreCancelled: the application's cancellation request was honored
+ *         or the remote server cancelled the call. The application must not
+ *         pass the cancelled call context handle to libfibre_call() anymore.
+ */
+FIBRE_PUBLIC LibFibreStatus libfibre_call(LibFibreFunction* func, LibFibreCallContext** handle,
+        LibFibreStatus status,
+        const unsigned char* tx_buf, size_t tx_len,
+        unsigned char* rx_buf, size_t rx_len,
+        const unsigned char** tx_end,
+        unsigned char** rx_end,
+        libfibre_call_cb_t callback, void* cb_ctx);
+
+/**
+ * @brief Starts sending data on the specified TX stream.
+ * 
+ * The TX operation must be considered in progress until the on_completed
+ * callback is called. Until then the application must not start another TX
+ * operation on the same stream. In the meantime the application can call
+ * libfibre_cancel_tx() at any time to abort the operation.
+ * 
+ * @param tx_stream: The stream on which to send data.
+ * @param tx_buf: The buffer to transmit. Must remain valid until the operation
+ *        completes.
+ * @param tx_len: Length of tx_buf.
+ * @param on_completed: Called when the operation completes, whether successful
+ *        or not.
+ * @param ctx: Arbitrary user data passed to the on_completed callback.
+ */
+FIBRE_PUBLIC void libfibre_start_tx(LibFibreTxStream* tx_stream, const uint8_t* tx_buf, size_t tx_len, on_tx_completed_cb_t on_completed, void* ctx);
+
+/**
+ * @brief Cancels an ongoing TX operation.
+ * 
+ * Must only be called if there is actually a TX operation in progress for which
+ * cancellation has not yet been requested.
+ * The application must still wait for the on_complete callback to be called
+ * before the operation can be considered finished. The completion callback may
+ * be called with kFibreCancelled or any other status.
+ * 
+ * TODO: specify if streams can be restarted (current doc of on_tx_completed_cb_t implies no)
+ * 
+ * @param tx_stream: The TX stream on which to cancel the ongoing TX operation.
+ */
+FIBRE_PUBLIC void libfibre_cancel_tx(LibFibreTxStream* tx_stream);
+
+/**
+ * @brief Permanently close TX stream.
+ * 
+ * Must not be called while a transfer is ongoing.
+ */
+FIBRE_PUBLIC void libfibre_close_tx(LibFibreTxStream* tx_stream, LibFibreStatus status);
+
+/**
+ * @brief Starts receiving data on the specified RX stream.
+ * 
+ * The RX operation must be considered in progress until the on_completed
+ * callback is called. Until then the application must not start another RX
+ * operation on the same stream. In the meantime the application can call
+ * libfibre_cancel_rx() at any time to abort the operation.
+ * 
+ * @param rx_stream: The stream on which to receive data.
+ * @param rx_buf: The buffer to receive to. Must remain valid until the
+ *        operation completes.
+ * @param rx_len: Length of rx_buf.
+ * @param on_completed: Called when the operation completes, whether successful
+ *        or not.
+ * @param ctx: Arbitrary user data passed to the on_completed callback.
+ */
+FIBRE_PUBLIC void libfibre_start_rx(LibFibreRxStream* rx_stream, uint8_t* rx_buf, size_t rx_len, on_rx_completed_cb_t on_completed, void* ctx);
+
+/**
+ * @brief Cancels an ongoing RX operation.
+ * 
+ * Must only be called if there is actually a RX operation in progress for which
+ * cancellation has not yet been requested.
+ * The application must still wait for the on_complete callback to be called
+ * before the operation can be considered finished. The completion callback may
+ * be called with kFibreCancelled or any other status.
+ * 
+ * TODO: specify if streams can be restarted (current doc of on_rx_completed_cb_t implies no)
+ * 
+ * @param rx_stream: The RX stream on which to cancel the ongoing RX operation.
+ */
+FIBRE_PUBLIC void libfibre_cancel_rx(LibFibreRxStream* rx_stream);
+
+/**
+ * @brief Permanently close RX stream.
+ * 
+ * Must not be called while a transfer is ongoing.
+ */
+FIBRE_PUBLIC void libfibre_close_rx(LibFibreRxStream* rx_stream, LibFibreStatus status);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // __LIBFIBRE_H

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/simple_serdes.hpp

@@ -0,0 +1,127 @@
+#ifndef __FIBRE_SIMPLE_SERDES
+#define __FIBRE_SIMPLE_SERDES
+
+#include "cpp_utils.hpp"
+#include "limits.h"
+#include <optional> // TODO: make C++11 backport of this
+#include <cstring>
+#include <stdint.h>
+
+template<typename T, bool BigEndian, typename = void>
+struct SimpleSerializer;
+template<typename T>
+using LittleEndianSerializer = SimpleSerializer<T, false>;
+template<typename T>
+using BigEndianSerializer = SimpleSerializer<T, true>;
+
+
+/* @brief Serializer/deserializer for arbitrary integral number types */
+// TODO: allow reading an arbitrary number of bits
+template<typename T, bool BigEndian>
+struct SimpleSerializer<T, BigEndian, typename std::enable_if_t<std::is_integral<T>::value>> {
+    static constexpr size_t BIT_WIDTH = std::numeric_limits<T>::digits;
+    static constexpr size_t BYTE_WIDTH = (BIT_WIDTH + 7) / 8;
+
+    template<typename TIterator>
+    static std::optional<T> read(TIterator* begin, TIterator end = nullptr) {
+        T result = 0;
+        if (BigEndian) {
+            for (size_t i = BYTE_WIDTH; i > 0; (i++, (*begin)++)) {
+                if (end && !(*begin < end))
+                    return std::nullopt;
+                uint8_t byte = **begin;
+                result |= static_cast<T>(byte) << ((i - 1) << 3);
+            }
+        } else {
+            for (size_t i = 0; i < BYTE_WIDTH; (i++, (*begin)++)) {
+                if (end && !(*begin < end))
+                    return std::nullopt;
+                uint8_t byte = **begin;
+                result |= static_cast<T>(byte) << (i << 3);
+            }
+        }
+        return result;
+    }
+
+    template<typename TIterator>
+    static bool write(T value, TIterator* begin, TIterator end = nullptr) {
+        if (BigEndian) {
+            for (size_t i = BYTE_WIDTH; i > 0; (i--, (*begin)++)) {
+                if (end && !(*begin < end))
+                    return false;
+                uint8_t byte = static_cast<uint8_t>((value >> ((i - 1) << 3)) & 0xff);
+                **begin = byte;
+            }
+        } else {
+            for (size_t i = 0; i < BYTE_WIDTH; (i++, (*begin)++)) {
+                if (end && !(*begin < end))
+                    return false;
+                uint8_t byte = static_cast<uint8_t>((value >> (i << 3)) & 0xff);
+                **begin = byte;
+            }
+        }
+        return true;
+    }
+};
+
+template<typename T>
+inline std::optional<T> read_le(fibre::cbufptr_t* buffer) {
+    static_assert(is_complete<LittleEndianSerializer<T>>(), "no LittleEndianSerializer is defined for type T");
+    return LittleEndianSerializer<T>::read(&buffer->begin(), buffer->end());
+}
+
+template<typename T>
+inline bool write_le(T value, fibre::bufptr_t* buffer) {
+    static_assert(is_complete<LittleEndianSerializer<T>>(), "no LittleEndianSerializer is defined for type T");
+    return LittleEndianSerializer<T>::write(value, &buffer->begin(), buffer->end());
+}
+
+template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
+inline size_t write_le(T value, uint8_t* buffer){
+    //TODO: add static_assert that this is still a little endian machine
+    std::memcpy(&buffer[0], &value, sizeof(value));
+    return sizeof(value);
+}
+
+template<typename T>
+typename std::enable_if_t<std::is_const<T>::value, size_t>
+write_le(T value, uint8_t* buffer) {
+    return write_le<std::remove_const_t<T>>(value, buffer);
+}
+
+template<>
+inline size_t write_le<float>(float value, uint8_t* buffer) {
+    static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
+    static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
+    uint32_t value_as_uint32;
+    std::memcpy(&value_as_uint32, &value, sizeof(uint32_t));
+    return write_le<uint32_t>(value_as_uint32, buffer);
+}
+
+template<typename T>
+inline size_t read_le(T* value, const uint8_t* buffer){
+    // TODO: add static_assert that this is still a little endian machine
+    std::memcpy(value, buffer, sizeof(*value));
+    return sizeof(*value);
+}
+
+template<>
+inline size_t read_le<float>(float* value, const uint8_t* buffer) {
+    static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
+    static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
+    return read_le(reinterpret_cast<uint32_t*>(value), buffer);
+}
+
+// @brief Reads a value of type T from the buffer.
+// @param buffer    Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read.
+// @param length    The number of available bytes in buffer. This value is updated to subtract the bytes that were read.
+template<typename T>
+static inline T read_le(const uint8_t** buffer, size_t* length) {
+    T result;
+    size_t cnt = read_le(&result, *buffer);
+    *buffer += cnt;
+    *length -= cnt;
+    return result;
+}
+
+#endif

ODrive-fw-v0.5.6/Firmware/fibre-cpp/include/fibre/status.hpp

@@ -0,0 +1,20 @@
+#ifndef __FIBRE_STATUS_HPP
+#define __FIBRE_STATUS_HPP
+
+namespace fibre {
+
+enum Status {
+    kFibreOk,
+    kFibreBusy, //<! The request will complete asynchronously
+    kFibreCancelled, //!< The operation was cancelled due to a request by the application or the remote peer
+    kFibreClosed, //!< The operation has finished orderly or shall be finished orderly
+    kFibreInvalidArgument, //!< Bug in the application
+    kFibreInternalError, //!< Bug in the local fibre implementation
+    kFibreProtocolError, //!< A remote peer is misbehaving (indicates bug in the remote peer)
+    kFibreHostUnreachable, //!< The remote peer can no longer be reached
+    //kFibreInsufficientData, // maybe we will introduce this to tell the caller that the granularity of the data is too small
+};
+
+}
+
+#endif // __FIBRE_STATUS_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/interfaces_template.j2

@@ -0,0 +1,102 @@
+/*[# This is the original template, thus the warning below does not apply to this file #]
+ * ============================ WARNING ============================
+ * ==== This is an autogenerated file.                          ====
+ * ==== Any changes to this file will be lost when recompiling. ====
+ * =================================================================
+ *
+ * This file contains base classes that correspond to the interfaces defined in
+ * your interface file. The objects you publish should inherit from these
+ * interfaces.
+ *
+ */
+#ifndef __FIBRE_INTERFACES_HPP
+#define __FIBRE_INTERFACES_HPP
+
+[[userdata.c_preamble]]
+
+#include <fibre/../../protocol.hpp>
+
+#pragma GCC push_options
+#pragma GCC optimize ("s")
+
+[%- macro rettype(func) %]
+[%- if not func.out -%]
+void
+[%- elif func.out | length == 1 -%]
+[[(func.out.values() | first).type.c_name]]
+[%- else -%]
+std::tuple<[% for arg in func.out.values() %][[arg.type.c_name]][[', ' if not loop.last]][% endfor %]>
+[%- endif -%]
+[%- endmacro %]
+
+[%- macro render_interface(intf) %]
+class [[intf.name | to_pascal_case]]Intf[% if intf.implements %] :[%- for base_intf in intf.implements %] public [[base_intf.c_name]][% endfor %][% endif %] {
+public:
+[%- for intf in intf.interfaces -%]
+[[render_interface(intf) | indent(4)]]
+[%- endfor %]
+[%- for enum in intf.enums %]
+    enum [[enum.name | to_pascal_case]] {
+[%- for k, value in enum['values'].items() %]
+        [[((enum.name | to_macro_case) + "_" + (k | to_macro_case)).ljust(32)]] = [% if enum.is_flags %]0x[['%08x' | format(value.value)]][% else %][[value.value]][% endif %],
+[%- endfor %]
+    };
+[%- endfor %]
+
+[%- for property in intf.attributes.values() %]
+[%- if property.type.fullname.startswith("fibre.Property") %]
+[%- if not property.c_getter and not property.c_setter %]
+    template<typename T> static inline auto get_[[property.name]](T* obj) { return [[property.type.c_name]]{&obj->[[property.c_name]]}; }
+    template<typename T> static inline void get_[[property.name]](T* obj, void* ptr) { new (ptr) [[property.type.c_name]]{&obj->[[property.c_name]]}; }[# these are for the set_endpoint_from_float function. This is unmaintainable and should go away #]
+[%- elif not property.c_setter %]
+    template<typename T> static inline auto get_[[property.name]](T* obj) { return [[property.type.c_name]]{obj, [](void* ctx){ return ([[property.type.value_type.c_name]])((T*)ctx)->[[property.c_getter]]; }}; }
+    template<typename T> static inline void get_[[property.name]](T* obj, void* ptr) { new (ptr) [[property.type.c_name]]{obj, [](void* ctx){ return ([[property.type.value_type.c_name]])((T*)ctx)->[[property.c_getter]]; }}; }
+[%- else %]
+    template<typename T> static inline auto get_[[property.name]](T* obj) { return [[property.type.c_name]]{obj, [](void* ctx){ return ([[property.type.value_type.c_name]])((T*)ctx)->[[property.c_getter]]; }, [](void* ctx, [[property.type.value_type.c_name]] value){ ((T*)ctx)->[[property.c_setter]](value); }}; }
+    template<typename T> static inline void get_[[property.name]](T* obj, void* ptr) { new (ptr) [[property.type.c_name]]{obj, [](void* ctx){ return ([[property.type.value_type.c_name]])((T*)ctx)->[[property.c_getter]]; }, [](void* ctx, [[property.type.value_type.c_name]] value){ ((T*)ctx)->[[property.c_setter]](value); }}; }
+[%- endif %]
+[%- else %]
+    template<typename T> static inline auto get_[[property.name]](T* obj) { return &obj->[[property.c_name]]; }
+[%- endif %]
+[%- endfor %]
+
+[%- for func in intf.functions.values() %]
+    virtual [[rettype(func)]] [[func.name | to_snake_case]]([% for in in func.in.values() %][% if loop.index0 %][[in.type.c_name]] [[in.name]][[', ' if not loop.last]][% endif %][% endfor %]) = 0;
+[%- endfor %]
+[%- for func in intf.functions.values() %]
+[%- for k, arg in func.in.items() | skip_first %]
+    [[arg.type.c_name]] [[func.name | to_snake_case]]_in_[[arg.name]]_; // for internal use by Fibre
+    template<typename T> static auto get_[[func.name | to_snake_case]]_in_[[arg.name]]_(T* obj) { return Property<[[arg.type.c_name]]>{&obj->[[func.name | to_snake_case]]_in_[[arg.name]]_}; }
+    template<typename T> static void get_[[func.name | to_snake_case]]_in_[[arg.name]]_(T* obj, void* ptr) { new (ptr) Property<[[arg.type.c_name]]>{&obj->[[func.name | to_snake_case]]_in_[[arg.name]]_}; }
+[%- endfor %]
+[%- for k, arg in func.out.items() %]
+    [[arg.type.c_name]] [[func.name | to_snake_case]]_out_[[arg.name]]_; // for internal use by Fibre
+    template<typename T> static auto get_[[func.name | to_snake_case]]_out_[[arg.name]]_(T* obj) { return Property<const [[arg.type.c_name]]>{&obj->[[func.name | to_snake_case]]_out_[[arg.name]]_}; }
+    template<typename T> static void get_[[func.name | to_snake_case]]_out_[[arg.name]]_(T* obj, void* ptr) { new (ptr) Property<const [[arg.type.c_name]]>{&obj->[[func.name | to_snake_case]]_out_[[arg.name]]_}; }
+[%- endfor %]
+[%- endfor %]
+};
+[%- endmacro %]
+
+[% for intf in toplevel_interfaces %]
+[[render_interface(intf)]]
+[% endfor %]
+
+[%- for _, enum in value_types.items() %]
+[%- if enum.is_flags %]
+// this is technically not thread-safe but practically it might be
+inline [[enum.c_name]] operator | ([[enum.c_name]] a, [[enum.c_name]] b) { return static_cast<[[enum.c_name]]>(static_cast<std::underlying_type_t<[[enum.c_name]]>>(a) | static_cast<std::underlying_type_t<[[enum.c_name]]>>(b)); }
+inline [[enum.c_name]] operator & ([[enum.c_name]] a, [[enum.c_name]] b) { return static_cast<[[enum.c_name]]>(static_cast<std::underlying_type_t<[[enum.c_name]]>>(a) & static_cast<std::underlying_type_t<[[enum.c_name]]>>(b)); }
+inline [[enum.c_name]] operator ^ ([[enum.c_name]] a, [[enum.c_name]] b) { return static_cast<[[enum.c_name]]>(static_cast<std::underlying_type_t<[[enum.c_name]]>>(a) ^ static_cast<std::underlying_type_t<[[enum.c_name]]>>(b)); }
+inline [[enum.c_name]]& operator |= ([[enum.c_name]] &a, [[enum.c_name]] b) { return reinterpret_cast<[[enum.c_name]]&>(reinterpret_cast<std::underlying_type_t<[[enum.c_name]]>&>(a) |= static_cast<std::underlying_type_t<[[enum.c_name]]>>(b)); }
+inline [[enum.c_name]]& operator &= ([[enum.c_name]] &a, [[enum.c_name]] b) { return reinterpret_cast<[[enum.c_name]]&>(reinterpret_cast<std::underlying_type_t<[[enum.c_name]]>&>(a) &= static_cast<std::underlying_type_t<[[enum.c_name]]>>(b)); }
+inline [[enum.c_name]]& operator ^= ([[enum.c_name]] &a, [[enum.c_name]] b) { return reinterpret_cast<[[enum.c_name]]&>(reinterpret_cast<std::underlying_type_t<[[enum.c_name]]>&>(a) ^= static_cast<std::underlying_type_t<[[enum.c_name]]>>(b)); }
+inline [[enum.c_name]] operator ~ ([[enum.c_name]] a) { return static_cast<[[enum.c_name]]>(~static_cast<std::underlying_type_t<[[enum.c_name]]>>(a)); }
+[%- endif %]
+[%- endfor %]
+
+
+
+#pragma GCC pop_options
+
+#endif // __FIBRE_INTERFACES_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/legacy_object_client.cpp

@@ -0,0 +1,694 @@
+
+#include "legacy_object_client.hpp"
+#include "legacy_protocol.hpp"
+#include <fibre/simple_serdes.hpp>
+#include "logging.hpp"
+#include "print_utils.hpp"
+#include "crc.hpp"
+#include <variant>
+#include <algorithm>
+
+DEFINE_LOG_TOPIC(LEGACY_OBJ);
+USE_LOG_TOPIC(LEGACY_OBJ);
+
+using namespace fibre;
+
+struct json_error {
+    const char* ptr;
+    std::string str;
+};
+
+struct json_value;
+using json_list = std::vector<std::shared_ptr<json_value>>;
+using json_dict = std::vector<std::pair<std::shared_ptr<json_value>, std::shared_ptr<json_value>>>;
+using json_value_variant = std::variant<std::string, int, json_list, json_dict, json_error>;
+
+struct json_value : json_value_variant {
+    //json_value(const json_value_variant& v) : json_value_variant{v} {}
+    template<typename T> json_value(T&& arg) : json_value_variant{std::forward<T>(arg)} {}
+    //json_value_variant v;
+};
+
+// helper functions
+bool json_is_str(json_value val) { return val.index() == 0; }
+bool json_is_int(json_value val) { return val.index() == 1; }
+bool json_is_list(json_value val) { return val.index() == 2; }
+bool json_is_dict(json_value val) { return val.index() == 3; }
+bool json_is_err(json_value val) { return val.index() == 4; }
+std::string json_as_str(json_value val) { return std::get<0>(val); }
+int json_as_int(json_value val) { return std::get<1>(val); }
+json_list json_as_list(json_value val) { return std::get<2>(val); }
+json_dict json_as_dict(json_value val) { return std::get<3>(val); }
+json_error json_as_err(json_value val) { return std::get<4>(val); }
+
+json_value json_make_error(const char* ptr, std::string str) {
+    return {json_error{ptr, str}};
+}
+
+
+void json_skip_whitespace(const char** begin, const char* end) {
+    while (*begin < end && std::isspace(**begin)) {
+        (*begin)++;
+    }
+}
+
+bool json_comp(const char* begin, const char* end, char c) {
+    return begin < end && *begin == c;
+}
+
+json_value json_parse(const char** begin, const char* end) {
+    // skip whitespace
+
+    if (*begin >= end) {
+        return json_make_error(*begin, "expected value but got EOF");
+    }
+
+    if (json_comp(*begin, end, '{')) {
+        // parse dict
+        (*begin)++; // consume leading '{'
+        json_dict dict;
+        bool expect_comma = false;
+
+        json_skip_whitespace(begin, end);
+        while (!json_comp(*begin, end, '}')) {
+            if (expect_comma) {
+                if (!json_comp(*begin, end, ',')) {
+                    return json_make_error(*begin, "expected ',' or '}'");
+                }
+                (*begin)++; // consume comma
+                json_skip_whitespace(begin, end);
+            }
+            expect_comma = true;
+
+            // Parse key-value pair
+            json_value key = json_parse(begin, end);
+            if (json_is_err(key)) return key;
+            json_skip_whitespace(begin, end);
+            if (!json_comp(*begin, end, ':')) {
+                return json_make_error(*begin, "expected :");
+            }
+            (*begin)++;
+            json_value val = json_parse(begin, end);
+            if (json_is_err(val)) return val;
+            dict.push_back({std::make_shared<json_value>(key), std::make_shared<json_value>(val)});
+
+            json_skip_whitespace(begin, end);
+        }
+
+        (*begin)++;
+        return {dict};
+
+    } else if (json_comp(*begin, end, '[')) {
+        // parse list
+        (*begin)++; // consume leading '['
+        json_list list;
+        bool expect_comma = false;
+
+        json_skip_whitespace(begin, end);
+        while (!json_comp(*begin, end, ']')) {
+            if (expect_comma) {
+                if (!json_comp(*begin, end, ',')) {
+                    return json_make_error(*begin, "expected ',' or ']'");
+                }
+                (*begin)++; // consume comma
+                json_skip_whitespace(begin, end);
+            }
+            expect_comma = true;
+
+            // Parse item
+            json_value val = json_parse(begin, end);
+            if (json_is_err(val)) return val;
+            list.push_back(std::make_shared<json_value>(val));
+
+            json_skip_whitespace(begin, end);
+        }
+
+        (*begin)++; // consume trailing ']'
+        return {list};
+
+    } else if (json_comp(*begin, end, '"')) {
+        // parse string
+        (*begin)++; // consume leading '"'
+        std::string str;
+
+        while (!json_comp(*begin, end, '"')) {
+            if (*begin >= end) {
+                return json_make_error(*begin, "expected '\"' but got EOF");
+            }
+            if (json_comp(*begin, end, '\\')) {
+                return json_make_error(*begin, "escaped strings not supported");
+            }
+            str.push_back(**begin);
+            (*begin)++;
+        }
+
+        (*begin)++; // consume trailing '"'
+        return {str};
+
+    } else if (std::isdigit(**begin)) {
+        // parse int
+
+        std::string str;
+        while (*begin < end && std::isdigit(**begin)) {
+            str.push_back(**begin);
+            (*begin)++;
+        }
+
+        return {std::stoi(str)}; // note: this can throw an exception if the int is too long
+
+    } else {
+        return json_make_error(*begin, "unexpected character '" + std::string(*begin, *begin + 1) + "'");
+    }
+}
+
+json_value json_dict_find(json_dict dict, std::string key) {
+    auto it = std::find_if(dict.begin(), dict.end(),
+        [&](std::pair<std::shared_ptr<json_value>, std::shared_ptr<json_value>>& kv){
+            return json_is_str(*kv.first) && json_as_str(*kv.first) == key;
+        });
+    return (it == dict.end()) ? json_make_error(nullptr, "key not found") : *it->second;
+}
+
+// not sure if this function exists in the STL
+template<typename TIt, typename TFunc, typename TNum = decltype(std::declval<TFunc>()(*std::declval<TIt>()))>
+TNum calc_sum(TIt begin, TIt end, TFunc func) {
+    TNum s = {};
+    for (TIt it = begin; it != end; ++it) {
+        s += func(*it);
+    }
+    return s;
+}
+
+std::unordered_map<std::string, size_t> codecs = {
+    {"bool", 1},
+    {"int8", 1},
+    {"uint8", 1},
+    {"int16", 2},
+    {"uint16", 2},
+    {"int32", 4},
+    {"uint32", 4},
+    {"int64", 8},
+    {"uint64", 8},
+    {"float", 4},
+    {"endpoint_ref", 4}
+};
+
+size_t get_codec_size(std::string codec) {
+    auto it = codecs.find(codec);
+    return (it == codecs.end()) ? 0 : it->second;
+}
+
+std::vector<LegacyFibreArg> parse_arglist(const json_value& list_val) {
+    std::vector<LegacyFibreArg> arglist;
+
+    for (auto& arg : json_is_list(list_val) ? json_as_list(list_val) : json_list()) {
+        if (!json_is_dict(*arg)) {
+            FIBRE_LOG(W) << "arglist is invalid";
+            continue;
+        }
+        auto dict = json_as_dict(*arg);
+        
+        json_value name_val = json_dict_find(dict, "name");
+        json_value id_val = json_dict_find(dict, "id");
+        json_value type_val = json_dict_find(dict, "type");
+
+        if (!json_is_str(name_val) || !json_is_int(id_val) || ((int)(size_t)json_as_int(id_val) != json_as_int(id_val)) || !json_is_str(type_val)) {
+            FIBRE_LOG(W) << "arglist is invalid";
+            continue;
+        }
+
+
+        arglist.push_back({
+            json_as_str(name_val),
+            json_as_str(type_val),
+            (json_as_str(type_val) == "endpoint_ref") ? "object_ref" : json_as_str(type_val),
+            get_codec_size(json_as_str(type_val)),
+            (json_as_str(type_val) == "endpoint_ref") ? sizeof(uintptr_t) : get_codec_size(json_as_str(type_val)),
+            (size_t)json_as_int(id_val),
+        });
+    }
+
+    return arglist;
+}
+
+void LegacyObjectClient::start(Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_found_root_object, Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_lost_root_object) {
+    FIBRE_LOG(D) << "start";
+    on_found_root_object_ = on_found_root_object;
+    on_lost_root_object_ = on_lost_root_object;
+    json_.clear();
+    receive_more_json();
+}
+
+std::shared_ptr<FibreInterface> LegacyObjectClient::get_property_interfaces(std::string codec, bool write) {
+    auto& dict = write ? rw_property_interfaces : ro_property_interfaces;
+
+    auto it = dict.find(codec);
+    if (it != dict.end()) {
+        return it->second;
+    }
+
+    auto intf_ptr = std::make_shared<FibreInterface>();
+    dict[codec] = intf_ptr;
+    FibreInterface& intf = *intf_ptr;
+    size_t size = get_codec_size(codec);
+    std::string app_codec = codec == "endpoint_ref" ? "object_ref" : codec;
+    size_t app_codec_size = codec == "endpoint_ref" ? sizeof(uintptr_t) : size;
+
+    if (!size || !app_codec_size) {
+        FIBRE_LOG(W) << "unknown size for codec " << codec;
+    }
+    
+    intf.name = std::string{} + "fibre.Property<" + (write ? "readwrite" : "readonly") + " " + codec + ">";
+    intf.functions.emplace("read", LegacyFunction{0, nullptr, {}, {{"value", codec, app_codec, size, app_codec_size, 0}}});
+    if (write) {
+        intf.functions.emplace("exchange", LegacyFunction{0, nullptr, {{"newval", codec, app_codec, size, app_codec_size, 0}}, {{"oldval", codec, app_codec, size, app_codec_size, 0}}});
+    }
+    
+    return intf_ptr;
+}
+
+std::shared_ptr<LegacyObject> LegacyObjectClient::load_object(json_value list_val) {
+    
+
+    if (!json_is_list(list_val)) {
+        FIBRE_LOG(W) << "interface members must be a list";
+        return nullptr;
+    }
+
+    LegacyObject obj{
+        .client = this,
+        .ep_num = 0,
+        .intf = std::make_shared<FibreInterface>(),
+        .known_to_application = false
+    };
+    auto obj_ptr = std::make_shared<LegacyObject>(obj);
+    FibreInterface& intf = *obj_ptr->intf;
+
+    for (auto& item: json_as_list(list_val)) {
+        if (!json_is_dict(*item)) {
+            FIBRE_LOG(W) << "expected dict";
+            continue;
+        }
+        auto dict = json_as_dict(*item);
+
+        json_value type = json_dict_find(dict, "type");
+        json_value name_val = json_dict_find(dict, "name");
+        std::string name = json_is_str(name_val) ? json_as_str(name_val) : "[anonymous]";
+
+        if (json_is_str(type) && json_as_str(type) == "object") {
+            std::shared_ptr<LegacyObject> subobj = load_object(json_dict_find(dict, "members"));
+            intf.attributes[name] = {subobj};
+
+        } else if (json_is_str(type) && json_as_str(type) == "function") {
+            json_value id = json_dict_find(dict, "id");
+            if (!json_is_int(id) || ((int)(size_t)json_as_int(id) != json_as_int(id))) {
+                continue;
+            }
+            intf.functions.emplace(name, LegacyFunction{
+                (size_t)json_as_int(id),
+                obj_ptr.get(),
+                parse_arglist(json_dict_find(dict, "inputs")),
+                parse_arglist(json_dict_find(dict, "outputs"))
+            });
+
+        } else if (json_is_str(type) && json_as_str(type) == "json") {
+            // Ignore
+
+        } else if (json_is_str(type)) {
+            std::string type_str = json_as_str(type);
+            json_value access = json_dict_find(dict, "access");
+            std::string access_str = json_is_str(access) ? json_as_str(access) : "r";
+            bool can_write = access_str.find('w') != std::string::npos;
+
+            json_value id = json_dict_find(dict, "id");
+            if (!json_is_int(id) || ((int)(size_t)json_as_int(id) != json_as_int(id))) {
+                continue;
+            }
+
+            LegacyObject subobj{
+                .client = this,
+                .ep_num = (size_t)json_as_int(id),
+                .intf = get_property_interfaces(type_str, can_write),
+                .known_to_application = false
+            };
+            auto subobj_ptr = std::make_shared<LegacyObject>(subobj);
+            objects_.push_back(subobj_ptr);
+            intf.attributes[name] = {subobj_ptr};
+
+        } else {
+            FIBRE_LOG(W) << "unsupported codec";
+        }
+    }
+
+    objects_.push_back(obj_ptr);
+    return obj_ptr;
+}
+
+void LegacyObjectClient::receive_more_json() {
+    write_le<uint32_t>(json_.size(), tx_buf_);
+    json_.resize(json_.size() + 1024);
+    bufptr_t rx_buf = {json_.data() + json_.size() - 1024, json_.data() + json_.size()};
+    protocol_->start_endpoint_operation(0, tx_buf_, rx_buf, &op_handle_, MEMBER_CB(this, on_received_json));
+}
+
+void LegacyObjectClient::on_received_json(EndpointOperationResult result) {
+    // The JSON read operation completed
+
+    op_handle_ = 0;
+
+    if (result.status == kStreamCancelled) {
+        return;
+    } else if (result.status == kStreamClosed) {
+        return;
+    } else if (result.status != kStreamOk) {
+        FIBRE_LOG(W) << "JSON read operation failed"; // TODO: add retry logic
+        return;
+    }
+
+    size_t n_received = result.rx_end - json_.data() - json_.size() + 1024;
+    json_.resize(json_.size() - 1024 + n_received);
+
+    if (n_received) {
+        receive_more_json();
+
+    } else {
+
+        FIBRE_LOG(D) << "received JSON of length " << json_.size();
+        //FIBRE_LOG(D) << "JSON: " << str{json_.data(), json_.data() + json_.size()};
+
+        const char *begin = reinterpret_cast<const char*>(json_.data());
+        auto val = json_parse(&begin, begin + json_.size());
+
+        if (json_is_err(val)) {
+            size_t pos = json_as_err(val).ptr - reinterpret_cast<const char*>(json_.data());
+            FIBRE_LOG(E) << "JSON parsing error: " << json_as_err(val).str << " at position " << pos;
+            return;
+        } else if (!json_is_list(val)) {
+            FIBRE_LOG(E) << "JSON data must be a list";
+            return;
+        }
+
+        FIBRE_LOG(D) << "sucessfully parsed JSON";
+        root_obj_ = load_object(val);
+        json_crc_ = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(PROTOCOL_VERSION, json_.data(), json_.size());
+        if (root_obj_) {
+            on_found_root_object_.invoke_and_clear(this, root_obj_);
+        }
+    }
+}
+
+
+std::optional<CallBufferRelease> LegacyFunction::call(void** call_handle,
+        CallBuffers buffers,
+        Callback<std::optional<CallBuffers>, CallBufferRelease> callback) {
+    
+    LegacyCallContext* ctx;
+
+    if (!*call_handle) {
+        // Instantiate new call
+        ctx = new LegacyCallContext();
+        ctx->func_ = this;
+
+        size_t total_tx_decoded_size = sizeof(uintptr_t);
+        for (auto& arg: inputs) {
+            total_tx_decoded_size += arg.app_size;
+        }
+
+        size_t total_rx_encoded_size = 0;
+        for (auto& arg: outputs) {
+            total_rx_encoded_size += arg.protocol_size;
+        }
+
+        ctx->tx_buf_.resize(total_tx_decoded_size);
+        ctx->rx_buf_.resize(total_rx_encoded_size);
+
+        *call_handle = ctx;
+    } else {
+        // Resume call
+        ctx = reinterpret_cast<LegacyCallContext*>(*call_handle);
+    }
+
+
+    std::variant<LegacyCallContext::ResultFromApp,
+                 LegacyCallContext::ResultFromProtocol> result = buffers;
+
+    // Run endpoint operations for as long as we can do this synchronously.
+    for (;;) {
+        auto continuation = ctx->get_next_task(result);
+        if (continuation.index() == 0) {
+            return std::get<0>(continuation);
+        } else if (continuation.index() == 1) {
+            auto proto_continuation = std::get<1>(continuation);
+            proto_continuation.client->start_endpoint_operation(
+                    proto_continuation.ep_num, proto_continuation.tx_buf,
+                    proto_continuation.rx_buf, &ctx->op_handle_,
+                    MEMBER_CB(ctx, resume_from_protocol));
+
+            if (!ctx->ep_result.has_value()) {
+                ctx->callback = callback;
+                return std::nullopt; // protocol will resume asynchronously
+            }
+
+            result = *ctx->ep_result;
+            ctx->ep_result = std::nullopt;
+
+            // TODO: ensure progress
+        } else {
+            return CallBufferRelease{kFibreInternalError, ctx->app_tx_end_, ctx->app_rx_buf_.begin()};
+        }
+    }
+
+    return ctx->resume_from_app(buffers, callback);
+}
+
+
+void LegacyCallContext::resume_from_protocol(EndpointOperationResult result) {
+    if (!callback) {
+        // No callback configured. This means that this function is being executed
+        // synchronously from inside LegacyFunction::call(). Set result and return.
+        ep_result = result;
+        return;
+    }
+    op_handle_ = 0;
+
+    std::variant<ResultFromApp, ResultFromProtocol> res = result; 
+
+    for (;;) {
+        auto continuation = get_next_task(res);
+        if (continuation.index() == 0) {
+            auto app_result = callback.invoke(std::get<0>(continuation));
+            if (std::get<0>(continuation).status != kFibreOk) {
+                if (app_result.has_value() && (app_result->status != kFibreClosed || app_result->rx_buf.size() || app_result->tx_buf.size())) {
+                    FIBRE_LOG(W) << "app tried to continue a closed call";
+                }
+                FIBRE_LOG(T) << "closing call";
+                delete this;
+                return;
+            } else {
+                res = *app_result;
+            }
+        } else if (continuation.index() == 1) {
+            auto proto_continuation = std::get<1>(continuation);
+            proto_continuation.client->start_endpoint_operation(
+                    proto_continuation.ep_num, proto_continuation.tx_buf,
+                    proto_continuation.rx_buf, &op_handle_,
+                    MEMBER_CB(this, resume_from_protocol));
+            return; // protocol will return asynchronously
+        } else {
+            callback.invoke({kFibreInternalError, app_tx_end_, app_rx_buf_.begin()});
+            return;
+        }
+    }
+}
+
+bool LegacyObjectClient::transcode(cbufptr_t src, bufptr_t dst, std::string src_codec, std::string dst_codec) {
+    if (src_codec == "object_ref" && dst_codec == "endpoint_ref") {
+        if (src.size() < sizeof(uintptr_t) || dst.size() < 4) {
+            return false;
+        }
+
+        uintptr_t val = *reinterpret_cast<const uintptr_t*>(src.begin());
+        LegacyObject* obj = reinterpret_cast<LegacyObject*>(val);
+        write_le<uint16_t>(obj ? obj->ep_num : 0, &dst);
+        write_le<uint16_t>(obj ? obj->client->json_crc_ : 0, &dst);
+
+    } else if (src_codec == "endpoint_ref" && dst_codec == "object_ref") {
+        if (src.size() < 4 || dst.size() < sizeof(uintptr_t)) {
+            return false;
+        }
+
+        uint16_t ep_num = *read_le<uint16_t>(&src);
+        uint16_t json_crc = *read_le<uint16_t>(&src);
+
+        LegacyObject* obj_ptr = nullptr;
+
+        if (ep_num && json_crc == json_crc_) {
+            for (auto& known_obj: objects_) {
+                if (known_obj->ep_num == ep_num) {
+                    obj_ptr = known_obj.get();
+                }
+            }
+        }
+
+        FIBRE_LOG(D) << "placing transcoded ptr " << reinterpret_cast<uintptr_t>(obj_ptr);
+        *reinterpret_cast<uintptr_t*>(dst.begin()) = reinterpret_cast<uintptr_t>(obj_ptr);
+
+    } else {
+        if (src.size() != dst.size()) {
+            return false;
+        }
+
+        memcpy(dst.begin(), src.begin(), src.size());
+    }
+
+    return true;
+}
+
+
+std::variant<LegacyCallContext::ContinueWithApp, LegacyCallContext::ContinueWithProtocol, LegacyCallContext::InternalError> LegacyCallContext::get_next_task(std::variant<ResultFromApp, ResultFromProtocol> continue_from) {
+    if (progress == 0) {
+        if (continue_from.index() != 0) {
+            FIBRE_LOG(E) << "expected continuation from app";
+            return InternalError{};
+        }
+
+        ResultFromApp result_from_app = std::get<0>(continue_from);
+
+        size_t n_copy = std::min(tx_buf_.size() - tx_pos_, result_from_app.tx_buf.size());
+        std::copy_n(result_from_app.tx_buf.begin(), n_copy, tx_buf_.begin() + tx_pos_);
+        result_from_app.tx_buf = result_from_app.tx_buf.skip(n_copy);
+        tx_pos_ += n_copy;
+
+        app_tx_end_ = result_from_app.tx_buf.begin();
+        app_rx_buf_ = result_from_app.rx_buf;
+
+        if (tx_pos_ < tx_buf_.size()) {
+            // application specified kFibreOk? => return kFibreOk
+            // application specified kFibreClosed? => return kFibreClosed
+            return ContinueWithApp{result_from_app.status, app_tx_end_, app_rx_buf_.begin()};
+        }
+
+    } else if (progress <= func_->inputs.size() + 1 + func_->outputs.size()) {
+        if (continue_from.index() != 1) {
+            FIBRE_LOG(E) << "expected continuation from protocol";
+            return InternalError{};
+        }
+
+        ResultFromProtocol result_from_protocol = std::get<1>(continue_from);
+
+        if (result_from_protocol.status == kStreamClosed) {
+            return ContinueWithApp{kFibreHostUnreachable, app_tx_end_, app_rx_buf_.begin()};
+        } else if (result_from_protocol.status != kStreamOk) {
+            FIBRE_LOG(W) << "protocol failed with " << result_from_protocol.status << " - propagating error to application";
+            return ContinueWithApp{kFibreHostUnreachable, app_tx_end_, app_rx_buf_.begin()};
+        }
+
+        tx_pos_ = result_from_protocol.tx_end - tx_buf_.data();
+        if (result_from_protocol.rx_end) {
+            rx_pos_ = result_from_protocol.rx_end - rx_buf_.data();
+        }
+
+    } else if (progress == func_->inputs.size() + 2 + func_->outputs.size()) {
+        if (continue_from.index() != 0) {
+            FIBRE_LOG(E) << "expected continuation from app";
+            return InternalError{};
+        }
+
+        ResultFromApp result_from_app = std::get<0>(continue_from);
+
+        if (result_from_app.status != kFibreOk && result_from_app.status != kFibreClosed) {
+            FIBRE_LOG(W) << "application failed with " << result_from_app.status << " - dropping this call";
+            return InternalError{};
+        }
+
+        app_tx_end_ = result_from_app.tx_buf.begin();
+        app_rx_buf_ = result_from_app.rx_buf;
+    }
+
+    if (progress == 0) {
+        // Transcode from application codec to protocol codec
+
+        obj_ = *reinterpret_cast<LegacyObject**>(tx_buf_.data());
+        FIBRE_LOG(T) << "object is " << as_hex(reinterpret_cast<uintptr_t>(obj_));
+        FIBRE_LOG(T) << "tx buf is " << as_hex(cbufptr_t{tx_buf_});
+
+        std::vector<uint8_t> transcoded;
+        size_t transcoded_size = calc_sum(func_->inputs.begin(), func_->inputs.end(),
+            [](LegacyFibreArg& arg) { return arg.protocol_size; });
+        FIBRE_LOG(T) << "transcoding " << func_->inputs.size() << " inputs from " << tx_buf_.size() << " B to " << transcoded_size << " B";
+        transcoded.resize(transcoded_size);
+        
+        tx_pos_ = sizeof(uintptr_t);
+        size_t transcoded_pos = 0;
+        for (auto& arg: func_->inputs) {
+            if (!obj_->client->transcode({tx_buf_.data() + tx_pos_, arg.app_size},
+                    {transcoded.data() + transcoded_pos, arg.protocol_size},
+                    arg.app_codec, arg.protocol_codec)) {
+                return ContinueWithApp{kFibreInternalError, app_tx_end_, app_rx_buf_.begin()};
+            }
+            transcoded_pos += arg.protocol_size;
+            tx_pos_ += arg.app_size;
+        }
+
+        tx_buf_ = transcoded;
+        tx_pos_ = 0;
+
+    } else if (progress == func_->inputs.size() + 1 + func_->outputs.size()) {
+        // Transcode from protocol codec to application codec
+
+        std::vector<uint8_t> transcoded;
+        for (auto& arg: func_->outputs) {
+            FIBRE_LOG(T) << "arg size " << arg.app_size;
+        }
+        size_t transcoded_size = calc_sum(func_->outputs.begin(), func_->outputs.end(),
+            [](LegacyFibreArg& arg) { return arg.app_size; });
+        FIBRE_LOG(T) << "transcoding " << func_->outputs.size() << " outputs from " << rx_buf_.size() << " B to " << transcoded_size << " B";
+        transcoded.resize(transcoded_size);
+        
+        rx_pos_ = 0;
+        size_t transcoded_pos = 0;
+        for (auto& arg: func_->outputs) {
+            if (!obj_->client->transcode({rx_buf_.data() + rx_pos_, arg.protocol_size},
+                    {transcoded.data() + transcoded_pos, arg.app_size},
+                    arg.protocol_codec, arg.app_codec)) {
+                return ContinueWithApp{kFibreInternalError, app_tx_end_, app_rx_buf_.begin()};
+            }
+            transcoded_pos += arg.app_size;
+            rx_pos_ += arg.protocol_size;
+        }
+
+        rx_buf_ = transcoded;
+        rx_pos_ = 0;
+
+        FIBRE_LOG(T) << "rx buf is " << as_hex(cbufptr_t{rx_buf_});
+    }
+
+    progress++;
+
+    if (progress == 1 && obj_->ep_num) {
+        // Single Endpoint Function - exchange everything in one go
+        progress = func_->inputs.size() + 1 + func_->outputs.size();
+        return ContinueWithProtocol{obj_->client->protocol_, obj_->ep_num, tx_buf_, rx_buf_};
+
+    } else if (progress <= func_->inputs.size()) {
+        // send arg
+        auto arg = func_->inputs[progress - 1];
+        return ContinueWithProtocol{obj_->client->protocol_, arg.ep_num, {tx_buf_.data() + tx_pos_, arg.protocol_size}, {}};
+    } else if (progress == func_->inputs.size() + 1) {
+        // send trigger
+        return ContinueWithProtocol{obj_->client->protocol_, func_->ep_num, {}, {}};
+    } else if (progress <= func_->inputs.size() + 1 + func_->outputs.size()) {
+        // receive arg
+        auto arg = func_->outputs[progress - 2 - func_->inputs.size()];
+        return ContinueWithProtocol{obj_->client->protocol_, arg.ep_num, {}, {rx_buf_.data() + rx_pos_, arg.protocol_size}};
+    } else if (progress == func_->inputs.size() + 2 + func_->outputs.size()) {
+        // return data to application
+        size_t n_copy = std::min(rx_buf_.size() - rx_pos_, app_rx_buf_.size());
+        std::copy_n(rx_buf_.data() + rx_pos_, n_copy, app_rx_buf_.begin());
+        app_rx_buf_ = app_rx_buf_.skip(n_copy);
+        rx_pos_ += n_copy;
+        return ContinueWithApp{rx_pos_ == rx_buf_.size() ? kFibreClosed : kFibreOk, app_tx_end_, app_rx_buf_.begin()};
+    }
+
+    return InternalError{};
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/legacy_object_client.hpp

@@ -0,0 +1,152 @@
+#ifndef __FIBRE_LEGACY_OBJECT_MODEL_HPP
+#define __FIBRE_LEGACY_OBJECT_MODEL_HPP
+
+#include <fibre/async_stream.hpp>
+#include <unordered_map>
+#include <vector>
+#include <memory>
+#include <string>
+#include <fibre/callback.hpp>
+#include <fibre/cpp_utils.hpp> // std::variant and std::optional C++ backport
+#include <fibre/fibre.hpp>
+
+struct json_value;
+
+namespace fibre {
+
+struct EndpointOperationResult {
+    StreamStatus status;
+    const uint8_t* tx_end;
+    uint8_t* rx_end;
+};
+
+// Lower 16 bits are the seqno. Upper 16 bits are all 1 for valid handles
+// (such that seqno 0 doesn't cause the handle to be 0)
+using EndpointOperationHandle = uint32_t;
+
+struct LegacyProtocolPacketBased;
+
+struct LegacyFibreArg {
+    std::string name;
+    std::string protocol_codec;
+    std::string app_codec;
+    size_t protocol_size;
+    size_t app_size;
+    size_t ep_num;
+};
+
+struct LegacyObject;
+
+struct LegacyFunction : Function {
+    LegacyFunction(std::vector<LegacyFibreArg> inputs, std::vector<LegacyFibreArg> outputs)
+        : ep_num(0), obj_(nullptr), inputs(inputs), outputs(outputs) {}
+    LegacyFunction(size_t ep_num, LegacyObject* obj, std::vector<LegacyFibreArg> inputs, std::vector<LegacyFibreArg> outputs)
+        : ep_num(ep_num), obj_(obj), inputs(inputs), outputs(outputs) {}
+
+    std::optional<CallBufferRelease>
+    call(void**, CallBuffers, Callback<std::optional<CallBuffers>, CallBufferRelease>) final;
+
+    size_t ep_num; // 0 for property read/write/exchange functions
+    LegacyObject* obj_; // null for property read/write/exchange functions (all other functions are associated with one object only)
+    std::vector<LegacyFibreArg> inputs;
+    std::vector<LegacyFibreArg> outputs;
+};
+
+struct FibreInterface;
+class LegacyObjectClient;
+
+struct LegacyFibreAttribute {
+    std::shared_ptr<LegacyObject> object;
+};
+
+struct FibreInterface {
+    std::string name;
+    std::unordered_map<std::string, LegacyFunction> functions;
+    std::unordered_map<std::string, LegacyFibreAttribute> attributes;
+};
+
+struct LegacyObject {
+    LegacyObjectClient* client;
+    size_t ep_num;
+    std::shared_ptr<FibreInterface> intf;
+    bool known_to_application;
+};
+
+struct LegacyCallContext {
+    LegacyFunction* func_;
+
+    size_t progress = 0; //!< 0: expecting more tx data
+                         //!< [1...n_inputs]: endpoint operations for sending inputs
+                         //!< n_inputs + 1: trigger endpoint operation
+                         //!< [n_inputs + 2, n_inputs + 2 + n_outputs]: endpoint operations for receiving outputs
+                         //!< n_inputs + 3 + n_outputs: reporting outputs to application
+                         
+    EndpointOperationHandle op_handle_ = 0;
+
+    std::vector<uint8_t> tx_buf_;
+    size_t tx_pos_ = 0;
+    std::vector<uint8_t> rx_buf_;
+    size_t rx_pos_ = 0;
+
+    const uint8_t* app_tx_end_;
+    bufptr_t app_rx_buf_;
+
+    Callback<std::optional<CallBuffers>, CallBufferRelease> callback;
+    std::optional<EndpointOperationResult> ep_result;
+
+    LegacyObject* obj_;
+
+    std::optional<CallBufferRelease>
+    resume_from_app(CallBuffers, Callback<std::optional<CallBuffers>, CallBufferRelease>);
+
+    void resume_from_protocol(EndpointOperationResult result);
+
+    struct ContinueWithProtocol {
+        LegacyProtocolPacketBased* client;
+        size_t ep_num;
+        cbufptr_t tx_buf;
+        bufptr_t rx_buf;
+    };
+
+    using ContinueWithApp = CallBufferRelease;
+    using ResultFromProtocol = EndpointOperationResult;
+    using ResultFromApp = CallBuffers;
+    struct InternalError {};
+
+    // Returns control either to the application or to the next endpoint operation
+    std::variant<ContinueWithApp, ContinueWithProtocol, InternalError> get_next_task(std::variant<ResultFromApp, ResultFromProtocol> continue_from);
+};
+
+class LegacyObjectClient {
+public:
+    LegacyObjectClient(LegacyProtocolPacketBased* protocol) : protocol_(protocol) {}
+
+    void start(Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_found_root_object, Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_lost_root_object);
+    bool transcode(cbufptr_t src, bufptr_t dst, std::string src_codec, std::string dst_codec);
+
+    // For direct access by LegacyProtocolPacketBased and libfibre.cpp
+    uint16_t json_crc_ = 0;
+    Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_lost_root_object_;
+    std::shared_ptr<LegacyObject> root_obj_;
+    std::vector<std::shared_ptr<LegacyObject>> objects_;
+    void* user_data_; // used by libfibre to store the libfibre context pointer
+    LegacyProtocolPacketBased* protocol_;
+
+private:
+    std::shared_ptr<FibreInterface> get_property_interfaces(std::string codec, bool write);
+    std::shared_ptr<LegacyObject> load_object(json_value list_val);
+    void receive_more_json();
+    void on_received_json(EndpointOperationResult result);
+
+    Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_found_root_object_;
+    uint8_t tx_buf_[4] = {0xff, 0xff, 0xff, 0xff};
+    EndpointOperationHandle op_handle_ = 0;
+    std::vector<uint8_t> json_;
+    //std::vector<LegacyCallContext*> pending_calls_;
+    std::unordered_map<std::string, std::shared_ptr<FibreInterface>> rw_property_interfaces;
+    std::unordered_map<std::string, std::shared_ptr<FibreInterface>> ro_property_interfaces;
+};
+
+}
+
+#endif // __FIBRE_LEGACY_OBJECT_MODEL_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/legacy_protocol.cpp

@@ -0,0 +1,567 @@
+
+
+#include "legacy_protocol.hpp"
+
+#include "protocol.hpp"
+#include "crc.hpp"
+#include "logging.hpp"
+#include "print_utils.hpp"
+#include <fibre/async_stream.hpp>
+#include <memory>
+#include <stdlib.h>
+#include <algorithm>
+
+DEFINE_LOG_TOPIC(LEGACY_PROTOCOL);
+USE_LOG_TOPIC(LEGACY_PROTOCOL);
+
+using namespace fibre;
+
+
+/* PacketWrapper -------------------------------------------------------------*/
+
+void PacketWrapper::start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) {
+    if (handle) {
+        *handle = reinterpret_cast<TransferHandle>(this);
+    }
+
+    if (state_ != kStateIdle) {
+        completer.invoke({kStreamError, buffer.begin()});
+    }
+
+    // TODO: support buffer size >= 128
+    if (buffer.size() >= 128) {
+        completer.invoke({kStreamError, buffer.begin()});
+    }
+
+    completer_ = completer;
+
+    header_buf_[0] = CANONICAL_PREFIX;
+    header_buf_[1] = static_cast<uint8_t>(buffer.size());
+    header_buf_[2] = calc_crc8<CANONICAL_CRC8_POLYNOMIAL>(CANONICAL_CRC8_INIT, header_buf_, 2);
+    
+    payload_buf_ = buffer;
+
+    uint16_t crc16 = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(CANONICAL_CRC16_INIT, buffer.begin(), buffer.size());
+    trailer_buf_[0] = (uint8_t)((crc16 >> 8) & 0xff),
+    trailer_buf_[1] = (uint8_t)((crc16 >> 0) & 0xff);
+
+    state_ = kStateSendingHeader;
+    expected_tx_end_ = header_buf_ + 3;
+    tx_channel_->start_write(header_buf_, &inner_transfer_handle_, MEMBER_CB(this, complete));
+}
+
+void PacketWrapper::cancel_write(TransferHandle transfer_handle) {
+    state_ = kStateCancelling;
+    tx_channel_->cancel_write(inner_transfer_handle_);
+}
+
+void PacketWrapper::complete(WriteResult result) {
+    if (state_ == kStateCancelling) {
+        state_ = kStateIdle;
+        completer_.invoke_and_clear({kStreamCancelled, payload_buf_.begin()});
+        return;
+    }
+
+    if (result.status != kStreamOk) {
+        state_ = kStateIdle;
+        completer_.invoke_and_clear({result.status, payload_buf_.begin()});
+        return;
+    }
+
+    if (result.end < expected_tx_end_) {
+        tx_channel_->start_write({result.end, expected_tx_end_}, &inner_transfer_handle_, MEMBER_CB(this, complete));
+        return;
+    }
+
+    if (state_ == kStateSendingHeader) {
+        state_ = kStateSendingPayload;
+        expected_tx_end_ = payload_buf_.end();
+        tx_channel_->start_write(payload_buf_, &inner_transfer_handle_, MEMBER_CB(this, complete));
+
+    } else if (state_ == kStateSendingPayload) {
+        state_ = kStateSendingTrailer;
+        expected_tx_end_ = trailer_buf_ + 2;
+        tx_channel_->start_write(trailer_buf_, &inner_transfer_handle_, MEMBER_CB(this, complete));
+
+    } else if (state_ == kStateSendingTrailer) {
+        state_ = kStateIdle;
+        completer_.invoke_and_clear({kStreamOk, payload_buf_.end()});
+    }
+}
+
+
+/* PacketUnwrapper -----------------------------------------------------------*/
+
+void PacketUnwrapper::start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) {
+    if (handle) {
+        *handle = reinterpret_cast<TransferHandle>(this);
+    }
+
+    if (state_ != kStateIdle) {
+        completer.invoke({kStreamError, buffer.begin()});
+    }
+
+    completer_ = completer;
+    payload_buf_ = buffer;
+
+    state_ = kStateReceivingHeader;
+    expected_rx_end_ = rx_buf_ + 3;
+    rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, MEMBER_CB(this, complete));
+}
+
+void PacketUnwrapper::cancel_read(TransferHandle transfer_handle) {
+    state_ = kStateCancelling;
+    rx_channel_->cancel_read(inner_transfer_handle_);
+}
+
+void PacketUnwrapper::complete(ReadResult result) {
+    // All code paths in this function must end with either of these two:
+    //   - rx_channel_->start_read() to bounce back control to the underlying stream
+    //   - safe_complete() to return control to the client
+
+    if (state_ == kStateCancelling) {
+        state_ = kStateIdle;
+        completer_.invoke_and_clear({kStreamCancelled, payload_buf_.begin()});
+        return;
+    }
+
+    if (result.status != kStreamOk) {
+        state_ = kStateIdle;
+        completer_.invoke_and_clear({result.status, payload_buf_.begin()});
+        return;
+    }
+
+    if (result.end < expected_rx_end_) {
+        rx_channel_->start_read({result.end, expected_rx_end_}, &inner_transfer_handle_, MEMBER_CB(this, complete));
+        return;
+    }
+
+    if (state_ == kStateReceivingHeader) {
+        size_t n_discard;
+
+        // Process header
+        if (rx_buf_[0] != CANONICAL_PREFIX) {
+            n_discard = 1;
+        } else if ((rx_buf_[1] & 0x80)) {
+            n_discard = 2; // TODO: support packets larger than 128 bytes
+        } else if (calc_crc8<CANONICAL_CRC8_POLYNOMIAL>(CANONICAL_CRC8_INIT, rx_buf_, 3)) {
+            n_discard = 3;
+        } else {
+            state_ = kStateReceivingPayload;
+            payload_length_ = std::min(payload_buf_.size(), (size_t)rx_buf_[1]);
+            expected_rx_end_ = payload_buf_.begin() + payload_length_;
+            rx_channel_->start_read(payload_buf_.take(payload_length_), &inner_transfer_handle_, MEMBER_CB(this, complete));
+            return;
+        }
+
+        // Header was bad: discard the bad header bytes and receive more
+        memmove(rx_buf_, rx_buf_ + n_discard, sizeof(rx_buf_) - n_discard);
+        rx_channel_->start_read(bufptr_t{rx_buf_}.skip(3 - n_discard), &inner_transfer_handle_, MEMBER_CB(this, complete));
+
+    } else if (state_ == kStateReceivingPayload) {
+        expected_rx_end_ = rx_buf_ + 2;
+        state_ = kStateReceivingTrailer;
+        rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, MEMBER_CB(this, complete));
+
+    } else if (state_ == kStateReceivingTrailer) {
+        uint16_t crc = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(CANONICAL_CRC16_INIT, payload_buf_.begin(), payload_length_);
+        crc = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(crc, rx_buf_, 2);
+
+        if (!crc) {
+            state_ = kStateIdle;
+            completer_.invoke_and_clear({kStreamOk, payload_buf_.begin() + payload_length_});
+        } else {
+            state_ = kStateReceivingHeader;
+            expected_rx_end_ = rx_buf_ + 3;
+            rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, MEMBER_CB(this, complete));
+        }
+    }
+}
+
+
+/* LegacyProtocolPacketBased -------------------------------------------------*/
+
+#if FIBRE_ENABLE_CLIENT
+
+/**
+ * @brief Starts a remote endpoint operation.
+ * 
+ * @param endpoint_id: The endpoint ID to invoke the operation on.
+ * @param tx_buf: The tx_buf to write to the endpoint. Must remain valid until
+ *        the completer is invoked.
+ * @param rx_length: The desired number of bytes to read from the endpoint. The
+ *        actual returned buffer may be smaller.
+ * @param completer: The completer that will be notified once the operation
+ *        completes (whether successful or not).
+ *        The buffer given to the completer is only valid if the status is
+ *        kStreamOk and until the completer returns.
+ * @param handle: The variable pointed to by this argument is set to a handle
+ *        that can be passed to cancel_endpoint_operation() to cancel the
+ *        ongoing operation. If the completer is invoked directly from within
+ *        this function then the handle is not set later than invoking the
+ *        completer.
+ */
+void LegacyProtocolPacketBased::start_endpoint_operation(uint16_t endpoint_id, cbufptr_t tx_buf, bufptr_t rx_buf, EndpointOperationHandle* handle, Callback<void, EndpointOperationResult> callback) {
+    outbound_seq_no_ = ((outbound_seq_no_ + 1) & 0x7fff);
+
+    EndpointOperation op = {
+        .seqno = (uint16_t)(outbound_seq_no_ | 0x0080), // FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ODrive ASCII protocol
+        .endpoint_id = endpoint_id,
+        .tx_buf = tx_buf,
+        .rx_buf = rx_buf,
+        .callback = callback
+    };
+
+    if (handle) {
+        *handle = op.seqno | 0xffff0000;
+    }
+
+    if (tx_handle_) {
+        FIBRE_LOG(D) << "Endpoint operation already in progress. Enqueuing this one.";
+
+        // A TX operation is already in progress. Enqueue this one.
+        pending_operations_.push_back(op);
+        return;
+    }
+
+    start_endpoint_operation(op);
+}
+
+void LegacyProtocolPacketBased::start_endpoint_operation(EndpointOperation op) {
+    write_le<uint16_t>(op.seqno, tx_buf_);
+    write_le<uint16_t>(op.endpoint_id | 0x8000, tx_buf_ + 2);
+    write_le<uint16_t>(op.rx_buf.size(), tx_buf_ + 4);
+
+    size_t mtu = std::min(sizeof(tx_buf_), tx_mtu_);
+    size_t n_payload = std::min(std::max(mtu, (size_t)8) - 8, op.tx_buf.size());
+
+    memcpy(tx_buf_ + 6, op.tx_buf.begin(), n_payload);
+
+    uint16_t trailer = (op.endpoint_id & 0x7fff) == 0 ?
+                       PROTOCOL_VERSION : client_.json_crc_;
+
+    write_le<uint16_t>(trailer, tx_buf_ + 6 + n_payload);
+
+    expected_acks_[op.seqno] = op;
+    transmitting_op_ = op.seqno | 0xffff0000;
+    tx_channel_->start_write(cbufptr_t{tx_buf_}.take(8 + n_payload), &tx_handle_, MEMBER_CB(this, on_write_finished));
+}
+
+
+void LegacyProtocolPacketBased::cancel_endpoint_operation(EndpointOperationHandle handle) {
+    if (!handle) {
+        return;
+    }
+
+    uint16_t seqno = static_cast<uint16_t>(handle & 0xffff);
+
+    Callback<void, EndpointOperationResult> callback;
+    const uint8_t* tx_end = nullptr;
+    uint8_t* rx_end = nullptr;
+
+    auto it0 = std::find_if(pending_operations_.begin(), pending_operations_.end(), [&](EndpointOperation& op) {
+        return op.seqno == seqno;
+    });
+
+    if (it0 != pending_operations_.end()) {
+        callback = it0->callback;
+        tx_end = it0->tx_buf.begin();
+        rx_end = it0->rx_buf.begin();
+        pending_operations_.erase(it0);
+    }
+
+    auto it1 = expected_acks_.find(seqno);
+
+    if (it1 != expected_acks_.end()) {
+        callback = it1->second.callback;
+        tx_end = it1->second.tx_buf.begin();
+        rx_end = it1->second.rx_buf.begin();
+        expected_acks_.erase(it1);
+    }
+
+    if (transmitting_op_ == handle) {
+        // Cancel the TX task because it belongs to the endpoint operation that
+        // is being cancelled.
+        tx_channel_->cancel_write(tx_handle_);
+    } else {
+        // Either we're waiting for an ack on this operation or it has not yet
+        // been sent. In both cases we can just complete immediately.
+        callback.invoke_and_clear({kStreamCancelled, tx_end, rx_end});
+    }
+}
+
+#endif
+
+#if FIBRE_ENABLE_SERVER
+
+// Returns part of the JSON interface definition.
+bool fibre::endpoint0_handler(fibre::cbufptr_t* input_buffer, fibre::bufptr_t* output_buffer) {
+    // The request must contain a 32 bit integer to specify an offset
+    std::optional<uint32_t> offset = read_le<uint32_t>(input_buffer);
+    
+    if (!offset.has_value()) {
+        // Didn't receive any offset
+        return false;
+    } else if (*offset == 0xffffffff) {
+        // If the offset is special value 0xFFFFFFFF, send back the JSON version ID instead
+        return write_le<uint32_t>(json_version_id_, output_buffer);
+    } else if (*offset >= embedded_json_length) {
+        // Attempt to read beyond the buffer end - return empty response
+        return true;
+    } else {
+        // Return part of the json file
+        size_t n_copy = std::min(output_buffer->size(), embedded_json_length - (size_t)*offset);
+        memcpy(output_buffer->begin(), embedded_json + *offset, n_copy);
+        *output_buffer = output_buffer->skip(n_copy);
+        return true;
+    }
+}
+
+#endif
+
+void LegacyProtocolPacketBased::on_write_finished(WriteResult result) {
+    tx_handle_ = 0;
+
+    if (rx_status_ != kStreamOk) {
+        on_rx_tx_closed(rx_status_);
+        return;
+    }
+
+#if FIBRE_ENABLE_CLIENT
+    if (transmitting_op_) {
+        uint16_t seqno = transmitting_op_ & 0xffff;
+        transmitting_op_ = 0;
+
+        auto it = expected_acks_.find(seqno);
+
+        size_t n_sent = std::max((size_t)(result.end - tx_buf_), (size_t)8) - 8;
+        it->second.tx_buf = it->second.tx_buf.skip(n_sent);
+        it->second.tx_done = true;
+
+        if (it->second.rx_done) {
+            // It's possible that the RX operation completes before the TX operation
+            auto op = it->second;
+            expected_acks_.erase(it);
+            op.callback.invoke_and_clear({kStreamOk, op.tx_buf.begin(), op.rx_buf.begin()});
+        } else if (result.status != kStreamOk) {
+            // If the TX task was a remote endpoint operation but didn't succeed
+            // we terminate that operation
+            auto op = it->second;
+            expected_acks_.erase(it);
+            op.callback.invoke_and_clear({result.status, result.end, op.rx_buf.begin()});
+        }
+
+        if (transmitting_op_) {
+            return;
+        }
+    }
+#endif
+
+    // TODO: should we prioritize the server or client side here?
+
+#if FIBRE_ENABLE_SERVER
+    if (rx_end_) {
+        // There is a write operation pending from the server side (i.e. an ack
+        // for a local endpoint operation).
+        uint8_t* rx_end = rx_end_;
+        rx_end_ = nullptr;
+        on_read_finished({kStreamOk, rx_end});
+#if FIBRE_ENABLE_CLIENT
+        if (transmitting_op_) {
+            return;
+        }
+#endif
+    }
+#endif
+
+#if FIBRE_ENABLE_CLIENT
+    if (pending_operations_.size() > 0) {
+        // There is a write operation pending from the client side (i.e. an
+        // outgoing remote endpoint operation).
+        EndpointOperation op = pending_operations_[0];
+        pending_operations_.erase(pending_operations_.begin());
+        start_endpoint_operation(op);
+        if (transmitting_op_) {
+            return;
+        }
+    }
+#endif
+}
+
+void LegacyProtocolPacketBased::on_read_finished(ReadResult result) {
+    TransferHandle dummy;
+
+    if (result.status == kStreamClosed) {
+        FIBRE_LOG(D) << "RX stream closed.";
+        on_rx_closed(kStreamClosed);
+        return;
+    } else if (result.status == kStreamCancelled) {
+        FIBRE_LOG(W) << "RX operation cancelled.";
+        on_rx_closed(kStreamCancelled);
+        return;
+    } else if (result.status != kStreamOk) {
+        FIBRE_LOG(W) << "RX error. Not restarting.";
+        // TODO: we should distinguish between permanent and temporary errors.
+        // If we try to restart after a permanent error we might end up in a
+        // busy loop.
+        on_rx_closed(kStreamError);
+        return;
+    }
+
+    cbufptr_t rx_buf = cbufptr_t{rx_buf_, result.end};
+    //FIBRE_LOG(D) << "got packet of length " << (result.end - rx_buf_) /*<< ": " << as_hex(rx_buf)*/;
+
+    std::optional<uint16_t> seq_no = read_le<uint16_t>(&rx_buf);
+
+    if (!seq_no.has_value()) {
+        FIBRE_LOG(W) << "packet too short";
+
+    } else if (*seq_no & 0x8000) {
+
+#if FIBRE_ENABLE_CLIENT
+        
+        auto it = expected_acks_.find(*seq_no & 0x7fff);
+
+        if (it == expected_acks_.end()) {
+            FIBRE_LOG(W) << "received unexpected ACK: " << (*seq_no & 0x7fff);
+        } else {
+            size_t n_copy = std::min((size_t)(result.end - rx_buf.begin()), it->second.rx_buf.size());
+            memcpy(it->second.rx_buf.begin(), rx_buf.begin(), n_copy);
+            it->second.rx_buf = it->second.rx_buf.skip(n_copy);
+            it->second.rx_done = true;
+            FIBRE_LOG(T) << "received ACK: " << (*seq_no & 0x7fff);
+
+            // It's possible that the RX operation completes before the TX operation
+            if (it->second.tx_done) {
+                auto op = it->second;
+                expected_acks_.erase(it);
+                op.callback.invoke_and_clear({kStreamOk, op.tx_buf.begin(), op.rx_buf.begin()});
+            }
+        }
+
+#else
+        FIBRE_LOG(W) << "received ack but client support is not compiled in";
+#endif
+
+    } else {
+
+#if FIBRE_ENABLE_SERVER
+        if (rx_buf.size() < 6) {
+            FIBRE_LOG(W) << "packet too short";
+            rx_channel_->start_read(rx_buf_, &dummy, MEMBER_CB(this, on_read_finished));
+            return;
+        }
+
+        // TODO: think about some kind of ordering guarantees
+        // currently the seq_no is just used to associate a response with a request
+
+        uint16_t endpoint_id = *read_le<uint16_t>(&rx_buf);
+        bool expect_response = endpoint_id & 0x8000;
+        endpoint_id &= 0x7fff;
+
+        if (expect_response && tx_handle_) {
+            // The operation expects a response but the output channel is still
+            // busy. Stop receiving for now. This function will be invoked again
+            // once the TX operation is finished.
+            rx_end_ = result.end;
+            return;
+        }
+
+        // Verify packet trailer. The expected trailer value depends on the selected endpoint.
+        // For endpoint 0 this is just the protocol version, for all other endpoints it's a
+        // CRC over the entire JSON descriptor tree (this may change in future versions).
+        uint16_t expected_trailer = endpoint_id ? fibre::json_crc_ : PROTOCOL_VERSION;
+        uint16_t actual_trailer = *(rx_buf.end() - 2) | (*(rx_buf.end() - 1) << 8);
+        if (expected_trailer != actual_trailer) {
+            FIBRE_LOG(D) << "trailer mismatch for endpoint " << endpoint_id << ": expected " << as_hex(expected_trailer) << ", got " << as_hex(actual_trailer);
+            rx_channel_->start_read(rx_buf_, &dummy, MEMBER_CB(this, on_read_finished));
+            return;
+        }
+        FIBRE_LOG(D) << "trailer ok for endpoint " << endpoint_id;
+
+        // TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
+
+        uint16_t expected_response_length = *read_le<uint16_t>(&rx_buf);
+
+        // Limit response length according to our local TX buffer size
+        if (expected_response_length > tx_mtu_ - 2)
+            expected_response_length = tx_mtu_ - 2;
+
+        fibre::cbufptr_t input_buffer{rx_buf.begin(), rx_buf.end() - 2};
+        fibre::bufptr_t output_buffer{tx_buf_ + 2, expected_response_length};
+        fibre::endpoint_handler(endpoint_id, &input_buffer, &output_buffer);
+
+        // Send response
+        if (expect_response) {
+            size_t actual_response_length = expected_response_length - output_buffer.size() + 2;
+            write_le<uint16_t>(*seq_no | 0x8000, tx_buf_);
+
+            FIBRE_LOG(D) << "send packet: " << as_hex(cbufptr_t{tx_buf_, actual_response_length});
+            tx_channel_->start_write({tx_buf_, actual_response_length}, &tx_handle_, MEMBER_CB(this, on_write_finished));
+        }
+#else
+        FIBRE_LOG(W) << "received request but server support is not compiled in";
+#endif
+    }
+
+    rx_channel_->start_read(rx_buf_, &dummy, MEMBER_CB(this, on_read_finished));
+}
+
+void LegacyProtocolPacketBased::on_rx_closed(StreamStatus status) {
+    if (tx_handle_) {
+        // TX operation still in progress - cancel TX operation and defer closing
+        // the protocol instance until the TX operation has finished.
+        rx_status_ = status;
+        tx_channel_->cancel_write(tx_handle_);
+    } else {
+        // No TX operation in progress - close protocol instance immediately.
+        on_rx_tx_closed(status);
+    }
+}
+
+void LegacyProtocolPacketBased::on_rx_tx_closed(StreamStatus status) {
+    if (status == kStreamClosed || status == kStreamCancelled) {
+        // TODO: handle app-initiated cancellation via cancel_endpoint_operation() (currently unused)
+        status = kStreamError;
+    }
+
+#if FIBRE_ENABLE_CLIENT
+    // Cancel pending endpoint operation
+    for (auto& op: pending_operations_) {
+        op.callback.invoke_and_clear({status, op.tx_buf.begin(), op.rx_buf.begin()});
+    }
+    pending_operations_.clear();
+
+    // Cancel all ongoing endpoint operations
+    for (auto& item: expected_acks_) {
+        if (item.second.callback) {
+            item.second.callback.invoke_and_clear({status, item.second.tx_buf.begin(), item.second.rx_buf.begin()});
+        }
+    }
+    expected_acks_.clear();
+
+    // Report that the root object was lost
+    if (client_.on_lost_root_object_ && client_.root_obj_) {
+        auto root_obj = client_.root_obj_;
+        client_.root_obj_ = nullptr;
+        client_.on_lost_root_object_.invoke(&client_, root_obj);
+    }
+#endif
+    on_stopped_.invoke_and_clear(this, status);
+}
+
+#if FIBRE_ENABLE_CLIENT
+void LegacyProtocolPacketBased::start(Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_found_root_object, Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_lost_root_object, Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped) {
+#else
+void LegacyProtocolPacketBased::start(Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped) {
+#endif
+    on_stopped_ = on_stopped;
+    TransferHandle dummy;
+    rx_channel_->start_read(rx_buf_, &dummy, MEMBER_CB(this, on_read_finished));
+
+#if FIBRE_ENABLE_CLIENT
+    if (on_stopped_) {
+        client_.start(on_found_root_object, on_lost_root_object);
+    }
+#endif
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/legacy_protocol.hpp

@@ -0,0 +1,173 @@
+#ifndef __FIBRE_LEGACY_PROTOCOL_HPP
+#define __FIBRE_LEGACY_PROTOCOL_HPP
+
+#include <fibre/async_stream.hpp>
+
+#ifdef FIBRE_ENABLE_CLIENT
+#include "legacy_object_client.hpp"
+#include <unordered_map>
+#include <optional>
+#include <queue>
+#endif
+
+namespace fibre {
+
+// Default CRC-8 Polynomial: x^8 + x^5 + x^4 + x^2 + x + 1
+// Can protect a 4 byte payload against toggling of up to 5 bits
+//  source: https://users.ece.cmu.edu/~koopman/crc/index.html
+constexpr uint8_t CANONICAL_CRC8_POLYNOMIAL = 0x37;
+constexpr uint8_t CANONICAL_CRC8_INIT = 0x42;
+
+// Default CRC-16 Polynomial: 0x9eb2 x^16 + x^13 + x^12 + x^11 + x^10 + x^8 + x^6 + x^5 + x^2 + 1
+// Can protect a 135 byte payload against toggling of up to 5 bits
+//  source: https://users.ece.cmu.edu/~koopman/crc/index.html
+// Also known as CRC-16-DNP
+constexpr uint16_t CANONICAL_CRC16_POLYNOMIAL = 0x3d65;
+constexpr uint16_t CANONICAL_CRC16_INIT = 0x1337;
+
+constexpr uint8_t CANONICAL_PREFIX = 0xAA;
+
+constexpr uint16_t PROTOCOL_VERSION = 1;
+
+
+class PacketWrapper : public AsyncStreamSink {
+public:
+    PacketWrapper(AsyncStreamSink* tx_channel)
+        : tx_channel_(tx_channel) {}
+
+    void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final;
+    void cancel_write(TransferHandle transfer_handle) final;
+
+private:
+    void complete(WriteResult result);
+
+    AsyncStreamSink* tx_channel_;
+    TransferHandle inner_transfer_handle_;
+    uint8_t header_buf_[3];
+    uint8_t trailer_buf_[2];
+    const uint8_t* expected_tx_end_;
+    cbufptr_t payload_buf_ = {nullptr, nullptr};
+    Callback<void, WriteResult> completer_;
+
+    enum {
+        kStateIdle,
+        kStateCancelling,
+        kStateSendingHeader,
+        kStateSendingPayload,
+        kStateSendingTrailer
+    } state_ = kStateIdle;
+};
+
+
+class PacketUnwrapper : public AsyncStreamSource {
+public:
+    PacketUnwrapper(AsyncStreamSource* rx_channel)
+        : rx_channel_(rx_channel) {}
+
+    void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) final;
+    void cancel_read(TransferHandle transfer_handle) final;
+
+private:
+    void complete(ReadResult result);
+
+    AsyncStreamSource* rx_channel_;
+    TransferHandle inner_transfer_handle_;
+    uint8_t rx_buf_[3];
+    uint8_t* expected_rx_end_;
+    size_t payload_length_ = 0;
+    bufptr_t payload_buf_ = {nullptr, nullptr};
+    Callback<void, ReadResult> completer_;
+
+    enum {
+        kStateIdle,
+        kStateCancelling,
+        kStateReceivingHeader,
+        kStateReceivingPayload,
+        kStateReceivingTrailer
+    } state_ = kStateIdle;
+};
+
+
+struct LegacyProtocolPacketBased {
+public:
+    LegacyProtocolPacketBased(AsyncStreamSource* rx_channel, AsyncStreamSink* tx_channel, size_t tx_mtu)
+        : rx_channel_(rx_channel), tx_channel_(tx_channel), tx_mtu_(std::min(tx_mtu, sizeof(tx_buf_))) {}
+
+    AsyncStreamSource* rx_channel_ = nullptr;
+    AsyncStreamSink* tx_channel_ = nullptr;
+    size_t tx_mtu_;
+    uint8_t tx_buf_[128];
+    uint8_t rx_buf_[128];
+
+    TransferHandle tx_handle_ = 0; // non-zero while a TX operation is in progress
+    uint8_t* rx_end_ = nullptr; // non-zero if an RX operation has finished but wasn't handled yet because the TX channel was busy
+    StreamStatus rx_status_ = kStreamOk; // non-ok if the RX process was terminated permanently.
+                                         // This signals to the TX process that it should close
+                                         // the protocol instance at the next possible instant.
+    
+    Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped_ = nullptr;
+
+#if FIBRE_ENABLE_CLIENT
+    void start_endpoint_operation(uint16_t endpoint_id, cbufptr_t tx_buf, bufptr_t rx_buf, EndpointOperationHandle* handle, Callback<void, EndpointOperationResult> callback);
+    void cancel_endpoint_operation(EndpointOperationHandle handle);
+
+    LegacyObjectClient client_{this};
+#endif
+
+#if FIBRE_ENABLE_CLIENT
+    void start(Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_found_root_object, Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_lost_root_object, Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped);
+#else
+    void start(Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped);
+#endif
+
+private:
+
+#if FIBRE_ENABLE_CLIENT
+    struct EndpointOperation {
+        uint16_t seqno;
+        uint16_t endpoint_id;
+        cbufptr_t tx_buf;
+        bool tx_done;
+        bufptr_t rx_buf;
+        bool rx_done;
+        Callback<void, EndpointOperationResult> callback;
+    };
+
+    void start_endpoint_operation(EndpointOperation op);
+
+    uint16_t outbound_seq_no_ = 0;
+    std::vector<EndpointOperation> pending_operations_; // operations that are waiting for TX
+    EndpointOperationHandle transmitting_op_ = 0; // operation that is in TX
+    std::unordered_map<uint16_t, EndpointOperation> expected_acks_; // operations that are waiting for RX
+#endif
+
+    void on_write_finished(WriteResult result);
+    void on_read_finished(ReadResult result);
+    void on_rx_closed(StreamStatus status);
+    void on_rx_tx_closed(StreamStatus status);
+};
+
+
+struct LegacyProtocolStreamBased {
+public:
+    LegacyProtocolStreamBased(AsyncStreamSource* rx_channel, AsyncStreamSink* tx_channel)
+        : unwrapper_(rx_channel), wrapper_(tx_channel) {}
+
+
+#if FIBRE_ENABLE_CLIENT
+    void start(Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_found_root_object, Callback<void, LegacyObjectClient*, std::shared_ptr<LegacyObject>> on_lost_root_object, Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped) {
+        inner_protocol_.start(on_found_root_object, on_lost_root_object, on_stopped);
+    }
+#else
+    void start(Callback<void, LegacyProtocolPacketBased*, StreamStatus> on_stopped) { inner_protocol_.start(on_stopped); }
+#endif
+
+private:
+    PacketUnwrapper unwrapper_;
+    PacketWrapper wrapper_;
+    LegacyProtocolPacketBased inner_protocol_{&unwrapper_, &wrapper_, 127};
+};
+
+}
+
+#endif // __FIBRE_LEGACY_PROTOCOL_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/libfibre.cpp

@@ -0,0 +1,586 @@
+
+#include <fibre/libfibre.h>
+#include <fibre/fibre.hpp>
+#include "logging.hpp"
+#include "print_utils.hpp"
+#include "legacy_protocol.hpp" // TODO: remove this include
+#include "legacy_object_client.hpp" // TODO: remove this include
+#include <algorithm>
+
+DEFINE_LOG_TOPIC(LIBFIBRE);
+USE_LOG_TOPIC(LIBFIBRE);
+
+
+struct LibFibreChannelDiscoveryCtx {
+    fibre::Domain* domain;
+};
+
+LibFibreFunction* to_c(fibre::Function* ptr) {
+    return reinterpret_cast<LibFibreFunction*>(ptr);
+}
+fibre::Function* from_c(LibFibreFunction* ptr) {
+    return reinterpret_cast<fibre::Function*>(ptr);
+}
+void** from_c(LibFibreCallContext** ptr) {
+    return reinterpret_cast<void**>(ptr);
+}
+LibFibreDomain* to_c(fibre::Domain* ptr) {
+    return reinterpret_cast<LibFibreDomain*>(ptr);
+}
+fibre::Domain* from_c(LibFibreDomain* ptr) {
+    return reinterpret_cast<fibre::Domain*>(ptr);
+}
+LibFibreObject* to_c(fibre::Object* ptr) {
+    return reinterpret_cast<LibFibreObject*>(ptr);
+}
+fibre::Object* from_c(LibFibreObject* ptr) {
+    return reinterpret_cast<fibre::Object*>(ptr);
+}
+LibFibreInterface* to_c(fibre::Interface* ptr) {
+    return reinterpret_cast<LibFibreInterface*>(ptr);
+}
+fibre::Interface* from_c(LibFibreInterface* ptr) {
+    return reinterpret_cast<fibre::Interface*>(ptr);
+}
+LibFibreStatus to_c(fibre::Status status) {
+    return static_cast<LibFibreStatus>(status);
+}
+fibre::Status from_c(LibFibreStatus status) {
+    return static_cast<fibre::Status>(status);
+}
+LibFibreChannelDiscoveryCtx* to_c(fibre::ChannelDiscoveryContext* ptr) {
+    return reinterpret_cast<LibFibreChannelDiscoveryCtx*>(ptr);
+}
+fibre::ChannelDiscoveryContext* from_c(LibFibreChannelDiscoveryCtx* ptr) {
+    return reinterpret_cast<fibre::ChannelDiscoveryContext*>(ptr);
+}
+
+
+static const struct LibFibreVersion libfibre_version = { 0, 1, 4 };
+
+class FIBRE_PRIVATE ExternalEventLoop final : public fibre::EventLoop {
+public:
+    ExternalEventLoop(LibFibreEventLoop impl) : impl_(impl) {}
+
+    bool post(fibre::Callback<void> callback) final {
+        return impl_.post && ((*impl_.post)(callback.get_ptr(), callback.get_ctx()) == 0);
+    }
+
+    bool register_event(int event_fd, uint32_t events, fibre::Callback<void, uint32_t> callback) final {
+        return impl_.register_event && ((*impl_.register_event)(event_fd, events, callback.get_ptr(), callback.get_ctx()) == 0);
+    }
+
+    bool deregister_event(int event_fd) final {
+        return impl_.deregister_event && ((*impl_.deregister_event)(event_fd) == 0);
+    }
+
+    struct fibre::EventLoopTimer* call_later(float delay, fibre::Callback<void> callback) final {
+        if (!impl_.call_later) {
+            return nullptr;
+        }
+        return (fibre::EventLoopTimer*)(*impl_.call_later)(delay, callback.get_ptr(), callback.get_ctx());
+    }
+
+    bool cancel_timer(struct fibre::EventLoopTimer* timer) final {
+        return impl_.cancel_timer && ((*impl_.cancel_timer)((EventLoopTimer*)timer) == 0);
+    }
+
+private:
+    LibFibreEventLoop impl_;
+};
+
+class ExternalDiscoverer : public fibre::ChannelDiscoverer {
+    void start_channel_discovery(
+        fibre::Domain* domain,
+        const char* specs, size_t specs_len,
+        fibre::ChannelDiscoveryContext** handle) final;
+    int stop_channel_discovery(fibre::ChannelDiscoveryContext* handle) final;
+public:
+    on_start_discovery_cb_t on_start_discovery;
+    on_stop_discovery_cb_t on_stop_discovery;
+    void* cb_ctx;
+};
+
+
+void ExternalDiscoverer::start_channel_discovery(fibre::Domain* domain, const char* specs, size_t specs_len, fibre::ChannelDiscoveryContext** handle) {
+    LibFibreChannelDiscoveryCtx* ctx = new LibFibreChannelDiscoveryCtx{};
+    if (handle) {
+        *handle = from_c(ctx);
+    }
+    if (on_start_discovery) {
+        (*on_start_discovery)(cb_ctx, to_c(domain), specs, specs_len);
+    }
+}
+
+int ExternalDiscoverer::stop_channel_discovery(fibre::ChannelDiscoveryContext* handle) {
+    LibFibreChannelDiscoveryCtx* ctx = to_c(handle);
+    if (on_stop_discovery) {
+        (*on_stop_discovery)(cb_ctx, to_c(ctx->domain));
+    }
+    delete ctx;
+    return 0;
+}
+
+namespace fibre {
+
+class AsyncStreamLink final : public AsyncStreamSink, public AsyncStreamSource {
+public:
+    void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final;
+    void cancel_write(TransferHandle transfer_handle) final;
+    void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) final;
+    void cancel_read(TransferHandle transfer_handle) final;
+    void close(StreamStatus status);
+
+    Callback<void, ReadResult> read_completer_;
+    bufptr_t read_buf_;
+    Callback<void, WriteResult> write_completer_;
+    cbufptr_t write_buf_;
+};
+
+void AsyncStreamLink::start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) {
+    if (read_completer_) {
+        size_t n_copy = std::min(read_buf_.size(), buffer.size());
+        memcpy(read_buf_.begin(), buffer.begin(), n_copy);
+        read_completer_.invoke_and_clear({kStreamOk, read_buf_.begin() + n_copy});
+        completer.invoke({kStreamOk, buffer.begin() + n_copy});
+    } else {
+        if (handle) {
+            *handle = reinterpret_cast<uintptr_t>(this);
+        }
+        write_buf_ = buffer;
+        write_completer_ = completer;
+    }
+}
+
+void AsyncStreamLink::cancel_write(TransferHandle transfer_handle) {
+    write_completer_.invoke_and_clear({kStreamCancelled, write_buf_.begin()});
+}
+
+void AsyncStreamLink::start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) {
+    if (write_completer_) {
+        size_t n_copy = std::min(buffer.size(), write_buf_.size());
+        memcpy(buffer.begin(), write_buf_.begin(), n_copy);
+        write_completer_.invoke_and_clear({kStreamOk, write_buf_.begin() + n_copy});
+        completer.invoke({kStreamOk, buffer.begin() + n_copy});
+    } else {
+        if (handle) {
+            *handle = reinterpret_cast<uintptr_t>(this);
+        }
+        read_buf_ = buffer;
+        read_completer_ = completer;
+    }
+}
+
+void AsyncStreamLink::cancel_read(TransferHandle transfer_handle) {
+    read_completer_.invoke_and_clear({kStreamCancelled, read_buf_.begin()});
+}
+
+void AsyncStreamLink::close(StreamStatus status) {
+    write_completer_.invoke_and_clear({status, write_buf_.begin()});
+    read_completer_.invoke_and_clear({status, read_buf_.begin()});
+}
+
+}
+
+LibFibreStatus convert_status(fibre::StreamStatus status) {
+    switch (status) {
+        case fibre::kStreamOk: return kFibreOk;
+        case fibre::kStreamCancelled: return kFibreCancelled;
+        case fibre::kStreamClosed: return kFibreClosed;
+        default: return kFibreInternalError; // TODO: this may not always be appropriate
+    }
+}
+
+fibre::StreamStatus convert_status(LibFibreStatus status) {
+    switch (status) {
+        case kFibreOk: return fibre::kStreamOk;
+        case kFibreCancelled: return fibre::kStreamCancelled;
+        case kFibreClosed: return fibre::kStreamClosed;
+        default: return fibre::kStreamError; // TODO: this may not always be appropriate
+    }
+}
+
+struct FIBRE_PRIVATE LibFibreCtx {
+    ExternalEventLoop* event_loop;
+    //size_t n_discoveries = 0;
+    fibre::Context* fibre_ctx;
+    //std::unordered_map<std::string, std::shared_ptr<fibre::ChannelDiscoverer>> discoverers;
+};
+
+struct FIBRE_PRIVATE LibFibreDiscoveryCtx {
+    void on_found_object(fibre::Object* obj, fibre::Interface* intf);
+    void on_lost_object(fibre::Object* obj);
+
+    on_found_object_cb_t on_found_object_;
+    on_lost_object_cb_t on_lost_object_;
+    void* cb_ctx_;
+    fibre::Domain* domain_;
+};
+
+struct LibFibreTxStream {
+    void on_tx_done(fibre::WriteResult result) {
+        if (on_completed) {
+            (*on_completed)(ctx, this, convert_status(result.status), result.end);
+        }
+    }
+
+    fibre::AsyncStreamSink* sink;
+    fibre::TransferHandle handle;
+    on_tx_completed_cb_t on_completed;
+    void* ctx;
+    void (*on_closed)(LibFibreTxStream*, void*, fibre::StreamStatus);
+    void* on_closed_ctx;
+};
+
+struct LibFibreRxStream {
+    void on_rx_done(fibre::ReadResult result) {
+        if (on_completed) {
+            (*on_completed)(ctx, this, convert_status(result.status), result.end);
+        }
+    }
+
+    fibre::AsyncStreamSource* source;
+    fibre::TransferHandle handle;
+    on_rx_completed_cb_t on_completed;
+    void* ctx;
+    void (*on_closed)(LibFibreRxStream*, void*, fibre::StreamStatus);
+    void* on_closed_ctx;
+};
+
+
+void LibFibreDiscoveryCtx::on_found_object(fibre::Object* obj, fibre::Interface* intf) {
+    if (on_found_object_) {
+        FIBRE_LOG(D) << "discovered object " << fibre::as_hex(reinterpret_cast<uintptr_t>(obj));
+        (*on_found_object_)(cb_ctx_, to_c(obj), to_c(intf));
+    }
+}
+
+void LibFibreDiscoveryCtx::on_lost_object(fibre::Object* obj) {
+    if (on_lost_object_) {
+        FIBRE_LOG(D) << "lost object " << fibre::as_hex(reinterpret_cast<uintptr_t>(obj));
+        (*on_lost_object_)(cb_ctx_, to_c(obj));
+    }
+}
+
+const struct LibFibreVersion* libfibre_get_version() {
+    return &libfibre_version;
+}
+
+LibFibreCtx* libfibre_open(LibFibreEventLoop event_loop) {
+    LibFibreCtx* ctx = new LibFibreCtx();
+    ctx->event_loop = new ExternalEventLoop(event_loop);
+    ctx->fibre_ctx = fibre::open(ctx->event_loop);
+
+    if (!ctx->fibre_ctx) {
+        FIBRE_LOG(E) << "fibre::open failed";
+        delete ctx->event_loop;
+        delete ctx;
+        return nullptr;
+    }
+
+    return ctx;
+}
+
+void libfibre_close(LibFibreCtx* ctx) {
+    if (!ctx) {
+        FIBRE_LOG(E) << "invalid argument";
+        return;
+    }
+
+    fibre::close(ctx->fibre_ctx);
+    ctx->fibre_ctx = nullptr;
+
+    delete ctx->event_loop;
+    delete ctx;
+
+    FIBRE_LOG(D) << "closed (" << fibre::as_hex((uintptr_t)ctx) << ")";
+}
+
+
+void libfibre_register_backend(LibFibreCtx* ctx, const char* name, size_t name_length, on_start_discovery_cb_t on_start_discovery, on_stop_discovery_cb_t on_stop_discovery, void* cb_ctx) {
+    auto disc = new ExternalDiscoverer();
+    disc->on_start_discovery = on_start_discovery;
+    disc->on_stop_discovery = on_stop_discovery;
+    disc->cb_ctx = cb_ctx;
+    ctx->fibre_ctx->register_backend({name, name + name_length}, disc);
+}
+
+FIBRE_PUBLIC LibFibreDomain* libfibre_open_domain(LibFibreCtx* ctx,
+    const char* specs, size_t specs_len) {
+    if (!ctx) {
+        FIBRE_LOG(E) << "invalid context";
+        return nullptr;
+    } else {
+        FIBRE_LOG(D) << "opening domain";
+        return to_c(ctx->fibre_ctx->create_domain({specs, specs_len}));
+    }
+}
+
+void libfibre_close_domain(LibFibreDomain* domain) {
+    if (!domain) {
+        FIBRE_LOG(E) << "invalid domain";
+        return;
+    }
+    FIBRE_LOG(D) << "closing domain";
+
+    from_c(domain)->ctx->close_domain(from_c(domain));
+}
+
+void libfibre_add_channels(LibFibreDomain* domain, LibFibreRxStream** tx_channel, LibFibreTxStream** rx_channel, size_t mtu) {
+    fibre::AsyncStreamLink* tx_link = new fibre::AsyncStreamLink(); // libfibre => backend
+    fibre::AsyncStreamLink* rx_link = new fibre::AsyncStreamLink(); // backend => libfibre
+    LibFibreRxStream* tx = new LibFibreRxStream(); // libfibre => backend
+    LibFibreTxStream* rx = new LibFibreTxStream(); // backend => libfibre
+    tx->source = tx_link;
+    rx->sink = rx_link;
+
+    tx->on_closed = [](LibFibreRxStream* stream, void* ctx, fibre::StreamStatus status) {
+        auto link = reinterpret_cast<fibre::AsyncStreamLink*>(ctx);
+        link->close(status);
+        delete link;
+        delete stream;
+    };
+    tx->on_closed_ctx = tx_link;
+    rx->on_closed = [](LibFibreTxStream* stream, void* ctx, fibre::StreamStatus status) {
+        auto link = reinterpret_cast<fibre::AsyncStreamLink*>(ctx);
+        link->close(status);
+        delete link;
+        delete stream;
+    };
+    rx->on_closed_ctx = rx_link;
+
+    if (tx_channel) {
+        *tx_channel = tx;
+    }
+
+    if (rx_channel) {
+        *rx_channel = rx;
+    }
+
+    fibre::ChannelDiscoveryResult result = {fibre::kFibreOk, rx_link, tx_link, mtu};
+    from_c(domain)->add_channels(result);
+}
+
+void libfibre_start_discovery(LibFibreDomain* domain, LibFibreDiscoveryCtx** handle,
+        on_found_object_cb_t on_found_object, on_lost_object_cb_t on_lost_object,
+        on_stopped_cb_t on_stopped, void* cb_ctx) {
+    if (!domain) {
+        FIBRE_LOG(E) << "invalid argument";
+        if (on_stopped) {
+            (*on_stopped)(cb_ctx, kFibreInvalidArgument);
+        }
+        return;
+    }
+
+    // deleted in libfibre_stop_discovery()
+    LibFibreDiscoveryCtx* discovery_ctx = new LibFibreDiscoveryCtx();
+    discovery_ctx->on_found_object_ = on_found_object;
+    discovery_ctx->on_lost_object_ = on_lost_object;
+    discovery_ctx->cb_ctx_ = cb_ctx;
+    discovery_ctx->domain_ = from_c(domain);
+
+    if (handle) {
+        *handle = discovery_ctx;
+    }
+
+    from_c(domain)->start_discovery(MEMBER_CB(discovery_ctx, on_found_object),
+        MEMBER_CB(discovery_ctx, on_lost_object));
+}
+
+void libfibre_stop_discovery(LibFibreDiscoveryCtx* handle) {
+    if (!handle) {
+        FIBRE_LOG(E) << "bad handle";
+        return;
+    }
+
+    handle->domain_->stop_discovery();
+    delete handle;
+}
+
+
+void libfibre_subscribe_to_interface(LibFibreInterface* interface,
+        on_attribute_added_cb_t on_attribute_added,
+        on_attribute_removed_cb_t on_attribute_removed,
+        on_function_added_cb_t on_function_added,
+        on_function_removed_cb_t on_function_removed,
+        void* cb_ctx)
+{
+    auto intf = reinterpret_cast<fibre::FibreInterface*>(interface); // corresponding reverse cast in LibFibreDiscoveryCtx::complete() and libfibre_subscribe_to_interface()
+
+    for (auto& func: intf->functions) {
+        std::vector<const char*> input_names = {"obj"};
+        std::vector<const char*> input_codecs = {"object_ref"};
+        std::vector<const char*> output_names;
+        std::vector<const char*> output_codecs;
+        for (auto& arg: func.second.inputs) {
+            input_names.push_back(arg.name.data());
+            input_codecs.push_back(arg.app_codec.data());
+        }
+        for (auto& arg: func.second.outputs) {
+            output_names.push_back(arg.name.data());
+            output_codecs.push_back(arg.app_codec.data());
+        }
+        input_names.push_back(nullptr);
+        input_codecs.push_back(nullptr);
+        output_names.push_back(nullptr);
+        output_codecs.push_back(nullptr);
+
+        if (on_function_added) {
+            (*on_function_added)(cb_ctx,
+                to_c(&func.second),
+                func.first.data(), func.first.size(),
+                input_names.data(), input_codecs.data(),
+                output_names.data(), output_codecs.data());
+        }
+    }
+
+    for (auto& attr: intf->attributes) {
+        if (on_attribute_added) {
+            (*on_attribute_added)(cb_ctx,
+                reinterpret_cast<LibFibreAttribute*>(&attr.second), // corresponding reverse cast in libfibre_get_attribute()
+                attr.first.data(), attr.first.size(),
+                reinterpret_cast<LibFibreInterface*>(attr.second.object->intf.get()), // corresponding reverse cast in libfibre_subscribe_to_interface()
+                attr.second.object->intf->name.size() ? attr.second.object->intf->name.data() : nullptr, attr.second.object->intf->name.size()
+            );
+        }
+    }
+}
+
+LibFibreStatus libfibre_get_attribute(LibFibreObject* parent_obj, LibFibreAttribute* attr, LibFibreObject** child_obj_ptr) {
+    if (!parent_obj || !attr) {
+        return kFibreInvalidArgument;
+    }
+
+    fibre::LegacyObject* parent_obj_cast = reinterpret_cast<fibre::LegacyObject*>(parent_obj);
+    fibre::LegacyFibreAttribute* attr_cast = reinterpret_cast<fibre::LegacyFibreAttribute*>(attr); // corresponding reverse cast in libfibre_subscribe_to_interface()
+    auto& attributes = parent_obj_cast->intf->attributes;
+
+    bool is_member = std::find_if(attributes.begin(), attributes.end(),
+        [&](std::pair<const std::string, fibre::LegacyFibreAttribute>& kv) {
+            return &kv.second == attr_cast;
+        }) != attributes.end();
+
+    if (!is_member) {
+        FIBRE_LOG(W) << "attempt to fetch attribute from an object that does not implement it";
+        return kFibreInvalidArgument;
+    }
+
+    //LibFibreCtx* libfibre_ctx = reinterpret_cast<LibFibreCtx*>(parent_obj_cast->client->user_data_);
+    fibre::LegacyObject* child_obj = attr_cast->object.get();
+
+    if (!attr_cast->object->known_to_application) {
+        attr_cast->object->known_to_application = true;
+
+        //if (libfibre_ctx->on_construct_object) {
+        //    //FIBRE_LOG(D) << "constructing subobject " << fibre::as_hex(reinterpret_cast<uintptr_t>(child_obj));
+        //    (*libfibre_ctx->on_construct_object)(libfibre_ctx->cb_ctx,
+        //        reinterpret_cast<LibFibreObject*>(child_obj),
+        //        reinterpret_cast<LibFibreInterface*>(child_obj->intf.get()),
+        //        child_obj->intf->name.size() ? child_obj->intf->name.data() : nullptr, child_obj->intf->name.size());
+        //}
+    }
+
+    if (child_obj_ptr) {
+        *child_obj_ptr = reinterpret_cast<LibFibreObject*>(child_obj);
+    }
+
+    return kFibreOk;
+}
+
+/**
+ * @brief Inserts or removes the specified number of elements
+ * @param delta: Positive value: insert elements, negative value: remove elements
+ */
+void resize_at(std::vector<uint8_t>& vec, size_t pos, ssize_t delta) {
+    if (delta > 0) {
+        std::fill_n(std::inserter(vec, vec.begin() + pos), delta, 0);
+    } else {
+        vec.erase(std::min(vec.begin() + pos, vec.end()),
+                  std::min(vec.begin() + pos + -delta, vec.end()));
+    }
+}
+
+LibFibreStatus libfibre_call(LibFibreFunction* func, LibFibreCallContext** handle,
+        LibFibreStatus status,
+        const unsigned char* tx_buf, size_t tx_len,
+        unsigned char* rx_buf, size_t rx_len,
+        const unsigned char** tx_end,
+        unsigned char** rx_end,
+        libfibre_call_cb_t callback, void* cb_ctx) {
+    bool valid_args = func && handle
+                   && (!tx_len || tx_buf) // tx_buf valid
+                   && (!rx_len || rx_buf) // rx_buf valid
+                   && tx_end && rx_end // tx_end, rx_end valid
+                   && ((status != kFibreOk) || tx_len || rx_len || !handle); // progress
+    if (!valid_args) {
+        FIBRE_LOG(E) << "invalid argument";
+        return kFibreInvalidArgument;
+    }
+
+    struct Ctx { libfibre_call_cb_t callback; void* ctx; };
+    struct Ctx* ctx = new Ctx{callback, cb_ctx};
+
+    fibre::Callback<std::optional<fibre::CallBuffers>, fibre::CallBufferRelease> cb{
+        [](void* ctx_, fibre::CallBufferRelease result) -> std::optional<fibre::CallBuffers> {
+            auto ctx = reinterpret_cast<Ctx*>(ctx_);
+            const unsigned char* tx_buf;
+            size_t tx_len;
+            unsigned char* rx_buf;
+            size_t rx_len;
+            auto status = ctx->callback(ctx->ctx, to_c(result.status), result.tx_end, result.rx_end, &tx_buf, &tx_len, &rx_buf, &rx_len);
+            if (status == kFibreBusy) {
+                delete ctx;
+                return std::nullopt;
+            } else {
+                return fibre::CallBuffers{from_c(status), {tx_buf, tx_len}, {rx_buf, rx_len}};
+            }
+    }, ctx};
+
+
+    auto response = from_c(func)->call(from_c(handle), {from_c(status), {tx_buf, tx_len}, {rx_buf, rx_len}}, cb);
+
+    if (!response.has_value()) {
+        return kFibreBusy;
+    } else {
+        delete ctx;
+        *tx_end = response->tx_end;
+        *rx_end = response->rx_end;
+        return to_c(response->status);
+    }
+}
+
+void libfibre_start_tx(LibFibreTxStream* tx_stream,
+        const uint8_t* tx_buf, size_t tx_len, on_tx_completed_cb_t on_completed,
+        void* ctx) {
+    tx_stream->on_completed = on_completed;
+    tx_stream->ctx = ctx;
+    tx_stream->sink->start_write({tx_buf, tx_len}, &tx_stream->handle, MEMBER_CB(tx_stream, on_tx_done));
+}
+
+void libfibre_cancel_tx(LibFibreTxStream* tx_stream) {
+    tx_stream->sink->cancel_write(tx_stream->handle);
+}
+
+void libfibre_close_tx(LibFibreTxStream* tx_stream, LibFibreStatus status) {
+    if (tx_stream->on_closed) {
+        (tx_stream->on_closed)(tx_stream, tx_stream->on_closed_ctx, convert_status(status));
+    }
+}
+
+void libfibre_start_rx(LibFibreRxStream* rx_stream,
+        uint8_t* rx_buf, size_t rx_len, on_rx_completed_cb_t on_completed,
+        void* ctx) {
+    rx_stream->on_completed = on_completed;
+    rx_stream->ctx = ctx;
+    rx_stream->source->start_read({rx_buf, rx_len}, &rx_stream->handle, MEMBER_CB(rx_stream, on_rx_done));
+}
+
+void libfibre_cancel_rx(LibFibreRxStream* rx_stream) {
+    rx_stream->source->cancel_read(rx_stream->handle);
+}
+
+void libfibre_close_rx(LibFibreRxStream* rx_stream, LibFibreStatus status) {
+    if (rx_stream->on_closed) {
+        (rx_stream->on_closed)(rx_stream, rx_stream->on_closed_ctx, convert_status(status));
+    }
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/libfibre.version

@@ -0,0 +1,4 @@
+LIBFIBREABI_0.1.0 {
+    global: libfibre_*;
+    local: *;
+};

ODrive-fw-v0.5.6/Firmware/fibre-cpp/logging.cpp

@@ -0,0 +1,20 @@
+
+#include "logging.hpp"
+
+#if !defined(_WIN32) && !defined(_WIN64) && !defined(__linux__) && !defined(__APPLE__) && !defined(EMSCRIPTEN)
+
+namespace std {
+StdoutStream cerr;
+}
+
+#endif
+
+namespace fibre {
+
+Logger logger{};
+
+Logger* get_logger() {
+    return &logger;
+}
+
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/logging.hpp

@@ -0,0 +1,341 @@
+/**
+ * @brief Provides logging facilities
+ * 
+ * Log entries are associated with user defined topics. A user can define a
+ * topic using DEFINE_LOG_TOPIC(topicname) and activate the topic for the
+ * current scope using USE_LOG_TOPIC(topicname).
+ * 
+ * Currently all log entries are posted to stderr in a thread-safe way.
+ *
+ * Whether an event is actually logged depends on the current log verbosity of
+ * the corresponding topic. The log verbosity is defined by the following
+ * sources (in order of their precedence).
+ * 
+ *  0. maximum log verbosity setting (see below)
+ *  1. runtime environment variable "FIBRE_LOG_[topicname]"
+ *  2. runtime environment variable "FIBRE_LOG"
+ *  3. topic specific default log verbosity defined using CONFIG_LOG_TOPIC(...)
+ *  4. FIBRE_DEFAULT_LOG_VERBOSITY defined before this file (using #define or -D compiler flag)
+ *  5. FIBRE_DEFAULT_LOG_VERBOSITY defined in this file
+ * 
+ * The maximum log verbosity can be defined separately from the default log
+ * verbosity. The maximum log verbosity bound always applies, regardless of how
+ * the actual log verbosity is specified. This allows keeping the binary small
+ * by optimizing away unnecessary log entries at compile time.
+ * The maximum log verbosity is defined by the following sources (in order of
+ * their precedence):
+ * 
+ *  1. topic specific max log verbosity defined using CONFIG_LOG_TOPIC(...)
+ *  2. FIBRE_MAX_LOG_VERBOSITY defined before this file (using #define or -D compiler flag)
+ *  3. FIBRE_MAX_LOG_VERBOSITY defined in this file
+ * 
+ * TODO: ensure that the optimizer can indeed strip the unused strings (currently not the case)
+ * 
+ * 
+ * Example:
+ * 
+ * @code
+ * 
+ * DEFINE_LOG_TOPIC(MAIN);
+ * USE_LOG_TOPIC(MAIN);
+ * 
+ * int main(void) {
+ *     FIBRE_LOG(D) << "Hello Log!";
+ *     if (open("inexistent_file", O_RDONLY) < 0) {
+ *         FIBRE_LOG(E) << "Could not open file: " << sys_err();
+ *     }
+ *     return 0;
+ * }
+ * 
+ * @endcode
+ * 
+ * Using logging in header files is possible but undocumented (TODO: fix)
+ */
+
+#ifndef __FIBRE_LOGGING_HPP
+#define __FIBRE_LOGGING_HPP
+
+/**
+ * @brief Tag type to print the last system error
+ * 
+ * The statement `std::out << sys_err();` will print the last system error
+ * in the following format: "error description (errno)".
+ * This is based on `GetLastError()` (Windows) or `errno` (all other systems).
+ */
+struct sys_err {};
+
+
+// TODO: support lite-version of logging on embedded systems
+#if FIBRE_MAX_LOG_VERBOSITY
+
+#include <fibre/cpp_utils.hpp>
+
+#include <string.h>
+#include <chrono>
+
+#if defined(_WIN32) || defined(_WIN64)
+#include "windows.h"
+#endif
+
+#if defined(_WIN32) || defined(_WIN64) || defined(__linux__) || defined(__APPLE__) || defined(EMSCRIPTEN)
+#include <iostream>
+#include <iomanip>
+#else
+
+// We don't want <iostream> included on an embedded system as it makes the
+// binary huge.
+
+struct StdoutStream : std::ostream {
+    void operator <<(const char * str) {
+        printf("%s", str);
+    }
+};
+
+namespace std {
+extern StdoutStream cerr;
+}
+
+#endif
+
+#if defined(_WIN32) || defined(_WIN64) || defined(__linux__)
+
+#include <mutex>
+using TMutex = std::mutex;
+using TLock = std::unique_lock<TMutex>;
+
+#else
+
+using TMutex = int;
+struct TLock {
+    TLock() {}
+    TLock(TMutex) {}
+};
+
+#endif
+
+namespace fibre {
+
+// Maximum log verbosity that should be compiled into the binary.
+// Log entries with a higher verbosity should be optimized away.
+#ifndef FIBRE_MAX_LOG_VERBOSITY
+#  define FIBRE_MAX_LOG_VERBOSITY LOG_LEVEL_T
+#endif
+
+// Default log verbosity that should be used for all topic. This may be
+// overridden by other sources, see description in the beginning of this file.
+#ifndef FIBRE_DEFAULT_LOG_VERBOSITY
+#  define FIBRE_DEFAULT_LOG_VERBOSITY LOG_LEVEL_W
+#endif
+
+/**
+ * @brief Generates one log entry.
+ * 
+ * The log entry will be associates with the topic specified in "USE_LOG_TOPIC".
+ * 
+ * Note that the log entry must only be used in the statement it is generated. (TODO: fix)
+ * 
+ * @param level: Can be one of "E", "W", "D", or other levels defined in
+ *        log_level_t.
+ * @returns a stream for writing into the log entry
+ */
+#define FIBRE_LOG(level) \
+    fibre::make_log_entry<current_log_topic, fibre::LOG_LEVEL_ ## level>( \
+        fibre::get_file_name(MAKE_SSTRING(__FILE__){}), __LINE__, __func__ \
+    ).get_stream()
+
+/**
+ * @brief Defines a log topic. A log topic must be defined exactly once in every
+ * translation unit it is used.
+ */
+#define DEFINE_LOG_TOPIC(name) \
+    struct LOG_TOPIC_ ## name { \
+        static const char * get_label() { \
+            static const char label[] = #name; \
+            return label; \
+        } \
+    }
+
+/**
+ * @brief Activates the use of the specified log topic for the current scope
+ * (and all subscopes)
+ */
+#define USE_LOG_TOPIC(name) using current_log_topic = LOG_TOPIC_ ## name
+
+/**
+ * @brief Overrides the general log verbosity settings for a specific topic.
+ * If used, this should be placed in the same scope as the corresponding
+ * DEFINE_LOG_TOPIC.
+ */
+#define CONFIG_LOG_TOPIC(topic, default_verbosity, max_verbosity) \
+template<> constexpr log_level_t get_default_log_verbosity<LOG_TOPIC_ ## topic>() { return (default_verbosity); } \
+template<> constexpr log_level_t get_max_log_verbosity<LOG_TOPIC_ ## topic>() { return (max_verbosity); }
+
+
+/** @brief Log verbosity levels */
+enum log_level_t {
+    LOG_LEVEL_F = 0, // fatal
+    LOG_LEVEL_E = 1, // error
+    LOG_LEVEL_W = 2, // warning
+    LOG_LEVEL_I = 3, // info
+    LOG_LEVEL_D = 4, // debug
+    LOG_LEVEL_T = 5, // trace
+};
+
+class NullBuffer : public std::streambuf {
+public:
+    int overflow(int c) { return c; }
+};
+
+// Source: https://stackoverflow.com/questions/15845505/how-to-get-higher-precision-fractions-of-a-second-in-a-printout-of-current-tim
+static std::string get_local_time() {
+  auto now(std::chrono::system_clock::now());
+  auto seconds_since_epoch(
+      std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()));
+
+  // Construct time_t using 'seconds_since_epoch' rather than 'now' since it is
+  // implementation-defined whether the value is rounded or truncated.
+  std::time_t now_t(
+      std::chrono::system_clock::to_time_t(
+          std::chrono::system_clock::time_point(seconds_since_epoch)));
+
+  char temp[10];
+  if (!std::strftime(temp, 10, "%H:%M:%S.", std::localtime(&now_t)))
+    return "";
+
+  return std::string(temp) +
+      std::to_string((now.time_since_epoch() - seconds_since_epoch).count());
+}
+
+class Logger {
+public:
+    class Entry {
+    public:
+        Entry() : base_stream_(null_stream), lock_() {}
+
+        Entry(std::ostream& base_stream, log_level_t level, const char* topic, const char* filename, size_t line_no, const char *funcname, TMutex& mutex)
+            : base_stream_(base_stream), lock_(mutex)
+        {
+            switch (level) {
+            case LOG_LEVEL_W:
+                base_stream << "\x1b[93;1m";
+                break;
+            case LOG_LEVEL_E:
+            case LOG_LEVEL_F:
+                base_stream << "\x1b[91;1m";
+                break;
+            default:
+                break;
+            }
+            base_stream << get_local_time() << " ";
+            base_stream << std::dec << "[" << topic << "] ";
+            //base_stream << std::dec << filename << ":" << line_no << " in " << funcname << "(): ";
+        }
+        ~Entry() { get_stream() << "\x1b[0m" << std::endl; }
+        std::ostream& get_stream() { return base_stream_; };
+    private:
+        NullBuffer null_buffer{};
+        std::ostream null_stream{&null_buffer};
+        std::ostream& base_stream_;
+        TLock lock_;
+    };
+    
+    TMutex mutex_;
+};
+
+
+
+template<typename TOPIC>
+constexpr log_level_t get_default_log_verbosity() { return (log_level_t)FIBRE_DEFAULT_LOG_VERBOSITY; }
+
+template<typename TOPIC>
+constexpr log_level_t get_max_log_verbosity() { return (log_level_t)FIBRE_MAX_LOG_VERBOSITY; }
+
+/**
+ * @brief Resolves the currently active log verbosity for the given topic.
+ * See top of this file for a detailed description of the algorithm.
+ */
+template<typename TOPIC>
+log_level_t get_current_log_verbosity() {
+    char var_name[sizeof("FIBRE_LOG_") + strlen(TOPIC::get_label())];
+    strcpy(var_name, "FIBRE_LOG_");
+    strcat(var_name, TOPIC::get_label());
+
+    // TODO: provide a way to disable the 
+    const char * var_val = std::getenv(var_name);
+    if (!var_val) {
+        var_val = std::getenv("FIBRE_LOG");
+    }
+
+    log_level_t log_level = get_default_log_verbosity<TOPIC>();
+    if (var_val) {
+        unsigned long num = strtoul(var_val, nullptr, 10);
+        log_level = (log_level_t)num;
+    }
+
+    if (log_level > get_max_log_verbosity<TOPIC>()) {
+        log_level = get_max_log_verbosity<TOPIC>();
+    }
+    return log_level;
+}
+
+
+
+/*
+template<typename TStream, typename T, typename... Ts>
+void send_to_stream(TStream&& stream);
+
+template<typename TStream>
+void send_to_stream(TStream&& stream) { }
+
+template<typename TStream, typename T, typename... Ts>
+void send_to_stream(TStream&& stream, T&& value, Ts&&... values) {
+    send_to_stream(std::forward<TStream>(stream) << std::forward<T>(value), std::forward<Ts>(values)...);
+}*/
+
+
+Logger* get_logger(); // defined in logging.cpp
+
+template<typename TOPIC, log_level_t LEVEL>
+Logger::Entry make_log_entry(const char *filename, size_t line_no, const char *funcname) {
+    if (get_current_log_verbosity<TOPIC>() < LEVEL) {
+        return {};
+    } else {
+        Logger* logger = get_logger();
+        return { std::cerr, LEVEL, TOPIC::get_label(), filename, line_no, funcname, logger->mutex_ };
+    }
+}
+
+template<typename TFilepath>
+constexpr const char * get_file_name(TFilepath file_path) {
+    return (file_path /*file_path.after_last_index_of('/')*/).c_str(); // TODO: extract file name (without path)
+}
+
+}
+
+
+namespace std {
+static inline std::ostream& operator<<(std::ostream& stream, const sys_err&) {
+#if defined(_WIN32) || defined(_WIN64)
+    auto error_code = GetLastError();
+#else
+    auto error_code = errno;
+#endif
+    return stream << strerror(error_code) << " (" << error_code << ")";
+}
+}
+
+
+#else
+
+#define DEFINE_LOG_TOPIC(topic)
+#define USE_LOG_TOPIC(topic)
+
+struct NullStream {
+    template<typename T> NullStream& operator<<(T val) { return *this; }
+};
+
+#define FIBRE_LOG(level) NullStream()
+
+#endif // FIBRE_MAX_LOG_VERBOSITY
+
+#endif // __FIBRE_LOGGING_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/package.lua

@@ -0,0 +1,143 @@
+
+fibre_root = tup.getcwd()
+
+-- Returns a table that contains the Fibre code files and the flags required to
+-- compile and link those files.
+--
+-- args: A dictionary containing the fibre options. Refer to the Compile Options
+--       in README.md for a list of available options. For example the option
+--       `FIBRE_ENABLE_SERVER` maps to the argument `args.enable_server`.
+-- In addition:
+-- args.pkgconf: Controls the use of the pkgconf or pkg-config utility that
+--     shall be used to locate build dependencies. Can be one of the following:
+--        - A string: Use the binary provided by the string. Fail if it doesn't
+--          exist.
+--        - true: Use "pkgconf" and fall back to "pkg-config" if "pkgconf"
+--          doesn't exist. Fail if both don't exist.
+--        - false: Don't use pkg-config. The user is responsible of determining
+--          the required compile and link flags.
+--        - nil: Try both "pkgconf" and "pkg-config". If both don't exist fall
+--          back to a hardcoded list of well-known settings.
+--
+-- Returns: A dictionary with the following items:
+--  code_files: A list of strings that name the C++ code files to be compiled.
+--              The names are relative to package.lua.
+--  include_dirs: A list of directories that must be added to the include path
+--                when compiling the code files. The paths are relative to
+--                package.lua.
+--  cflags: A list of flags that should be passed to the compiler/linker when
+--          compiling and linking the code files.
+--  ldflags: A list of linker flags that should be passed to the linker when
+--           linking the object files.
+function get_fibre_package(args)
+    pkg = {
+        root = fibre_root,
+        code_files = {
+            'fibre.cpp',
+            'channel_discoverer.cpp',
+        },
+        include_dirs = {'include'},
+        cflags = {},
+        ldflags = {},
+    }
+
+    -- Select a pkgconf function
+    if args.pkgconf == true or args.pkgconf == nil then
+        -- Autodetect pkgconf
+        if test_pkgconf('pkgconf') then
+            print("using pkgconf")
+            pkgconf_file = 'pkgconf'
+            pkgconf = real_pkgconf
+        elseif test_pkgconf('pkg-config') then
+            print("using pkg-config")
+            pkgconf_file = 'pkg-config'
+            pkgconf = real_pkgconf
+        elseif args.pkgconf == nil then
+            print("using hardcoded pkgconf")
+            pkgconf = hardcoded_pkgconf
+        else
+            error("couldn't find pkgconf nor pkg-config")
+        end
+
+    elseif args.pkgconf == false then
+        print("not using pkgconf")
+        pkgconf_file = nil
+        pkgconf = null_pkgconf
+    else
+        print("using pkgconf: "..args.pkgconf)
+        pkgconf_file = args.pkgconf
+        pkgconf = real_pkgconf
+    end
+
+    pkg.cflags += '-DFIBRE_ENABLE_SERVER='..(args.enable_server and '1' or '0')
+    pkg.cflags += '-DFIBRE_ENABLE_CLIENT='..(args.enable_client and '1' or '0')
+    pkg.cflags += '-DFIBRE_ENABLE_EVENT_LOOP='..(args.enable_event_loop and '1' or '0')
+    pkg.cflags += '-DFIBRE_ALLOW_HEAP='..(args.allow_heap and '1' or '0')
+    pkg.cflags += '-DFIBRE_MAX_LOG_VERBOSITY='..(args.max_log_verbosity or '5')
+    pkg.cflags += '-DFIBRE_DEFAULT_LOG_VERBOSITY='..(args.default_log_verbosity or '2')
+    pkg.cflags += '-DFIBRE_ENABLE_LIBUSB_BACKEND='..(args.enable_libusb_backend and '1' or '0')
+    pkg.cflags += '-DFIBRE_ENABLE_TCP_SERVER_BACKEND='..(args.enable_tcp_server_backend and '1' or '0')
+    pkg.cflags += '-DFIBRE_ENABLE_TCP_CLIENT_BACKEND='..(args.enable_tcp_client_backend and '1' or '0')
+
+    if args.enable_libusb_backend then
+        pkg.code_files += 'platform_support/libusb_transport.cpp'
+        pkgconf(pkg, "libusb-1.0")
+
+        -- TODO: only add pthread on linux and windows
+        pkg.ldflags += '-lpthread'
+    end
+    if args.max_log_verbosity == nil or (args.max_log_verbosity > 0) then
+        pkg.code_files += 'logging.cpp'
+    end
+    if args.enable_client then
+        pkg.code_files += 'legacy_object_client.cpp'
+    end
+    if args.enable_client or args.enable_server then
+        pkg.code_files += 'legacy_protocol.cpp'
+    end
+    if args.enable_event_loop then
+        pkg.code_files += 'platform_support/epoll_event_loop.cpp'
+    end
+    if args.enable_tcp_client_backend or args.enable_tcp_server_backend then
+        -- TODO: chose between windows and posix backend
+        pkg.code_files += 'platform_support/posix_tcp_backend.cpp'
+        pkg.code_files += 'platform_support/posix_socket.cpp'
+        pkg.ldflags += '-lanl'
+    end
+
+    return pkg
+end
+
+-- Runs the specified shell command immediately (not as part of the dependency
+-- graph).
+-- Returns the values (return_code, stdout) where stdout has the trailing new
+-- line removed.
+function fibre_run_now(command)
+    local handle
+    handle = io.popen(command)
+    local output = handle:read("*a")
+    local rc = {handle:close()}
+    return string.sub(output, 0, -2), rc[1]
+end
+
+function test_pkgconf(name)
+    local str, rc = fibre_run_now(name.." --version 2>&1 >/dev/null")
+    return rc
+end
+
+function real_pkgconf(pkg, lib)
+    pkg.cflags += fibre_run_now(pkgconf_file..' '..lib..' --cflags')
+    pkg.ldflags += fibre_run_now(pkgconf_file..' '..lib..' --libs')
+end
+
+function null_pkgconf(pkg, lib)
+    -- don't do anything
+end
+
+function hardcoded_pkgconf(pkg, lib)
+    libs = {
+        ['libusb-1.0'] = {cflags = {}, ldflags = {}},
+    }
+    tup.append_table(pkg.cflags, libs[lib].cflags)
+    tup.append_table(pkg.ldflags, libs[lib].ldflags)
+end

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/epoll_event_loop.cpp

@@ -0,0 +1,204 @@
+
+#include "epoll_event_loop.hpp"
+#include "../logging.hpp"
+
+#include <sys/epoll.h>
+#include <sys/types.h>
+#include <sys/eventfd.h>
+#include <unistd.h>
+#include <string.h>
+
+using namespace fibre;
+
+DEFINE_LOG_TOPIC(EVENT_LOOP);
+USE_LOG_TOPIC(EVENT_LOOP);
+
+
+bool EpollEventLoop::start(Callback<void> on_started) {
+    if (epoll_fd_ >= 0) {
+        FIBRE_LOG(E) << "already started";
+        return false;
+    }
+
+    epoll_fd_ = epoll_create1(0);
+    if (epoll_fd_ < 0) {
+        FIBRE_LOG(E) << "epoll_create1() failed";
+        return false;
+    }
+
+    bool ok = true;
+
+    post_fd_ = eventfd(0, 0);
+
+    bool post_fd_ok = (post_fd_ >= 0)
+            && register_event(post_fd_, EPOLLIN, MEMBER_CB(this, run_callbacks))
+            && post(on_started);
+
+    if (!post_fd_ok) {
+        FIBRE_LOG(E) << "failed to create an event for posting callbacks onto the event loop";
+        ok = false;
+    }
+
+    // Run for as long as there are callbacks pending posted or there's at least
+    // one file descriptor other than post_fd_ registerd.
+    while (pending_callbacks_.size() || (context_map_.size() > 1)) {
+        iterations_++;
+
+        do {
+            FIBRE_LOG(D) << "epoll_wait...";
+            n_triggered_events_ = epoll_wait(epoll_fd_, triggered_events_, max_triggered_events_, -1);
+            FIBRE_LOG(D) << "epoll_wait unblocked by " << n_triggered_events_ << " events";
+            if (errno == EINTR) {
+                FIBRE_LOG(D) << "interrupted";
+            }
+        } while (n_triggered_events_ < 0 && errno == EINTR); // ignore syscall interruptions. This happens for instance during suspend.
+
+        if (n_triggered_events_ <= 0) {
+            FIBRE_LOG(E) << "epoll_wait() failed with " <<  n_triggered_events_ << ": " << sys_err() << " - Terminating worker thread.";
+            ok = false;
+            break;
+        }
+
+        // Handle events
+        for (int i = 0; i < n_triggered_events_; ++i) {
+            EventContext* ctx = (EventContext*)triggered_events_[i].data.ptr;
+            if (ctx) {
+                try { // TODO: not sure if using "try" without throwing exceptions will do unwanted things with the stack
+                    ctx->callback.invoke(triggered_events_[i].events);
+                } catch (...) {
+                    FIBRE_LOG(E) << "worker callback threw an exception.";
+                }
+            }
+        }
+    }
+
+    FIBRE_LOG(D) << "epoll loop exited";
+
+    if ((post_fd_ >= 0) && !deregister_event(post_fd_)) {
+        FIBRE_LOG(E) << "deregister_event() failed";
+        ok = false;
+    }
+
+    if ((post_fd_ >= 0) && close(post_fd_) != 0) {
+        FIBRE_LOG(E) << "close() failed: " << sys_err();
+        ok = false;
+    }
+    post_fd_ = -1;
+
+    if (close(epoll_fd_) != 0) {
+        FIBRE_LOG(E) << "close() failed: " << sys_err();
+        ok = false;
+    }
+    epoll_fd_ = -1;
+
+    return ok;
+}
+
+bool EpollEventLoop::post(Callback<void> callback) {
+    if (epoll_fd_ < 0) {
+        FIBRE_LOG(E) << "not started";
+        return false;
+    }
+
+    {
+        std::unique_lock<std::mutex> lock(pending_callbacks_mutex_);
+        pending_callbacks_.push_back(callback);
+    }
+
+    const uint64_t val = 1;
+    if (write(post_fd_, &val, sizeof(val)) != sizeof(val)) {
+        FIBRE_LOG(E) << "write() failed" << sys_err();
+        return false;
+    }
+    return true;
+}
+
+bool EpollEventLoop::register_event(int event_fd, uint32_t events, Callback<void, uint32_t> callback) {
+    if (epoll_fd_ < 0) {
+        FIBRE_LOG(E) << "not initialized";
+        return false;
+    }
+    
+    if (event_fd < 0) {
+        FIBRE_LOG(E) << "invalid argument";
+        return false;
+    }
+
+    EventContext* ctx = new EventContext{callback};
+    struct epoll_event ev = {
+        .events = events,
+        .data = { .ptr = ctx }
+    };
+    context_map_[event_fd] = ctx;
+
+    if (epoll_ctl(epoll_fd_, EPOLL_CTL_ADD, event_fd, &ev) != 0) {
+        FIBRE_LOG(E) << "epoll_ctl(" << event_fd << "...) failed: " << sys_err();
+        delete ctx;
+        return false;
+    }
+
+    FIBRE_LOG(D) << "registered epoll event " << event_fd;
+
+    return true;
+}
+
+bool EpollEventLoop::deregister_event(int event_fd) {
+    if (epoll_fd_ < 0) {
+        FIBRE_LOG(E) << "not running";
+        return false;
+    }
+
+    int result = true;
+
+    if (epoll_ctl(epoll_fd_, EPOLL_CTL_DEL, event_fd, nullptr) != 0) {
+        FIBRE_LOG(E) << "epoll_ctl() failed: " << sys_err();
+        result = false;
+    }
+
+    EventContext* callback = context_map_[event_fd];
+
+    auto it = context_map_.find(event_fd);
+    if (it == context_map_.end()) {
+        FIBRE_LOG(E) << "event context not found";
+        return false;
+    }
+
+    for (int i = 0; i < n_triggered_events_; ++i) {
+        if ((EventContext*)(triggered_events_[i].data.ptr) == it->second) {
+            triggered_events_[i].data.ptr = nullptr;
+        }
+    }
+
+    context_map_.erase(it);
+    
+    return result;
+}
+
+struct EventLoopTimer* EpollEventLoop::call_later(float delay, Callback<void> callback) {
+    FIBRE_LOG(E) << "not implemented"; // TODO: implement
+    return nullptr;
+}
+
+bool EpollEventLoop::cancel_timer(EventLoopTimer* timer) {
+    FIBRE_LOG(E) << "not implemented"; // TODO: implement
+    return false;
+}
+
+void EpollEventLoop::run_callbacks(uint32_t) {
+    // TODO: warn if read fails
+    uint64_t val;
+    if (read(post_fd_, &val, sizeof(val)) != sizeof(val)) {
+        FIBRE_LOG(E) << "failed to read from post file descriptor";
+    }
+
+    std::vector<Callback<void>> pending_callbacks;
+
+    {
+        std::unique_lock<std::mutex> lock(pending_callbacks_mutex_);
+        std::swap(pending_callbacks, pending_callbacks_);
+    }
+
+    for (auto& cb: pending_callbacks) {
+        cb.invoke();
+    }
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/epoll_event_loop.hpp

@@ -0,0 +1,69 @@
+#ifndef __FIBRE_LINUX_EVENT_LOOP_HPP
+#define __FIBRE_LINUX_EVENT_LOOP_HPP
+
+//#include <thread>
+#include <sys/epoll.h>
+#include <unordered_map>
+#include <vector>
+#include <mutex>
+//#include <algorithm>
+
+#include <fibre/event_loop.hpp>
+
+namespace fibre {
+
+/**
+ * @brief Event loop based on the Linux-specific `epoll()` infrastructure.
+ * 
+ * Thread safety: None of the public functions are thread-safe with respect to
+ * each other. However they are thread safe with respect to the internal event
+ * loop, that means register_event() and deregister_event() can be called from
+ * within an event callback (which executes on the event loop thread), provided
+ * those calls are properly synchronized with calls from other threads.
+ */
+class EpollEventLoop : public EventLoop {
+public:
+
+    /**
+     * @brief Starts the event loop on the current thread and places the
+     * specified start callback on the event queue.
+     * 
+     * The function returns when the event loop becomes empty or if a platform
+     * error occurs.
+     */
+    bool start(Callback<void> on_started);
+
+    bool post(Callback<void> callback) final;
+    bool register_event(int fd, uint32_t events, Callback<void, uint32_t> callback) final;
+    bool deregister_event(int fd) final;
+    struct EventLoopTimer* call_later(float delay, Callback<void> callback) final;
+    bool cancel_timer(EventLoopTimer* timer) final;
+
+private:
+    struct EventContext {
+        //int fd;
+        Callback<void, uint32_t> callback;
+    };
+
+    void run_callbacks(uint32_t);
+
+    int epoll_fd_ = -1;
+    int post_fd_ = -1;
+    unsigned int iterations_ = 0;
+
+    std::unordered_map<int, EventContext*> context_map_; // required to deregister callbacks
+
+    static const size_t max_triggered_events_ = 16; // max number of events that can be handled per iteration
+    int n_triggered_events_ = 0;
+    struct epoll_event triggered_events_[max_triggered_events_];
+
+    // List of callbacks that were submitted through post().
+    std::vector<Callback<void>> pending_callbacks_;
+
+    // Mutex to protect pending_callbacks_
+    std::mutex pending_callbacks_mutex_;
+};
+
+}
+
+#endif // __FIBRE_LINUX_EVENT_LOOP_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/libusb_transport.cpp

@@ -0,0 +1,684 @@
+/**
+ * @brief Transport provider: libusb
+ * 
+ * Platform Compatibility: Linux, Windows, macOS
+ */
+
+#include "libusb_transport.hpp"
+#include "../logging.hpp"
+#include "../print_utils.hpp"
+#include <fibre/fibre.hpp>
+
+#include <algorithm>
+#include <string.h>
+
+#if !FIBRE_ALLOW_HEAP
+#  error "The libusb backend requires heap allocation."
+#endif
+
+using namespace fibre;
+
+DEFINE_LOG_TOPIC(USB);
+USE_LOG_TOPIC(USB);
+
+// This probably has no noteworthy effect since we automatically restart
+// timed out operations anyway.
+constexpr unsigned int kBulkTimeoutMs = 10000;
+
+// Only relevant for platforms don't support hotplug detection and thus
+// need polling.
+constexpr unsigned int kPollingIntervalMs = 1000;
+
+/* LibusbDiscoverer ----------------------------------------------------------*/
+
+/**
+ * @brief Initializes the discoverer.
+ * 
+ * Asynchronous tasks will be executed on the provided event_loop.
+ * 
+ * @param event_loop: The event loop that is used to execute background tasks. The
+ *        pointer must be non-null and initialized when this function is called.
+ *        It must remain initialized until deinit() of this discoverer was called.
+ */
+bool LibusbDiscoverer::init(EventLoop* event_loop) {
+    if (!event_loop)
+        return false;
+    event_loop_ = event_loop;
+
+    if (libusb_init(&libusb_ctx_) != LIBUSB_SUCCESS) {
+        FIBRE_LOG(E) << "libusb_init() failed: " << sys_err();
+        return deinit(0), false;
+    }
+
+    // Fetch initial list of file-descriptors we have to monitor.
+    // Note: this will fail on Windows. Since this is used for epoll, we need a
+    // different approach for Windows anyway.
+    const struct libusb_pollfd** pollfds = libusb_get_pollfds(libusb_ctx_);
+    using_sparate_libusb_thread_ = !pollfds;
+
+    if (!using_sparate_libusb_thread_) {
+        // This code path is taken on Linux
+        FIBRE_LOG(D) << "Using externally provided event loop";
+
+        // Check if libusb needs special time-based polling on this platform
+        if (libusb_pollfds_handle_timeouts(libusb_ctx_) == 0) {
+            FIBRE_LOG(D) << "Using time-based polling";
+        }
+
+        // libusb maintains a (dynamic) list of file descriptors that need to be
+        // monitored (via select/poll/epoll) so that I/O events can be processed when
+        // needed. Since we use the async libusb interface, we do the monitoring
+        // ourselves. That means we always need keep track of the libusb file
+        // descriptor list.
+
+        // Subscribe to changes to the list of file-descriptors we have to monitor.
+        libusb_set_pollfd_notifiers(libusb_ctx_,
+                [](int fd, short events, void *user_data) {
+                    ((LibusbDiscoverer*)user_data)->on_add_pollfd(fd, events);
+                },
+                [](int fd, void *user_data) {
+                    ((LibusbDiscoverer*)user_data)->on_remove_pollfd(fd);
+                }, this);
+
+        // Fetch initial list of file-descriptors we have to monitor.
+        // Note: this will fail on Windows. Since this is used for epoll, we need a
+        // different approach for Windows anyway.
+        const struct libusb_pollfd** pollfds = libusb_get_pollfds(libusb_ctx_);
+        if (!pollfds) {
+            return deinit(2), false;
+        }
+
+        for (size_t i = 0; pollfds[i]; ++i) {
+            on_add_pollfd(pollfds[i]->fd, pollfds[i]->events);
+        }
+        libusb_free_pollfds(pollfds);
+        pollfds = nullptr;
+
+    } else {
+        FIBRE_LOG(D) << "Using internal event loop thread";
+
+        // This code path is taken on Windows (which does not support epoll)
+        run_internal_event_loop_ = true;
+        internal_event_loop_thread_ = new std::thread([](void* ctx) {
+            ((LibusbDiscoverer*)ctx)->internal_event_loop();
+        }, this);
+    }
+
+    if (libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) {
+        // This code path is taken on Linux
+        FIBRE_LOG(D) << "Using libusb native hotplug detection";
+
+        // Subscribe to hotplug events
+        int result = libusb_hotplug_register_callback(libusb_ctx_,
+                             (libusb_hotplug_event)(LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED | LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT),
+                             LIBUSB_HOTPLUG_ENUMERATE /* trigger callback for all currently connected devices too */,
+                             LIBUSB_HOTPLUG_MATCH_ANY, LIBUSB_HOTPLUG_MATCH_ANY, LIBUSB_HOTPLUG_MATCH_ANY,
+                             [](struct libusb_context *ctx, struct libusb_device *dev, libusb_hotplug_event event, void *user_data){
+                                 return ((LibusbDiscoverer*)user_data)->on_hotplug(dev, event);
+                             }, this, &hotplug_callback_handle_);
+        if (LIBUSB_SUCCESS != result) {
+            FIBRE_LOG(E) << "Error subscribing to hotplug events";
+            hotplug_callback_handle_ = 0;
+            return deinit(3), false;
+        }
+
+    } else {
+        // This code path is taken on Windows
+        FIBRE_LOG(D) << "Using periodic polling to discover devices";
+
+        poll_devices_now(); // this will also start a timer to poll again periodically
+    }
+
+    if (!pollfds && libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) {
+        // The hotplug callback handler above is not yet thread-safe. To make it thread-safe
+        // we'd need to post it on the application's event loop.
+        FIBRE_LOG(E) << "Hotplug detection with separate libusb thread will cause trouble.";
+    }
+
+    return true;
+}
+
+bool LibusbDiscoverer::deinit(int stage) {
+    // TODO: verify that all devices are closed and hotplug detection is disabled
+
+    if (stage > 3 && libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) {
+        libusb_hotplug_deregister_callback(libusb_ctx_, hotplug_callback_handle_);
+    }
+    
+    if (stage > 3 && device_polling_timer_) {
+        event_loop_->cancel_timer(device_polling_timer_);
+        device_polling_timer_ = nullptr;
+    }
+
+    if (stage > 2 && !run_internal_event_loop_) {
+        // Deregister libusb events from our event loop.
+        const struct libusb_pollfd** pollfds = libusb_get_pollfds(libusb_ctx_);
+        if (pollfds) {
+            for (size_t i = 0; pollfds[i]; ++i) {
+                on_remove_pollfd(pollfds[i]->fd);
+            }
+            libusb_free_pollfds(pollfds);
+            pollfds = nullptr;
+        }
+    }
+
+    if (stage > 1 && !run_internal_event_loop_) {
+        libusb_set_pollfd_notifiers(libusb_ctx_, nullptr, nullptr, nullptr);
+    }
+
+    if (stage > 0 && run_internal_event_loop_) {
+        run_internal_event_loop_ = false;
+        libusb_interrupt_event_handler(libusb_ctx_);
+        internal_event_loop_thread_->join();
+        delete internal_event_loop_thread_;
+        internal_event_loop_thread_ = nullptr;
+    }
+
+    if (stage > 0) {
+        // TODO: we should probably deinit and close all connected channels
+        for (auto& dev: known_devices_) {
+            libusb_unref_device(dev.second.dev);
+        }
+    }
+
+    // FIXME: the libusb_hotplug_deregister_callback call will still trigger a
+    // usb_handler event. We need to wait until this has finished before we
+    // truly discard libusb resources
+    // Update: is this still relevant?
+    //usleep(100000);
+
+    if (stage > 0) {
+        libusb_exit(libusb_ctx_);
+        libusb_ctx_ = nullptr;
+    }
+
+    event_loop_ = nullptr;
+
+    return true;
+}
+
+/**
+ * @brief Starts looking for Fibre devices accessible through USB.
+ *
+ * Multiple discovery requests can be active at the same time but beware that a
+ * channel will be announced to all matching subscribers so be careful with access
+ * multiplexing.
+ * 
+ * If the function succeeds, an opaque context pointer is returned which must be
+ * passed to stop_channel_discovery() to terminate this particular request.
+ * 
+ * @param specs: See README of the main Fibre repository for details.
+ *        (https://github.com/samuelsadok/fibre/tree/devel).
+ *
+ * @param on_found_channels: Invoked when a matching pair of RX/TX channels is found.
+ *        This callback will also be called for any matching channels that already exist when
+ *        the discovery is started.
+ */
+void LibusbDiscoverer::start_channel_discovery(Domain* domain, const char* specs, size_t specs_len, ChannelDiscoveryContext** handle) {
+    FIBRE_LOG(D) << "starting discovery with filter \"" << std::string(specs, specs_len) << "\"";
+
+    InterfaceSpecs interface_specs;
+
+    try_parse_key(specs, specs + specs_len, "bus", &interface_specs.bus);
+    try_parse_key(specs, specs + specs_len, "address", &interface_specs.address);
+    try_parse_key(specs, specs + specs_len, "idVendor", &interface_specs.vendor_id);
+    try_parse_key(specs, specs + specs_len, "idProduct", &interface_specs.product_id);
+    try_parse_key(specs, specs + specs_len, "bInterfaceClass", &interface_specs.interface_class);
+    try_parse_key(specs, specs + specs_len, "bInterfaceSubClass", &interface_specs.interface_subclass);
+    try_parse_key(specs, specs + specs_len, "bInterfaceProtocol", &interface_specs.interface_protocol);
+
+    MyChannelDiscoveryContext* subscription = new MyChannelDiscoveryContext{};
+    subscription->interface_specs = interface_specs;
+    subscription->domain = domain;
+    subscriptions_.push_back(subscription);
+
+    for (auto& dev: known_devices_) {
+        consider_device(dev.second.dev, subscription);
+    }
+
+    if (handle) {
+        *handle = subscription;
+    }
+
+    return;
+}
+
+/**
+ * @brief Stops an object discovery process that was started with start_channel_discovery().
+ * 
+ * Channels which were already discovered will remain open. However if the discovery is restarted
+ * it is possible that the same channels are returned again (their pointers need not match the old instance).
+ * 
+ * The discovery must be considered still in progress until the callback is
+ * invoked with kFibreCancelled.
+ */
+int LibusbDiscoverer::stop_channel_discovery(ChannelDiscoveryContext* handle) {
+    auto it = std::find(subscriptions_.begin(), subscriptions_.end(), handle);
+
+    if (it == subscriptions_.end()) {
+        FIBRE_LOG(E) << "Not an active subscription";
+        return -1;
+    }
+
+    subscriptions_.erase(it);
+    delete handle;
+    return 0;
+}
+
+/**
+ * @brief Runs the event handling loop. This function blocks until
+ * run_internal_event_loop_ is false.
+ * 
+ * This loop is only executed on Windows. On other platforms the provided EventLoop is used.
+ */
+void LibusbDiscoverer::internal_event_loop() {
+    while (run_internal_event_loop_)
+        libusb_handle_events(libusb_ctx_);
+}
+
+void LibusbDiscoverer::on_event_loop_iteration() {
+    if (event_loop_timer_) {
+        FIBRE_LOG(D) << "cancelling event loop timer";
+        event_loop_->cancel_timer(event_loop_timer_);
+        event_loop_timer_ = nullptr;
+    }
+
+    timeval tv = { .tv_sec = 0, .tv_usec = 0 };
+    if (libusb_handle_events_timeout(libusb_ctx_, &tv) != 0) {
+        FIBRE_LOG(E) << "libusb_handle_events_timeout() failed";
+    }
+
+    timeval timeout;
+    if (libusb_get_next_timeout(libusb_ctx_, &timeout)) {
+        float timeout_sec = (float)timeout.tv_sec + (float)timeout.tv_usec * 1e-6;
+        FIBRE_LOG(D) << "setting event loop timeout to " << timeout_sec << " s";
+        event_loop_timer_ = event_loop_->call_later(timeout_sec,
+            MEMBER_CB(this, on_event_loop_iteration));
+    }
+}
+
+/**
+ * @brief Called when libusb wants to add a file descriptor to our event loop.
+ */
+void LibusbDiscoverer::on_add_pollfd(int fd, short events) {
+    event_loop_->register_event(fd, events,
+        MEMBER_CB(this, on_event_loop_iteration2));
+}
+
+/**
+ * @brief Called when libusb wants to remove a file descriptor to our event loop.
+ */
+void LibusbDiscoverer::on_remove_pollfd(int fd) {
+    event_loop_->deregister_event(fd);
+}
+
+/**
+ * @brief Called by libusb when a USB device was plugged in or out.
+ *
+ * If this function returns a non-zero value, libusb removes this filter.
+ */
+int LibusbDiscoverer::on_hotplug(struct libusb_device *dev,
+                                 libusb_hotplug_event event) {
+    uint8_t bus_number = libusb_get_bus_number(dev);
+    uint8_t dev_number = libusb_get_device_address(dev);
+    
+    if (LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED == event) {
+        FIBRE_LOG(D) << "device arrived: bus " << (int)bus_number << ", " << (int)dev_number;
+
+        // add empty placeholder to the list of known devices
+        known_devices_[bus_number << 8 | dev_number] = {
+            .dev = libusb_ref_device(dev),
+            .handle = nullptr
+        };
+
+        for (auto& subscription: subscriptions_) {
+            consider_device(dev, subscription);
+        }
+
+    } else if (LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT == event) {
+        FIBRE_LOG(D) << "device left: bus " << (int)bus_number << ", " << (int)dev_number;
+
+        auto it = known_devices_.find(bus_number << 8 | dev_number);
+
+        if (it != known_devices_.end()) {
+            for (auto& ep: it->second.ep_in) {
+                ep->deinit();
+            }
+            for (auto& ep: it->second.ep_out) {
+                ep->deinit();
+            }
+            if (it->second.handle) {
+                libusb_close(it->second.handle);
+            }
+
+            known_devices_.erase(it);
+        }
+
+        libusb_unref_device(dev);
+
+    } else {
+        FIBRE_LOG(W) << "Unexpected event: " << event;
+    }
+    
+    return 0;
+}
+
+void LibusbDiscoverer::poll_devices_now() {
+    FIBRE_LOG(D) << "poll_devices_now() called.";
+
+    device_polling_timer_ = nullptr;
+
+    libusb_device** list = nullptr;
+    ssize_t n_devices = libusb_get_device_list(libusb_ctx_, &list);
+    std::unordered_map<uint16_t, libusb_device*> current_devices;
+
+    if (n_devices < 0) {
+        FIBRE_LOG(W) << "libusb_get_device_list() failed.";
+    } else {
+        for (ssize_t i = 0; i < n_devices; ++i) {
+            uint8_t bus_number = libusb_get_bus_number(list[i]);
+            uint8_t dev_number = libusb_get_device_address(list[i]);
+            current_devices[bus_number << 8 | dev_number] = list[i];
+        }
+
+        // Call on_hotplug for all new devices
+        for (auto& dev: current_devices) {
+            if (known_devices_.find(dev.first) == known_devices_.end()) {
+                on_hotplug(dev.second, LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED);
+
+                // Immediately forget about the devices that weren't opened on plugin.
+                // The reason is this: On Windows the device address and even the
+                // device pointer can remain equal across device reset. Since we don't
+                // poll at infinite frequency This means we could miss a device reset.
+                // To avoid this, we reinspect the all unopened devices on
+                // every polling iteration.
+                auto it = known_devices_.find(dev.first);
+                if (it->second.handle == nullptr) {
+                    known_devices_.erase(it);
+                }
+            }
+        }
+
+        // Call on_hotplug for all lost devices
+
+        std::vector<libusb_device*> lost_devices;
+
+        for (auto& dev: known_devices_) {
+            if (current_devices.find(dev.first) == current_devices.end()) {
+                lost_devices.push_back(dev.second.dev);
+            }
+        }
+
+        for (auto& dev: lost_devices) {
+            on_hotplug(dev, LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT);
+        }
+
+        libusb_free_device_list(list, 1 /* unref the devices */);
+    }
+
+    // It's possible that the discoverer was deinited during this function.
+    if (event_loop_) {
+        device_polling_timer_ = event_loop_->call_later(kPollingIntervalMs * 0.001f,
+            MEMBER_CB(this, poll_devices_now));
+    }
+}
+
+void LibusbDiscoverer::consider_device(struct libusb_device *device, MyChannelDiscoveryContext* subscription) {
+    uint8_t bus_number = libusb_get_bus_number(device);
+    uint8_t dev_number = libusb_get_device_address(device);
+
+    bool mismatch = (subscription->interface_specs.bus != -1 && bus_number != subscription->interface_specs.bus)
+                 || (subscription->interface_specs.address != -1 && dev_number != subscription->interface_specs.address);
+
+    if (mismatch) {
+        return;
+    }
+
+    if (subscription->interface_specs.vendor_id != -1 || subscription->interface_specs.product_id != -1) {
+        struct libusb_device_descriptor dev_desc;
+        int result = libusb_get_device_descriptor(device, &dev_desc);
+        if (result != LIBUSB_SUCCESS) {
+            FIBRE_LOG(W) << "Failed to get device descriptor: " << result;
+        }
+
+        mismatch = (subscription->interface_specs.vendor_id != -1 && dev_desc.idVendor != subscription->interface_specs.vendor_id)
+                || (subscription->interface_specs.product_id != -1 && dev_desc.idProduct != subscription->interface_specs.product_id);
+
+        if (mismatch) {
+            return;
+        }
+    }
+    
+    struct libusb_config_descriptor* config_desc = nullptr;
+
+    if (libusb_get_active_config_descriptor(device, &config_desc) != LIBUSB_SUCCESS) {
+        FIBRE_LOG(E) << "Failed to get active config descriptor: " << sys_err();
+    } else {
+        for (uint8_t i = 0; i < config_desc->bNumInterfaces; ++i) {
+            for (int j = 0; j < config_desc->interface[i].num_altsetting; ++j) {
+                // TODO: probably we should only chose one alt setting
+                const struct libusb_interface_descriptor* intf_desc = &(config_desc->interface[i].altsetting[j]);
+
+                mismatch = (subscription->interface_specs.interface_class != -1 && intf_desc->bInterfaceClass != subscription->interface_specs.interface_class)
+                        || (subscription->interface_specs.interface_subclass != -1 && intf_desc->bInterfaceSubClass != subscription->interface_specs.interface_subclass)
+                        || (subscription->interface_specs.interface_protocol != -1 && intf_desc->bInterfaceProtocol != subscription->interface_specs.interface_protocol);
+                if (mismatch) {
+                    continue;
+                }
+
+                // We found a matching interface. Now find one bulk IN and one bulk OUT endpoint.
+                const libusb_endpoint_descriptor* libusb_ep_in = nullptr;
+                const libusb_endpoint_descriptor* libusb_ep_out = nullptr;
+                for (uint8_t k = 0; k < intf_desc->bNumEndpoints; ++k) {
+                    if ((intf_desc->endpoint[k].bmAttributes & 0x03) == LIBUSB_TRANSFER_TYPE_BULK
+                        && (intf_desc->endpoint[k].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_IN) {
+                        libusb_ep_in = &intf_desc->endpoint[k];
+                    } else if ((intf_desc->endpoint[k].bmAttributes & 0x03) == LIBUSB_TRANSFER_TYPE_BULK
+                               && (intf_desc->endpoint[k].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_OUT) {
+                        libusb_ep_out = &intf_desc->endpoint[k];
+                    }
+                }
+
+                Device& my_dev = known_devices_[bus_number << 8 | dev_number];
+
+                // If the same device was already returned in a previous discovery
+                // then it will already be open.
+
+                if (!my_dev.handle) {
+                    int result = libusb_open(device, &my_dev.handle);
+                    if (LIBUSB_SUCCESS != result) {
+                        FIBRE_LOG(E) << "Could not open USB device: " << result;
+                        continue;
+                    }
+                }
+
+                int result = libusb_claim_interface(my_dev.handle, i);
+                if (LIBUSB_SUCCESS != result) {
+                    FIBRE_LOG(E) << "Could not claim interface " << i << " on USB device: " << result;
+                    continue;
+                }
+
+                size_t mtu = SIZE_MAX;
+
+                LibusbBulkInEndpoint* ep_in = new LibusbBulkInEndpoint();
+                if (libusb_ep_in && ep_in->init(this, my_dev.handle, libusb_ep_in->bEndpointAddress)) {
+                    my_dev.ep_in.push_back(ep_in);
+                    mtu = std::min(mtu, (size_t)libusb_ep_in->wMaxPacketSize);
+                } else {
+                    delete ep_in;
+                    ep_in = nullptr;
+                }
+
+                LibusbBulkOutEndpoint* ep_out = new LibusbBulkOutEndpoint();
+                if (libusb_ep_out && ep_out->init(this, my_dev.handle, libusb_ep_out->bEndpointAddress)) {
+                    my_dev.ep_out.push_back(ep_out);
+                    mtu = std::min(mtu, (size_t)libusb_ep_out->wMaxPacketSize);
+                } else {
+                    delete ep_out;
+                    ep_out = nullptr;
+                }
+
+                subscription->domain->add_channels({kFibreOk, ep_in, ep_out, mtu});
+            }
+        }
+
+        libusb_free_config_descriptor(config_desc);
+        config_desc = nullptr;
+    }
+}
+
+
+/* LibusbBulkEndpoint --------------------------------------------------------*/
+
+template<typename TRes>
+bool LibusbBulkEndpoint<TRes>::init(LibusbDiscoverer* parent, libusb_device_handle* handle, uint8_t endpoint_id) {
+    parent_ = parent;
+    handle_ = handle;
+    transfer_ = libusb_alloc_transfer(0);
+    endpoint_id_ = endpoint_id;
+    return true;
+}
+
+template<typename TRes>
+bool LibusbBulkEndpoint<TRes>::deinit() {
+    if (completer_) {
+        FIBRE_LOG(E) << "Transfer on EP " << as_hex(endpoint_id_) << " still in progress. This is gonna be messy.";
+    }
+
+    libusb_free_transfer(transfer_);
+    transfer_ = nullptr;
+    return true;
+}
+
+template<typename TRes>
+void LibusbBulkEndpoint<TRes>::start_transfer(bufptr_t buffer, TransferHandle* handle, Callback<void, TRes> completer) {
+    if (handle) {
+        *handle = reinterpret_cast<TransferHandle>(this);
+    }
+
+    if (completer_) {
+        FIBRE_LOG(E) << "transfer already in progress";
+        completer.invoke({kStreamError, nullptr});
+        return;
+    }
+
+    if (!handle_) {
+        FIBRE_LOG(E) << "device not open";
+        completer.invoke({kStreamError, nullptr});
+        return;
+    }
+
+    auto direct_callback = [](struct libusb_transfer* transfer){
+        ((LibusbBulkEndpoint<TRes>*)transfer->user_data)->on_transfer_finished();
+    };
+
+    // This callback is used if we start our own libusb thread
+    // separate from the application's event loop thread
+    auto indirect_callback = [](struct libusb_transfer* transfer){
+        auto ep = (LibusbBulkEndpoint<TRes>*)transfer->user_data;
+        ep->parent_->event_loop_->post(MEMBER_CB(ep, on_transfer_finished));
+    };
+
+    //FIBRE_LOG(D) << "transfer of size " << buffer.size();
+    libusb_fill_bulk_transfer(transfer_, handle_, endpoint_id_,
+        buffer.begin(), buffer.size(),
+        parent_->using_sparate_libusb_thread_ ? indirect_callback : direct_callback,
+        this, kBulkTimeoutMs);
+    
+    completer_ = completer;
+    submit_transfer();
+}
+
+template<typename TRes>
+void LibusbBulkEndpoint<TRes>::cancel_transfer(TransferHandle transfer_handle) {
+    if (!completer_) {
+        FIBRE_LOG(E) << "transfer not in progress";
+        return;
+    }
+
+    libusb_cancel_transfer(transfer_);
+}
+
+template<typename TRes>
+void LibusbBulkEndpoint<TRes>::submit_transfer() {
+    int result = libusb_submit_transfer(transfer_);
+    if (LIBUSB_SUCCESS == result) {
+        // ok
+        FIBRE_LOG(T) << "started USB transfer on EP " << as_hex(endpoint_id_);
+    } else if (LIBUSB_ERROR_NO_DEVICE == result) {
+        FIBRE_LOG(W) << "couldn't start USB transfer on EP " << as_hex(endpoint_id_) << ": " << libusb_error_name(result);
+        completer_.invoke_and_clear({kStreamClosed, nullptr});
+    } else {
+        FIBRE_LOG(W) << "couldn't start USB transfer on EP " << as_hex(endpoint_id_) << ": " << libusb_error_name(result);
+        completer_.invoke_and_clear({kStreamError, nullptr});
+    }
+}
+
+template<typename TRes>
+void LibusbBulkEndpoint<TRes>::on_transfer_finished() {
+    // We ignore timeouts here and just retry. If the application wishes to have
+    // a timeout on the transfer it can just call cancel_transfer() after a while.
+    if (transfer_->status == LIBUSB_TRANSFER_TIMED_OUT) {
+        submit_transfer();
+        return;
+    }
+    
+    libusb_device* dev = libusb_get_device(handle_);
+
+    StreamStatus status;
+
+    if (transfer_->status == LIBUSB_TRANSFER_COMPLETED) {
+        status = kStreamOk;
+    } else if (transfer_->status == LIBUSB_TRANSFER_CANCELLED) {
+        status = kStreamCancelled;
+    } else {
+        // The error that we get on device removal tends to be inaccurate.
+        // Sometimes it's LIBUSB_TRANSFER_STALL, sometimes
+        // LIBUSB_TRANSFER_ERROR. Therefore we just check if the device
+        // is still present to determine which error code to return.
+        // TODO: this detection doesn't really work. The device is still in the
+        // device list at this point when it just got unplugged. For now we
+        // just ignore transfer errors.
+
+        bool found = false;
+
+        libusb_device** list;
+        ssize_t n_devices = libusb_get_device_list(parent_->libusb_ctx_, &list);
+
+        if (n_devices >= 0) {
+            for (size_t i = 0; i < (size_t)n_devices; ++i) {
+                if (list[i] == dev) {
+                    // found = true;
+                    break;
+                }
+            }
+            libusb_free_device_list(list, 1);
+        }
+
+        if (found) {
+            status = kStreamError;
+        } else {
+            FIBRE_LOG(D) << "device removed during transfer";
+            status = kStreamClosed;
+        }
+    }
+
+    (status == kStreamError ? FIBRE_LOG(W) : FIBRE_LOG(T))
+        << "USB transfer on EP " << as_hex(endpoint_id_) << " finished with " << libusb_error_name(transfer_->status);
+
+    if (status == kStreamClosed) {
+        handle_ = nullptr; // Ensure that no new transfer is started
+    }
+
+    uint8_t* end = std::max(transfer_->buffer + transfer_->actual_length, transfer_->buffer);
+    completer_.invoke_and_clear({status, end});
+    
+    // If libusb does hotplug detection itself then we don't need to handle
+    // device removal here. Libusb will call the corresponding hotplug callback.
+    if (status == kStreamClosed && !parent_->hotplug_callback_handle_) {
+        if (!parent_->using_sparate_libusb_thread_) {
+            FIBRE_LOG(E) << "It's not a good idea to unref the device from within this callback. This will probably hang.";
+        }
+        parent_->on_hotplug(dev, LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT);
+    }
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/libusb_transport.hpp

@@ -0,0 +1,125 @@
+#ifndef __FIBRE_USB_DISCOVERER_HPP
+#define __FIBRE_USB_DISCOVERER_HPP
+
+#include <fibre/event_loop.hpp>
+#include <fibre/async_stream.hpp>
+#include <fibre/channel_discoverer.hpp>
+
+#include <libusb.h>
+#include <thread>
+#include <vector>
+#include <unordered_map>
+
+namespace fibre {
+
+class LibusbBulkInEndpoint;
+class LibusbBulkOutEndpoint;
+
+template<typename TRes> class LibusbBulkEndpoint;
+
+class LibusbDiscoverer : public ChannelDiscoverer {
+public:
+
+    struct InterfaceSpecs {
+        int bus = -1; // -1 to ignore
+        int address = -1; // -1 to ignore
+        int vendor_id = -1; // -1 to ignore
+        int product_id = -1; // -1 to ignore
+        int interface_class = -1; // -1 to ignore
+        int interface_subclass = -1; // -1 to ignore
+        int interface_protocol = -1; // -1 to ignore
+    };
+
+    struct MyChannelDiscoveryContext : ChannelDiscoveryContext {
+        InterfaceSpecs interface_specs;
+        Domain* domain;
+    };
+
+    constexpr static const char* get_name() { return "usb"; }
+    bool init(EventLoop* event_loop);
+    bool deinit() { return deinit(INT_MAX); }
+    void start_channel_discovery(Domain* domain, const char* specs, size_t specs_len, ChannelDiscoveryContext** handle) final;
+    int stop_channel_discovery(ChannelDiscoveryContext* handle) final;
+
+private:
+    friend class LibusbBulkEndpoint<ReadResult>;
+    friend class LibusbBulkEndpoint<WriteResult>;
+
+    struct Device {
+        struct libusb_device* dev;
+        struct libusb_device_handle* handle;
+        std::vector<LibusbBulkInEndpoint*> ep_in;
+        std::vector<LibusbBulkOutEndpoint*> ep_out;
+    };
+
+    bool deinit(int stage);
+    void internal_event_loop();
+    void on_event_loop_iteration();
+    void on_event_loop_iteration2(uint32_t) { on_event_loop_iteration(); }
+    void on_add_pollfd(int fd, short events);
+    void on_remove_pollfd(int fd);
+    int on_hotplug(struct libusb_device *dev, libusb_hotplug_event event);
+    void poll_devices_now();
+    void consider_device(struct libusb_device *device, MyChannelDiscoveryContext* subscription);
+
+    EventLoop* event_loop_ = nullptr;
+    bool using_sparate_libusb_thread_; // true on Windows. Initialized in init()
+    libusb_context *libusb_ctx_ = nullptr; // libusb session
+    libusb_hotplug_callback_handle hotplug_callback_handle_ = 0;
+    bool run_internal_event_loop_ = false;
+    std::thread* internal_event_loop_thread_;
+    EventLoopTimer* device_polling_timer_;
+    EventLoopTimer* event_loop_timer_ = nullptr;
+    std::unordered_map<uint16_t, Device> known_devices_; // key: bus_number << 8 | dev_number
+    std::vector<MyChannelDiscoveryContext*> subscriptions_;
+};
+
+template<typename TRes>
+class LibusbBulkEndpoint {
+public:
+    bool init(LibusbDiscoverer* parent, struct libusb_device_handle* handle, uint8_t endpoint_id);
+    bool deinit();
+
+protected:
+    void start_transfer(bufptr_t buffer, TransferHandle* handle, Callback<void, TRes> completer);
+    void cancel_transfer(TransferHandle transfer_handle);
+
+private:
+    void submit_transfer();
+    void on_transfer_finished();
+
+    LibusbDiscoverer* parent_ = nullptr;
+    struct libusb_device_handle* handle_ = nullptr;
+    uint8_t endpoint_id_ = 0;
+    struct libusb_transfer* transfer_ = nullptr;
+    Callback<void, TRes> completer_ = nullptr;
+};
+
+class LibusbBulkInEndpoint final : public LibusbBulkEndpoint<ReadResult>, public AsyncStreamSource {
+public:
+    void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) final {
+        start_transfer(buffer, handle, completer);
+    }
+
+    void cancel_read(TransferHandle transfer_handle) final {
+        cancel_transfer(transfer_handle);
+    }
+};
+
+class LibusbBulkOutEndpoint final : public LibusbBulkEndpoint<WriteResult>, public AsyncStreamSink {
+public:
+    void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final {
+        start_transfer({
+            (unsigned char*)buffer.begin(),
+            buffer.size()
+        }, handle, completer);
+    }
+
+    void cancel_write(TransferHandle transfer_handle) final {
+        cancel_transfer(transfer_handle);
+    }
+};
+
+}
+
+#endif // __FIBRE_USB_DISCOVERER_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/posix_socket.cpp

@@ -0,0 +1,505 @@
+
+#include "posix_socket.hpp"
+#include "../logging.hpp"
+#include "../print_utils.hpp"
+
+#include <errno.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <netdb.h>
+#include <signal.h>
+#include <sys/epoll.h>
+
+DEFINE_LOG_TOPIC(SOCKET);
+USE_LOG_TOPIC(SOCKET);
+
+#define MAX_CONCURRENT_CONNECTIONS 128
+
+using namespace fibre;
+
+namespace fibre {
+/**
+ * @brief Tag type to print the last socket error.
+ * 
+ * This is very similar to sys_err(), except that on Windows it uses
+ * WSAGetLastError() instead of `errno` to fetch the last error code.
+ */
+struct sock_err {
+    sock_err() :
+#if defined(_WIN32) || defined(_WIN64)
+        error_number(WSAGetLastError()) {}
+#else
+        error_number(errno) {}
+#endif
+
+    sock_err(int error_number) : error_number(error_number) {}
+
+    int error_number;
+};
+}
+
+namespace std {
+std::ostream& operator<<(std::ostream& stream, const struct sockaddr_storage& val) {
+    char buf[128];
+
+    if ((val.ss_family == AF_INET) && (inet_ntop(val.ss_family, ((struct sockaddr*)&val)->sa_data+2, buf, sizeof(buf)))) {
+        return stream << buf;
+    } else if ((val.ss_family == AF_INET6) && (inet_ntop(val.ss_family, ((struct sockaddr*)&val)->sa_data+6, buf, sizeof(buf)))) {
+        return stream << buf;
+    } else {
+        return stream << "(invalid address)";
+    }
+}
+
+std::ostream& operator<<(std::ostream& stream, const fibre::sock_err& err) {
+    return stream << strerror(err.error_number) << " (" << err.error_number << ")";
+}
+}
+
+struct fibre::AddressResolutionContext {
+    struct addrinfo hints{};
+    std::string address_str;
+    std::string port_str;
+    EventLoop* event_loop;
+    Callback<void, std::optional<cbufptr_t>> callback;
+    int cmpl_fd;
+    struct gaicb gaicb{};
+    struct gaicb* list[1];
+
+    void on_gai_completed();
+};
+
+bool fibre::start_resolving_address(EventLoop* event_loop, std::tuple<std::string, int> address, bool passive, AddressResolutionContext** handle, Callback<void, std::optional<cbufptr_t>> callback) {
+    // deleted in on_gai_completed()
+    AddressResolutionContext* ctx = new AddressResolutionContext();
+
+    ctx->address_str = std::get<0>(address);
+    ctx->port_str = std::to_string(std::get<1>(address));
+    ctx->event_loop = event_loop;
+    ctx->callback = callback;
+
+    ctx->hints = {
+        .ai_flags = (passive ? AI_PASSIVE : 0),
+        .ai_family = AF_UNSPEC,
+        .ai_socktype = 0, // this makes apparently no difference for numerical addresses
+    };
+
+    ctx->gaicb = {
+        .ar_name = ctx->address_str.c_str(),
+        .ar_service = ctx->port_str.c_str(),
+        .ar_request = &ctx->hints
+    };
+    ctx->list[0] = &ctx->gaicb;
+
+    // An extra thread will be created once getaddrinfo_a() completes. This
+    // thread will post a callback onto the original event loop to do the actual
+    // handling of the result. This is of course exceedingly stupid but it's
+    // less bad than throwing around with actual signals that could hit threads
+    // that don't expect it.
+
+    struct sigevent sig = {
+        .sigev_value = { .sival_ptr = ctx },
+        //.sigev_signo = SIGRTMIN,
+        .sigev_notify = SIGEV_THREAD,
+    };
+    sig.sigev_notify_function = [](union sigval sigval) {
+        auto ctx = ((AddressResolutionContext*)sigval.sival_ptr);
+        ctx->event_loop->post(MEMBER_CB(ctx, on_gai_completed));
+    };
+
+    FIBRE_LOG(D) << "starting address resolution for " << ctx->address_str;
+    if (getaddrinfo_a(GAI_NOWAIT, ctx->list, 1, &sig) != 0) {
+        FIBRE_LOG(E) << "getaddrinfo_a() failed";
+        delete ctx;
+        return false;
+    }
+
+    return true;
+}
+
+void fibre::cancel_resolving_address(AddressResolutionContext* handle) {
+    gai_cancel(&handle->gaicb);
+}
+
+void AddressResolutionContext::on_gai_completed() {
+    FIBRE_LOG(D) << "address resolution complete";
+    if (gai_error(&gaicb) != 0) {
+        FIBRE_LOG(W) << "failed to resolve " << address_str << ": " << sys_err();
+    } else {
+        // this returns multiple addresses
+        for (struct addrinfo* addr = gaicb.ar_result; addr; addr = addr->ai_next) {
+            FIBRE_LOG(D) << "resolved IP: " << *(struct sockaddr_storage*)addr->ai_addr;
+            cbufptr_t buf = {(const uint8_t*)addr->ai_addr, (size_t)addr->ai_addrlen};
+            callback.invoke(buf);
+        }
+    }
+    freeaddrinfo(gaicb.ar_result);
+    callback.invoke(std::nullopt); // Announce completion of the request
+    delete this;
+}
+
+struct fibre::ConnectionContext {
+    EventLoop* event_loop;
+    socket_id_t socket_id;
+    Callback<void, std::optional<socket_id_t>> callback;
+
+    void on_connection_complete(uint32_t mask);
+    void on_accept(uint32_t mask);
+};
+
+bool fibre::start_connecting(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected) {
+    auto the_addr = reinterpret_cast<const struct sockaddr*>(addr.begin());
+
+    ConnectionContext* context = new ConnectionContext();
+    context->event_loop = event_loop;
+    context->socket_id = socket(the_addr->sa_family, type | SOCK_NONBLOCK, protocol);
+    context->callback = on_connected;
+
+    if (IS_INVALID_SOCKET(context->socket_id)) {
+        FIBRE_LOG(E) << "failed to open socket: " << sock_err();
+        goto fail0;
+    }
+   
+    if (connect(context->socket_id, the_addr, addr.size()) == 0) {
+        if (errno != EINPROGRESS) {
+            FIBRE_LOG(E) << "connect() failed: " << sock_err();
+            goto fail1;
+        }
+    }
+
+    if (!event_loop->register_event(context->socket_id, EPOLLOUT, MEMBER_CB(context, on_connection_complete))) {
+            FIBRE_LOG(E) << "failed to register event: " << sock_err();
+        goto fail1;
+    }
+
+    if (ctx) {
+        *ctx = context;
+    }
+
+    return true;
+
+fail1:
+    close(context->socket_id);
+fail0:
+    delete context;
+    return false;
+}
+
+void fibre::stop_connecting(ConnectionContext* ctx) {
+    if (!ctx->event_loop->deregister_event(ctx->socket_id)) {
+        FIBRE_LOG(W) << "failed to deregister event";
+    }
+    if (close(ctx->socket_id) != 0) {
+        FIBRE_LOG(W) << "failed to close socket";
+    }
+    ctx->socket_id = INVALID_SOCKET;
+    ctx->callback.invoke_and_clear(std::nullopt);
+    delete ctx;
+}
+
+void fibre::ConnectionContext::on_connection_complete(uint32_t mask) {
+    bool failed;
+    int error_code;
+    socklen_t error_code_size = sizeof(error_code);
+    if (getsockopt(socket_id, SOL_SOCKET, SO_ERROR, &error_code, &error_code_size) != 0) {
+        FIBRE_LOG(W) << "connection failed (unknown error)";
+        failed = true;
+    } else if (error_code != 0) {
+        FIBRE_LOG(W) << "connection failed: " << sock_err{error_code};
+        failed = true;
+    } else {
+        failed = false;
+    }
+    
+    event_loop->deregister_event(socket_id);
+    callback.invoke(failed ? std::nullopt : std::make_optional(socket_id));
+    close(socket_id); // The callback must duplicate the socket id if it intends
+                      // to keep using it.
+    delete this;
+}
+
+bool fibre::start_listening(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected) {
+    auto the_addr = reinterpret_cast<const struct sockaddr*>(addr.begin());
+    int flag = 1;
+
+    ConnectionContext* context = new ConnectionContext();
+    context->event_loop = event_loop;
+    context->socket_id = socket(the_addr->sa_family, type | SOCK_NONBLOCK, protocol);
+    context->callback = on_connected;
+
+    if (IS_INVALID_SOCKET(context->socket_id)) {
+        FIBRE_LOG(E) << "failed to open socket: " << sock_err();
+        goto fail0;
+    }
+
+    // Reuse local address.
+    // This helps reusing ports that were previously not closed cleanly and
+    // are therefore still lingering in the TIME_WAIT state.
+    if (setsockopt(context->socket_id, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(flag))) {
+        FIBRE_LOG(E) << "failed to make socket reuse addresses: " << sock_err();
+        goto fail1;
+    }
+   
+    if (bind(context->socket_id, the_addr, addr.size())) {
+        FIBRE_LOG(E) << "failed to bind socket: " << sock_err();
+        goto fail1;
+    }
+
+    // make this socket a passive socket
+    if (listen(context->socket_id, MAX_CONCURRENT_CONNECTIONS) != 0) {
+        FIBRE_LOG(E) << "failed to listen on TCP: " << sys_err();
+        goto fail1;
+    }
+
+    if (!event_loop->register_event(context->socket_id, EPOLLIN, MEMBER_CB(context, on_accept))) {
+            FIBRE_LOG(E) << "failed to register event: " << sock_err();
+        goto fail1;
+    }
+
+    return true;
+
+fail1:
+    close(context->socket_id);
+fail0:
+    delete context;
+    return false;
+}
+
+void fibre::stop_listening(ConnectionContext* ctx) {
+    stop_connecting(ctx); // same implementation
+}
+
+void fibre::ConnectionContext::on_accept(uint32_t mask) {
+    struct sockaddr_storage remote_addr;
+    socklen_t slen = sizeof(remote_addr);
+
+    FIBRE_LOG(D) << "incoming TCP connection";
+    int new_socket_id = accept(socket_id, reinterpret_cast<struct sockaddr *>(&remote_addr), &slen);
+    if (IS_INVALID_SOCKET(new_socket_id)) {
+        FIBRE_LOG(E) << "accept() returned invalid socket: " << sock_err();
+        return; // ignore and wait for next incoming connection
+    }
+
+    callback.invoke(std::make_optional(new_socket_id));
+    close(new_socket_id); // The callback must duplicate the socket id if it intends
+                          // to keep using it.
+}
+
+bool PosixSocket::init(EventLoop* event_loop, socket_id_t socket_id) {
+    if (!IS_INVALID_SOCKET(socket_id_)) {
+        FIBRE_LOG(E) << "already initialized";
+        return false;
+    }
+
+    socket_id = dup(socket_id);
+    if (IS_INVALID_SOCKET(socket_id)) {
+        FIBRE_LOG(E) << "failed to duplicate socket: " << sock_err();
+        return false;
+    }
+
+    //if (!event_loop->register_event(socket_id, 0, MEMBER_CB(this, on_event))) {
+    //    FIBRE_LOG(E) << "failed to register socket event";
+    //    close(socket_id);
+    //    return false;
+    //}
+
+    event_loop_ = event_loop;
+    socket_id_ = socket_id;
+    return true;
+}
+
+bool PosixSocket::deinit() {
+    if (IS_INVALID_SOCKET(socket_id_)) {
+        FIBRE_LOG(E) << "not initialized";
+        return false;
+    }
+
+    bool result = true;
+    if (::close(socket_id_)) {
+        FIBRE_LOG(E) << "close() failed: " << sock_err();
+        result = false;
+    }
+
+    socket_id_ = INVALID_SOCKET;
+    return result;
+}
+
+void PosixSocket::start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) {
+    if (rx_callback_) {
+        FIBRE_LOG(E) << "RX request already pending";
+        completer.invoke({kStreamError});
+        return;
+    }
+
+    if (handle) {
+        *handle = reinterpret_cast<TransferHandle>(this);
+    }
+
+    auto result = read_sync(buffer);
+    if (result.has_value()) {
+        completer.invoke(*result);
+    } else {
+        rx_buf_ = buffer;
+        rx_callback_ = completer;
+        update_subscription();
+    }
+}
+
+void PosixSocket::cancel_read(TransferHandle transfer_handle) {
+    if (transfer_handle != reinterpret_cast<TransferHandle>(this)) {
+        FIBRE_LOG(E) << "invalid handle";
+    } else if (!rx_callback_) {
+        FIBRE_LOG(E) << "no RX pending";
+    } else {
+        rx_callback_.invoke_and_clear({kStreamCancelled, rx_buf_.begin()});
+    }
+}
+
+void PosixSocket::start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) {
+    if (tx_callback_) {
+        FIBRE_LOG(E) << "TX request already pending";
+        completer.invoke({kStreamError});
+        return;
+    }
+
+    if (handle) {
+        *handle = reinterpret_cast<TransferHandle>(this);
+    }
+
+    auto result = write_sync(buffer);
+    if (result.has_value()) {
+        completer.invoke(*result);
+    } else {
+        tx_buf_ = buffer;
+        tx_callback_ = completer;
+        update_subscription();
+    }
+}
+
+void PosixSocket::cancel_write(TransferHandle transfer_handle) {
+    if (transfer_handle != reinterpret_cast<TransferHandle>(this)) {
+        FIBRE_LOG(E) << "invalid handle";
+    } else if (!tx_callback_) {
+        FIBRE_LOG(E) << "no TX pending";
+    } else {
+        tx_callback_.invoke_and_clear({kStreamCancelled, tx_buf_.begin()});
+    }
+}
+
+std::optional<ReadResult> PosixSocket::read_sync(bufptr_t buffer) {
+    if (buffer.size() == 0) {
+        // Empty buffers mess with our socket-close detection
+        FIBRE_LOG(W) << "empty buffer not permitted";
+    }
+
+    socklen_t slen = sizeof(remote_addr_);
+    ssize_t n_received = recvfrom(socket_id_, buffer.begin(), buffer.size(),
+            MSG_DONTWAIT, reinterpret_cast<struct sockaddr *>(&remote_addr_), &slen);
+    
+    if (n_received < 0) {
+        // If recvfrom returns -1 an errno is set to indicate the error.
+        auto err = sock_err{};
+        if (err.error_number == EAGAIN || err.error_number == EWOULDBLOCK) {
+            return std::nullopt;
+        } else {
+            FIBRE_LOG(E) << "Socket read failed: " << err;
+            return {{kStreamError, buffer.end()}}; // the function might have written to the buffer
+        }
+
+    } else if ((size_t)n_received > buffer.size()) {
+        FIBRE_LOG(E) << "received too many bytes";
+        return {{kStreamError, buffer.end()}};
+
+    } else if (n_received == 0) {
+        FIBRE_LOG(D) << "socket closed (RX half)";
+        return {{kStreamClosed, buffer.begin()}};
+
+    } else {
+        FIBRE_LOG(D) << "Received " << n_received << " bytes from " << remote_addr_;
+        return {{kStreamOk, buffer.begin() + n_received}};
+    }
+}
+
+std::optional<WriteResult> PosixSocket::write_sync(cbufptr_t buffer) {
+    if (buffer.size() == 0) {
+        // Empty buffers mess with our socket-close detection
+        FIBRE_LOG(W) << "empty buffer not permitted";
+    }
+
+    int n_sent = sendto(socket_id_, buffer.begin(), buffer.size(), MSG_DONTWAIT,
+            reinterpret_cast<struct sockaddr*>(&remote_addr_), sizeof(remote_addr_));
+    if (n_sent < 0) {
+        // If sendto returns -1 an errno is set to indicate the error.
+        auto err = sock_err{};
+        if (err.error_number == EAGAIN || err.error_number == EWOULDBLOCK) {
+            return std::nullopt;
+        } else {
+            FIBRE_LOG(E) << "Socket write failed: " << err;
+            return {{kStreamError, buffer.end()}}; // the function might have written to the buffer
+        }
+
+    } else if ((size_t)n_sent > buffer.size()) {
+        FIBRE_LOG(E) << "sent too many bytes";
+        return {{kStreamError, buffer.end()}};
+
+    } else if (n_sent == 0) {
+        FIBRE_LOG(D) << "socket closed (TX half)";
+        return {{kStreamClosed, buffer.begin()}};
+
+    } else {
+        FIBRE_LOG(D) << "Sent " << n_sent << " bytes to " << remote_addr_;
+        return {{kStreamOk, buffer.begin() + n_sent}};
+    }
+}
+
+void PosixSocket::update_subscription() {
+    uint32_t new_mask = (tx_callback_ ? EPOLLOUT : 0)
+                      | (rx_callback_ ? EPOLLIN : 0);
+    if (new_mask != mask_) {
+        if (mask_) {
+            event_loop_->deregister_event(socket_id_);
+        }
+        mask_ = new_mask;
+        if (new_mask) {
+            event_loop_->register_event(socket_id_, new_mask, MEMBER_CB(this, on_event));
+        }
+    }
+}
+
+void PosixSocket::on_event(uint32_t mask) {
+
+    if (mask & EPOLLIN) {
+        // The socket is ready for RX. If an RX request is pending, handle it
+        // here, otherwise ignore the event.
+
+        if (rx_callback_) {
+            auto result = read_sync(rx_buf_);
+            rx_buf_ = {};
+            if (result.has_value()) {
+                rx_callback_.invoke_and_clear(*result);
+            }
+        }
+    }
+
+    if (mask & EPOLLOUT) {
+        // The socket is ready for RX. If an RX request is pending, handle it
+        // here, otherwise ignore the event.
+
+        if (tx_callback_) {
+            auto result = write_sync(tx_buf_);
+            tx_buf_ = {};
+            if (result.has_value()) {
+                tx_callback_.invoke_and_clear(*result);
+            }
+        }
+    }
+
+    if (mask & ~(EPOLLIN | EPOLLOUT)) {
+        FIBRE_LOG(E) << "unknown event mask: " << as_hex(mask);
+    }
+
+    update_subscription();
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/posix_socket.hpp

@@ -0,0 +1,173 @@
+#ifndef __FIBRE_POSIX_SOCKET_HPP
+#define __FIBRE_POSIX_SOCKET_HPP
+
+#include <fibre/event_loop.hpp>
+#include <netinet/in.h>
+#include <string>
+#include <fibre/cpp_utils.hpp>
+#include <fibre/bufptr.hpp>
+#include <fibre/async_stream.hpp>
+
+namespace fibre {
+
+
+#if defined(__linux__)
+//using PosixSocketWorker = LinuxWorker; // TODO: rename to EPollWorker or LinuxEPollWorker
+using socket_id_t = int;
+#elif defined(_WIN32) || defined(_WIN64)
+//using PosixSocketWorker = PosixPollWorker;
+using socket_id_t = SOCKET;
+#else
+//using PosixSocketWorker = KQueueWorker;
+using socket_id_t = int;
+#endif
+
+#if defined(_Win32) || defined(_Win64)
+#define IS_INVALID_SOCKET(socket_id)    (socket_id == INVALID_SOCKET)
+#else
+#define INVALID_SOCKET (-1)
+#define IS_INVALID_SOCKET(socket_id)    (socket_id < 0)
+#endif
+
+struct AddressResolutionContext;
+struct ConnectionContext;
+
+/**
+ * @brief Starts resolving a hostname (such as www.google.com) to one or
+ * multiple IP addresses.
+ * 
+ * If available, both IPv4 and IPv6 addresses are returned.
+ * 
+ * @param passive: If false, the returned address will be suitable for use with
+ *        connect(2), sendto(2), or sendmsg(2).
+ * @param callback: Invoked for every address that is found. Invoked with null
+ *        if no more addresses are available, including in case of an error or
+ *        cancellation.
+ * 
+ * @returns: false if the lookup could not be started. `callback` will not be
+ *           called.
+ */
+bool start_resolving_address(EventLoop* event_loop,
+    std::tuple<std::string, int> address, bool passive,
+    AddressResolutionContext** handle,
+    Callback<void, std::optional<cbufptr_t>> callback);
+
+/**
+ * @brief Cancels the ongoing address resolution.
+ * 
+ * The cancellation is complete once the associated callback is invoked with
+ * null.
+ */
+void cancel_resolving_address(AddressResolutionContext* handle);
+
+/**
+ * @brief Starts connecting to the specified address
+ * 
+ * @param addr: The address to connect to. Usually this buffer contains an
+ *        address of the type `struct sockaddr`. The family parameter of this
+ *        address will be passed as 1st argument to socket().
+ * @param type: Will be passed as 2nd argument to socket(). Can be for
+ *        instance SOCK_DGRAM or SOCKET_STREAM.
+ * @param protocol: Will be passed as 3rd argument to socket(). Can be for
+ *        instance IPPROTO_UDP or IPPROTO_TCP.
+ * @param on_connected: Called when the connection attempt succeeds or fails.
+ *        If the connection was established, the socket ID is passed to the
+ *        callback. This socket ID will only be valid for the duration of the
+ *        callback and must be duplicated (dup) if the application intends to
+ *        keep using it.
+ *        If the connection failed, std::nullopt is passed.
+ */
+bool start_connecting(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected);
+void stop_connecting(ConnectionContext* ctx);
+
+/**
+ * @brief Starts listening and accepting connections on the specified local
+ * address.
+ *
+ * @param addr: The local address to listen on. Usually this buffer contains an
+ *        address of the type `struct sockaddr`. The family parameter of this
+ *        address will be passed as 1st argument to socket().
+ * @param type: Will be passed as 2nd argument to socket(). Can be for
+ *        instance SOCK_DGRAM or SOCKET_STREAM.
+ * @param protocol: Will be passed as 3rd argument to socket(). Can be for
+ *        instance IPPROTO_UDP or IPPROTO_TCP.
+ * @param on_connected: Called for every connection that is accepted. The new
+ *        socket ID is passed to the callback. This socket ID will only be valid
+ *        for the duration of the callback and must be duplicated (dup) if the
+ *        application intends to keep using it.
+ *        If the attempt to listen fails permanently or is cancelled,
+ *        std::nullopt is passed.
+ */
+bool start_listening(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected);
+void stop_listening(ConnectionContext* ctx);
+
+/**
+ * @brief AsyncStreamSource and AsyncStreamSink based on a Posix or WinSock
+ * socket ID.
+ * 
+ * Note: To make this work on Windows, a "poll"-based worker must be implemented.
+ */
+class PosixSocket final : public AsyncStreamSource, public AsyncStreamSink {
+public:
+    /**
+     * @brief Initializes the object with the given socket ID.
+     * 
+     * The socket must be bound to a local address before this function is
+     * called.
+     * 
+     * @param socket_id: For Unix-like systems this should be a file descriptor,
+     *        for Windows this should be a Windows Socket ID (as returned by
+     *        socket()).
+     *        The socket must be in non-blocking mode (opened with O_NONBLOCK).
+     *        The socket will internally be duplicated using dup() so it can be
+     *        closed after this call.
+     */
+    bool init(EventLoop* event_loop, socket_id_t socket_id);
+
+    /**
+     * @brief Deinits a socket that was initialized with init().
+     */
+    bool deinit();
+
+    void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) final;
+    void cancel_read(TransferHandle transfer_handle) final;
+
+    void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final;
+    void cancel_write(TransferHandle transfer_handle) final;
+
+    /**
+     * @brief Returns the remote address of this socket.
+     * 
+     * For connectionless sockets this is origin of the most recently received
+     * data and it is only valid from the moment something was actually received.
+     * 
+     * For connection-oriented sockets this address is valid as soon as the
+     * socket is initialized.
+     */
+    struct sockaddr_storage get_remote_address() const { return remote_addr_; }
+
+private:
+    std::optional<ReadResult> read_sync(bufptr_t buffer);
+    std::optional<WriteResult> write_sync(cbufptr_t buffer);
+    void update_subscription();
+    void on_event(uint32_t mask);
+
+    int socket_id_ = INVALID_SOCKET;
+    EventLoop* event_loop_ = nullptr;
+    struct sockaddr_storage remote_addr_ = {0}; // updated after each RX event
+    uint32_t mask_ = 0; // current event subscription mask
+    bufptr_t rx_buf_{}; // valid while there is an RX request pending
+    cbufptr_t tx_buf_{}; // valid while there is a TX request pending
+    Callback<void, ReadResult> rx_callback_; // valid while there is an RX request pending
+    Callback<void, WriteResult> tx_callback_; // valid while there is a TX request pending
+};
+
+}
+
+#include <iostream>
+
+namespace std {
+std::ostream& operator<<(std::ostream& stream, const struct sockaddr_storage& val);
+}
+
+#endif // __FIBRE_POSIX_SOCKET_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/posix_tcp_backend.cpp

@@ -0,0 +1,138 @@
+
+#include "posix_tcp_backend.hpp"
+#include "posix_socket.hpp"
+#include "../logging.hpp"
+#include <fibre/fibre.hpp>
+#include <signal.h>
+#include <unistd.h>
+#include <algorithm>
+#include <string.h>
+
+DEFINE_LOG_TOPIC(TCP);
+USE_LOG_TOPIC(TCP);
+
+using namespace fibre;
+
+bool PosixTcpBackend::init(EventLoop* event_loop) {
+    if (event_loop_) {
+        FIBRE_LOG(E) << "already initialized";
+        return false;
+    }
+    event_loop_ = event_loop;
+    return true;
+}
+
+bool PosixTcpBackend::deinit() {
+    if (!event_loop_) {
+        FIBRE_LOG(E) << "not initialized";
+        return false;
+    }
+    if (n_discoveries_) {
+        FIBRE_LOG(W) << "some discoveries still ongoing";
+    }
+    event_loop_ = nullptr;
+    return true;
+}
+
+void PosixTcpBackend::start_channel_discovery(Domain* domain, const char* specs, size_t specs_len, ChannelDiscoveryContext** handle) {
+    const char* address_begin;
+    const char* address_end;
+    int port;
+
+    if (!event_loop_) {
+        FIBRE_LOG(E) << "not initialized";
+        //on_found_channels.invoke({kFibreInvalidArgument, nullptr, nullptr, 0});
+        return; // TODO: error reporting
+    }
+
+    if (!try_parse_key(specs, specs + specs_len, "address", &address_begin, &address_end)) {
+        FIBRE_LOG(E) << "no address specified";
+        //on_found_channels.invoke({kFibreInvalidArgument, nullptr, nullptr, 0});
+        return; // TODO: error reporting
+    }
+
+    if (!try_parse_key(specs, specs + specs_len, "port", &port)) {
+        FIBRE_LOG(E) << "no port specified";
+        //on_found_channels.invoke({kFibreInvalidArgument, nullptr, nullptr, 0});
+        return; // TODO: error reporting
+    }
+
+    n_discoveries_++;
+
+    TcpChannelDiscoveryContext* ctx = new TcpChannelDiscoveryContext(); // TODO: free
+    ctx->parent = this;
+    ctx->address = {{address_begin, address_end}, port};
+    ctx->domain = domain;
+    ctx->resolve_address();
+}
+
+int PosixTcpBackend::stop_channel_discovery(ChannelDiscoveryContext* handle) {
+    // TODO
+    n_discoveries_--;
+    return 0;
+}
+
+void PosixTcpBackend::TcpChannelDiscoveryContext::resolve_address() {
+    if (addr_resolution_ctx) {
+        FIBRE_LOG(E) << "already resolving";
+        return;
+    }
+
+    if (!start_resolving_address(parent->event_loop_, address, false, &addr_resolution_ctx, MEMBER_CB(this, on_found_address))) {
+        FIBRE_LOG(E) << "cannot start address resolution";
+        return;
+    }
+}
+
+void PosixTcpBackend::TcpChannelDiscoveryContext::on_found_address(std::optional<cbufptr_t> addr) {
+    FIBRE_LOG(D) << "found address";
+
+    if (addr.has_value()) {
+        // Resolved an address. If it wasn't already known, try to connect to it.
+        std::vector<uint8_t> vec{addr->begin(), addr->end()};
+        bool is_known = std::find_if(known_addresses.begin(), known_addresses.end(),
+            [&](AddrContext& val){ return val.addr == vec; }) != known_addresses.end();
+
+        if (!is_known) {
+            AddrContext ctx = {.addr = vec};
+            if (parent->start_opening_connections(parent->event_loop_, *addr, SOCK_STREAM, IPPROTO_TCP, &ctx.connection_ctx, MEMBER_CB(this, on_connected))) {
+                known_addresses.push_back(ctx);
+            } else {
+                // TODO
+            }
+        }
+    } else {
+        // No more addresses.
+        addr_resolution_ctx = nullptr;
+        if (known_addresses.size() == 0) {
+            // No addresses could be found. Try again using exponential backoff.
+            parent->event_loop_->call_later(lookup_period, MEMBER_CB(this, resolve_address));
+            lookup_period = std::min(lookup_period * 3.0f, 3600.0f); // exponential backoff with at most 1h period
+        } else {
+            // Some addresses are known from this lookup or from a previous
+            // lookup. Resolve addresses again in 1h.
+            parent->event_loop_->call_later(3600.0, MEMBER_CB(this, resolve_address));
+        }
+    }
+}
+
+void PosixTcpBackend::TcpChannelDiscoveryContext::on_connected(std::optional<socket_id_t> socket_id) {
+    if (socket_id.has_value()) {
+        auto socket = new PosixSocket{}; // TODO: free
+        if (socket->init(parent->event_loop_, *socket_id)) {
+            domain->add_channels({kFibreOk, socket, socket, SIZE_MAX});
+            return;
+        }
+        delete socket;
+    }
+
+    FIBRE_LOG(D) << "not connected";
+    // Try to reconnect soon
+    lookup_period = 1.0f;
+    resolve_address();
+}
+
+void PosixTcpBackend::TcpChannelDiscoveryContext::on_disconnected() {
+    lookup_period = 1.0f; // reset exponential backoff
+    resolve_address();
+}

ODrive-fw-v0.5.6/Firmware/fibre-cpp/platform_support/posix_tcp_backend.hpp

@@ -0,0 +1,80 @@
+#ifndef __FIBRE_POSIX_TCP_BACKEND_HPP
+#define __FIBRE_POSIX_TCP_BACKEND_HPP
+
+#include <fibre/event_loop.hpp>
+#include "posix_socket.hpp"
+#include <fibre/channel_discoverer.hpp>
+#include <string>
+#include <netdb.h>
+
+namespace fibre {
+
+/**
+ * TCP client and TCP server implementations are identical up to the function
+ * that is used to convert an address to one or more connected socket IDs.
+ * The client uses the posix function `connect` to do so, while the server uses
+ * the posix functions `listen` and `accept`.
+ */
+class PosixTcpBackend : public ChannelDiscoverer {
+public:
+    bool init(EventLoop* event_loop);
+    bool deinit();
+
+    void start_channel_discovery(Domain* domain, const char* specs, size_t specs_len, ChannelDiscoveryContext** handle) final;
+    int stop_channel_discovery(ChannelDiscoveryContext* handle) final;
+
+private:
+    struct TcpChannelDiscoveryContext {
+        PosixTcpBackend* parent;
+        std::tuple<std::string, int> address;
+        Domain* domain;
+        AddressResolutionContext* addr_resolution_ctx;
+        ConnectionContext* connection_ctx;
+        float lookup_period = 1.0f; // wait 1s for next address resolution
+
+        struct AddrContext {
+            std::vector<uint8_t> addr;
+            ConnectionContext* connection_ctx;
+        };
+
+        std::vector<AddrContext> known_addresses;
+        void resolve_address();
+        void on_found_address(std::optional<cbufptr_t> addr);
+        void on_connected(std::optional<socket_id_t> socket_id);
+        void on_disconnected();
+    };
+
+    virtual bool start_opening_connections(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected) = 0;
+    virtual void cancel_opening_connections(ConnectionContext* ctx) = 0;
+
+    EventLoop* event_loop_ = nullptr;
+    size_t n_discoveries_ = 0;
+};
+
+class PosixTcpClientBackend : public PosixTcpBackend {
+public:
+    constexpr static const char* get_name() { return "tcp-client"; }
+
+    bool start_opening_connections(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected) final {
+        return start_connecting(event_loop, addr, type, protocol, ctx, on_connected);
+    }
+    void cancel_opening_connections(ConnectionContext* ctx) final {
+        stop_connecting(ctx);
+    }
+};
+
+class PosixTcpServerBackend : public PosixTcpBackend {
+public:
+    constexpr static const char* get_name() { return "tcp-server"; }
+
+    bool start_opening_connections(EventLoop* event_loop, cbufptr_t addr, int type, int protocol, ConnectionContext** ctx, Callback<void, std::optional<socket_id_t>> on_connected) final {
+        return start_listening(event_loop, addr, type, protocol, ctx, on_connected);
+    }
+    void cancel_opening_connections(ConnectionContext* ctx) final {
+        stop_listening(ctx);
+    }
+};
+
+}
+
+#endif // __FIBRE_POSIX_TCP_BACKEND_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/print_utils.hpp

@@ -0,0 +1,137 @@
+#ifndef __FIBRE_PRINT_UTILS_HPP
+#define __FIBRE_PRINT_UTILS_HPP
+
+#include <string>
+#include <ostream>
+#include <fibre/bufptr.hpp>
+
+namespace fibre {
+
+template<typename T>
+constexpr size_t hex_digits() {
+    return (std::numeric_limits<T>::digits + 3) / 4;
+}
+
+/* @brief Converts a hexadecimal digit to a uint8_t.
+* @param output If not null, the digit's value is stored in this output
+* Returns true if the char is a valid hex digit, false otherwise
+*/
+static inline bool hex_digit_to_byte(char ch, uint8_t* output) {
+    uint8_t nil_output = 0;
+    if (!output)
+        output = &nil_output;
+    if (ch >= '0' && ch <= '9')
+        return (*output) = ch - '0', true;
+    if (ch >= 'a' && ch <= 'f')
+        return (*output) = ch - 'a' + 10, true;
+    if (ch >= 'A' && ch <= 'F')
+        return (*output) = ch - 'A' + 10, true;
+    return false;
+}
+
+/* @brief Converts a hex string to an integer
+* @param output If not null, the result is stored in this output
+* Returns true if the string represents a valid hex value, false otherwise.
+*/
+template<typename TInt>
+bool hex_string_to_int(const char * str, size_t length, TInt* output) {
+    constexpr size_t N_DIGITS = hex_digits<TInt>();
+    TInt result = 0;
+    if (length > N_DIGITS)
+        length = N_DIGITS;
+    for (size_t i = 0; i < length && str[i]; i++) {
+        uint8_t digit = 0;
+        if (!hex_digit_to_byte(str[i], &digit))
+            return false;
+        result <<= 4;
+        result += digit;
+    }
+    if (output)
+        *output = result;
+    return true;
+}
+
+template<typename TInt>
+bool hex_string_to_int(const char * str, TInt* output) {
+    return hex_string_to_int<TInt>(str, hex_digits<TInt>(), output);
+}
+
+template<typename TInt, size_t ICount>
+bool hex_string_to_int_arr(const char * str, size_t length, TInt (&output)[ICount]) {
+    for (size_t i = 0; i < ICount; i++) {
+        if (!hex_string_to_int<TInt>(&str[i * hex_digits<TInt>()], &output[i]))
+            return false;
+    }
+    return true;
+}
+
+template<typename TInt, size_t ICount>
+bool hex_string_to_int_arr(const char * str, TInt (&output)[ICount]) {
+    return hex_string_to_int_arr(str, hex_digits<TInt>() * ICount, output);
+}
+
+// TODO: move to print_utils.hpp
+template<typename T>
+class HexPrinter {
+public:
+    HexPrinter(T val, bool prefix) : val_(val) /*, prefix_(prefix)*/ {
+        const char digits[] = "0123456789abcdef";
+        size_t prefix_length = prefix ? 2 : 0;
+        if (prefix) {
+            str[0] = '0';
+            str[1] = 'x';
+        }
+        str[prefix_length + hex_digits<T>()] = '\0';
+        
+        for (size_t i = 0; i < hex_digits<T>(); ++i) {
+            str[prefix_length + hex_digits<T>() - i - 1] = digits[val & 0xf];
+            val >>= 4;
+        }
+    }
+    std::string to_string() const { return str; }
+    void to_string(char* buf) const {
+        for (size_t i = 0; (i < sizeof(str)) && str[i]; ++i)
+            buf[i] = str[i];
+    }
+
+    T val_;
+    //bool prefix_;
+    char str[hex_digits<T>() + 3]; // 3 additional characters 0x and \0
+};
+
+template<typename T>
+std::ostream& operator<<(std::ostream& stream, const HexPrinter<T>& printer) {
+    // TODO: specialize for char
+    return stream << printer.to_string();
+}
+
+template<typename T>
+HexPrinter<T> as_hex(T val, bool prefix = true) { return HexPrinter<T>(val, prefix); }
+
+template<typename T>
+class HexArrayPrinter {
+public:
+    HexArrayPrinter(T* ptr, size_t length) : ptr_(ptr), length_(length) {}
+    T* ptr_;
+    size_t length_;
+};
+
+template<typename TStream, typename T>
+TStream& operator<<(TStream& stream, const HexArrayPrinter<T>& printer) {
+    for (size_t pos = 0; pos < printer.length_; ++pos) {
+        stream << " " << as_hex(printer.ptr_[pos]);
+        if (((pos + 1) % 16) == 0)
+            stream << "\n";
+    }
+    return stream;
+}
+
+template<typename T, size_t ILength>
+HexArrayPrinter<T> as_hex(T (&val)[ILength]) { return HexArrayPrinter<T>(val, ILength); }
+
+template<typename T>
+HexArrayPrinter<T> as_hex(generic_bufptr_t<T> buffer) { return HexArrayPrinter<T>(buffer.begin(), buffer.size()); }
+
+}
+
+#endif // __FIBRE_PRINT_UTILS_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/protocol.hpp

@@ -0,0 +1,310 @@
+/*
+see protocol.md for the protocol specification
+*/
+
+#ifndef __PROTOCOL_HPP
+#define __PROTOCOL_HPP
+
+#include <functional>
+#include <limits>
+#include <cmath>
+//#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <cstring>
+#include <fibre/cpp_utils.hpp>
+#include <fibre/bufptr.hpp>
+#include <fibre/simple_serdes.hpp>
+
+
+typedef struct {
+    uint16_t json_crc;
+    uint16_t endpoint_id;
+} endpoint_ref_t;
+
+
+namespace fibre {
+// These symbols are defined in the autogenerated endpoints.hpp
+extern const unsigned char embedded_json[];
+extern const size_t embedded_json_length;
+extern const uint16_t json_crc_;
+extern const uint32_t json_version_id_;
+bool endpoint_handler(int idx, cbufptr_t* input_buffer, bufptr_t* output_buffer);
+bool endpoint0_handler(cbufptr_t* input_buffer, bufptr_t* output_buffer);
+bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref);
+bool set_endpoint_from_float(endpoint_ref_t endpoint_ref, float value);
+}
+
+
+
+namespace fibre {
+template<typename T, typename = void>
+struct Codec {
+    static std::optional<T> decode(cbufptr_t* buffer) { return std::nullopt; }
+};
+
+template<> struct Codec<bool> {
+    static std::optional<bool> decode(cbufptr_t* buffer) { return (buffer->begin() == buffer->end()) ? std::nullopt : std::make_optional((bool)*(buffer->begin()++)); }
+    static bool encode(bool value, bufptr_t* buffer) { return SimpleSerializer<uint8_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<int8_t> {
+    static std::optional<int8_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int8_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(int8_t value, bufptr_t* buffer) { return SimpleSerializer<int8_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<uint8_t> {
+    static std::optional<uint8_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint8_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(uint8_t value, bufptr_t* buffer) { return SimpleSerializer<uint8_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<int16_t> {
+    static std::optional<int16_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int16_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(int16_t value, bufptr_t* buffer) { return SimpleSerializer<int16_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<uint16_t> {
+    static std::optional<uint16_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(uint16_t value, bufptr_t* buffer) { return SimpleSerializer<uint16_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<int32_t> {
+    static std::optional<int32_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int32_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(int32_t value, bufptr_t* buffer) { return SimpleSerializer<int32_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<uint32_t> {
+    static std::optional<uint32_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint32_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(uint32_t value, bufptr_t* buffer) { return SimpleSerializer<uint32_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<int64_t> {
+    static std::optional<int64_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int64_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(int64_t value, bufptr_t* buffer) { return SimpleSerializer<int64_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<uint64_t> {
+    static std::optional<uint64_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint64_t, false>::read(&(buffer->begin()), buffer->end()); }
+    static bool encode(uint64_t value, bufptr_t* buffer) { return SimpleSerializer<uint64_t, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<float> {
+    static std::optional<float> decode(cbufptr_t* buffer) {
+        std::optional<uint32_t> int_val = Codec<uint32_t>::decode(buffer);
+        return int_val.has_value() ? std::optional<float>(*reinterpret_cast<float*>(&*int_val)) : std::nullopt;
+    }
+    static bool encode(float value, bufptr_t* buffer) {
+        void* ptr = &value;
+        return Codec<uint32_t>::encode(*reinterpret_cast<uint32_t*>(ptr), buffer);
+    }
+};
+template<typename T>
+struct Codec<T, std::enable_if_t<std::is_enum<T>::value>> {
+    using int_type = std::underlying_type_t<T>;
+    static std::optional<T> decode(cbufptr_t* buffer) {
+        std::optional<int_type> int_val = SimpleSerializer<int_type, false>::read(&(buffer->begin()), buffer->end());
+        return int_val.has_value() ? std::make_optional(static_cast<T>(*int_val)) : std::nullopt;
+    }
+    static bool encode(T value, bufptr_t* buffer) { return SimpleSerializer<int_type, false>::write(value, &(buffer->begin()), buffer->end()); }
+};
+template<> struct Codec<endpoint_ref_t> {
+    static std::optional<endpoint_ref_t> decode(cbufptr_t* buffer) {
+        std::optional<uint16_t> val0 = SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end());
+        std::optional<uint16_t> val1 = SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end());
+        return (val0.has_value() && val1.has_value()) ? std::make_optional(endpoint_ref_t{*val1, *val0}) : std::nullopt;
+    }
+    static bool encode(endpoint_ref_t value, bufptr_t* buffer) {
+        return SimpleSerializer<uint16_t, false>::write(value.endpoint_id, &(buffer->begin()), buffer->end())
+            && SimpleSerializer<uint16_t, false>::write(value.json_crc, &(buffer->begin()), buffer->end());
+    }
+};
+}
+
+
+/* ToString / FromString functions -------------------------------------------*/
+/*
+* These functions are currently not used by Fibre and only here to
+* support the ODrive ASCII protocol.
+* TODO: find a general way for client code to augment endpoints with custom
+* functions
+*/
+
+template<typename T>
+struct format_traits_t;
+
+// template<> struct format_traits_t<float> { using type = void;
+//     static constexpr const char * fmt = "%f";
+//     static constexpr const char * fmtp = "%f";
+// };
+template<> struct format_traits_t<long long> { using type = void;
+    static constexpr const char * fmt = "%lld";
+    static constexpr const char * fmtp = "%lld";
+    using scn_type = long long;
+};
+template<> struct format_traits_t<unsigned long long> { using type = void;
+    static constexpr const char * fmt = "%llu";
+    static constexpr const char * fmtp = "%llu";
+    using scn_type = unsigned long long;
+};
+template<> struct format_traits_t<long> { using type = void;
+    static constexpr const char * fmt = "%ld";
+    static constexpr const char * fmtp = "%ld";
+    using scn_type = long;
+};
+template<> struct format_traits_t<unsigned long> { using type = void;
+    static constexpr const char * fmt = "%lu";
+    static constexpr const char * fmtp = "%lu";
+    using scn_type = unsigned long;
+};
+template<> struct format_traits_t<int> { using type = void;
+    static constexpr const char * fmt = "%d";
+    static constexpr const char * fmtp = "%d";
+    using scn_type = int;
+};
+template<> struct format_traits_t<unsigned int> { using type = void;
+    static constexpr const char * fmt = "%u";
+    static constexpr const char * fmtp = "%u";
+    using scn_type = unsigned int;
+};
+template<> struct format_traits_t<short> { using type = void;
+    static constexpr const char * fmt = "%d";
+    static constexpr const char * fmtp = "%d";
+    using scn_type = int;
+};
+template<> struct format_traits_t<unsigned short> { using type = void;
+    static constexpr const char * fmt = "%u";
+    static constexpr const char * fmtp = "%u";
+    using scn_type = unsigned int;
+};
+template<> struct format_traits_t<char> { using type = void;
+    static constexpr const char * fmt = "%d";
+    static constexpr const char * fmtp = "%d";
+    using scn_type = int;
+};
+template<> struct format_traits_t<unsigned char> { using type = void;
+    static constexpr const char * fmt = "%u";
+    static constexpr const char * fmtp = "%u";
+    using scn_type = unsigned int;
+};
+
+template<typename T, typename = typename format_traits_t<T>::type>
+static bool to_string(const T& value, char * buffer, size_t length, int) {
+    snprintf(buffer, length, format_traits_t<T>::fmtp, value);
+    return true;
+}
+// Special case for float because printf promotes float to double, and we get warnings
+template<typename T = float>
+static bool to_string(const float& value, char * buffer, size_t length, int) {
+    snprintf(buffer, length, "%f", (double)value);
+    return true;
+}
+template<typename T = bool>
+static bool to_string(const bool& value, char * buffer, size_t length, int) {
+    buffer[0] = value ? '1' : '0';
+    buffer[1] = 0;
+    return true;
+}
+template<typename T>
+static bool to_string(const T& value, char * buffer, size_t length, ...) {
+    return false;
+}
+
+template<typename T, typename = typename format_traits_t<T>::type>
+static bool from_string(const char * buffer, size_t length, T* property, int) {
+    // sscanf doesn't work well with integers that are smaller than int, so we
+    // first scan into an appropriate int type and then convert it.
+    // (e.g. %hhu has been observed to not work on some compilers)
+    typename format_traits_t<T>::scn_type val;
+    if (sscanf(buffer, format_traits_t<T>::fmt, &val) == 1) {
+        *property = (T)val;
+        return true;
+    } else {
+        return false;
+    }
+}
+// Special case for float because printf promotes float to double, and we get warnings
+template<typename T = float>
+static bool from_string(const char * buffer, size_t length, float* property, int) {
+    return sscanf(buffer, "%f", property) == 1;
+}
+template<typename T = bool>
+static bool from_string(const char * buffer, size_t length, bool* property, int) {
+    int val;
+    if (sscanf(buffer, "%d", &val) != 1)
+        return false;
+    *property = val;
+    return true;
+}
+template<typename T>
+static bool from_string(const char * buffer, size_t length, T* property, ...) {
+    return false;
+}
+
+
+//template<typename T, typename = typename std>
+//bool set_from_float_ex(float value, T* property) {
+//    return false;
+//}
+
+namespace conversion {
+//template<typename T>
+template<typename T>
+bool set_from_float_ex(float value, float* property, int) {
+    return *property = value, true;
+}
+template<typename T>
+bool set_from_float_ex(float value, bool* property, int) {
+    return *property = (value >= 0.0f), true;
+}
+template<typename T, typename = std::enable_if_t<std::is_integral<T>::value && !std::is_const<T>::value>>
+bool set_from_float_ex(float value, T* property, int) {
+    return *property = static_cast<T>(std::round(value)), true;
+}
+template<typename T>
+bool set_from_float_ex(float value, T* property, ...) {
+    return false;
+}
+template<typename T>
+bool set_from_float(float value, T* property) {
+    return set_from_float_ex<T>(value, property, 0);
+}
+}
+
+
+template<typename T>
+struct Property {
+    Property(void* ctx, T(*getter)(void*), void(*setter)(void*, T))
+        : ctx_(ctx), getter_(getter), setter_(setter) {}
+    Property(T* ctx)
+        : ctx_(ctx), getter_([](void* ctx){ return *(T*)ctx; }), setter_([](void* ctx, T val){ *(T*)ctx = val; }) {}
+    Property& operator*() { return *this; }
+    Property* operator->() { return this; }
+
+    T read() const {
+        return (*getter_)(ctx_);
+    }
+
+    T exchange(std::optional<T> value) const {
+        T old_value = (*getter_)(ctx_);
+        if (value.has_value()) {
+            (*setter_)(ctx_, *value);
+        }
+        return old_value;
+    }
+    
+    void* ctx_;
+    T(*getter_)(void*);
+    void(*setter_)(void*, T);
+};
+
+template<typename T>
+struct Property<const T> {
+    Property(void* ctx, T(*getter)(void*))
+        : ctx_(ctx), getter_(getter) {}
+    Property(const T* ctx)
+        : ctx_(const_cast<T*>(ctx)), getter_([](void* ctx){ return *(const T*)ctx; }) {}
+    Property& operator*() { return *this; }
+    Property* operator->() { return this; }
+
+    T read() const {
+        return (*getter_)(ctx_);
+    }
+
+    void* ctx_;
+    T(*getter_)(void*);
+};
+
+
+#endif

ODrive-fw-v0.5.6/Firmware/fibre-cpp/stream_utils.hpp

@@ -0,0 +1,188 @@
+#ifndef __FIBRE_STREAM_UTILS_HPP
+#define __FIBRE_STREAM_UTILS_HPP
+
+#include <fibre/async_stream.hpp>
+#include <string.h>
+
+namespace fibre {
+
+template<size_t I>
+class BufferedStreamSink {
+public:
+    BufferedStreamSink(AsyncStreamSink& sink) : sink_(sink) {}
+
+    /**
+     * @brief Enqueues as much of the specified buffer as possible.
+     * 
+     * Thread safety: only one write call is allowed at a time. The write call
+     * can be on a different thread from the underlying stream's event loop.
+     * (TODO: this is not true yet, see comment in function)
+     */
+    void write(cbufptr_t buf) {
+        size_t read_idx = read_idx_; // read_idx_ could change during this function
+
+        if ((read_idx + 1) % I == write_idx_) {
+            return;
+        }
+
+        // We subtract 1 from the read index because we never want the write
+        // pointer to catch up with the read pointer, cause then
+        // `write_idx_ == read_idx_` could mean both "full" and "empty".
+
+        read_idx = (read_idx + I - 1) % I; 
+
+        if (write_idx_ > read_idx) {
+            size_t n_copy = std::min(I - write_idx_, buf.size());
+            memcpy(buffer_ + write_idx_, buf.begin(), n_copy);
+            write_idx_ = (write_idx_ + n_copy) % I;
+            buf = buf.skip(n_copy);
+        }
+
+        size_t n_copy = std::min(read_idx - write_idx_, buf.size());
+        memcpy(buffer_ + write_idx_, buf.begin(), n_copy);
+        write_idx_ = (write_idx_ + n_copy) % I;
+
+        //if (!__atomic_exchange_n(&is_active_, true, __ATOMIC_SEQ_CST)) {
+        //    // TODO: calling the sink in here breaks the rule that async
+        //    // functions must only be called on the event loop thread.
+        //    // But to do that we need to implement a proper event loop where we
+        //    // can enqueue calls.
+        //    maybe_start_async_write();
+        //}
+    }
+
+    void maybe_start_async_write() {
+        if (is_active_) {
+            // nothing to do
+        } else if (read_idx_ < write_idx_) {
+            is_active_ = true;
+            sink_.start_write({buffer_ + read_idx_, buffer_ + write_idx_}, &transfer_handle_, MEMBER_CB(this, on_write_complete));
+        } else if (read_idx_ > write_idx_) {
+            is_active_ = true;
+            sink_.start_write({buffer_ + read_idx_, buffer_ + I}, &transfer_handle_, MEMBER_CB(this, on_write_complete));
+        } else {
+            // nothing to do
+        }
+    }
+
+private:
+    void on_write_complete(WriteResult result) {
+        is_active_ = false;
+        transfer_handle_ = 0;
+
+        if (result.status == kStreamOk) {
+            if (result.end < buffer_ || result.end > (buffer_ + I)) {
+                for (;;)
+                    transfer_handle_ = 0;
+            }
+            read_idx_ = (result.end - buffer_) % I;
+            maybe_start_async_write();
+        }
+    }
+
+    uint8_t buffer_[I];
+
+    // Both indices are in [0, I)
+    // They are equal if the buffer is empty (no valid data).
+    size_t write_idx_ = 0; // [0, I)
+    size_t read_idx_ = 0; // [0, I)
+    bool is_active_ = false;
+    TransferHandle transfer_handle_ = 0;
+
+    AsyncStreamSink& sink_;
+};
+
+/**
+ * @brief Buffers up to NSlots concurrent async write requests.
+ * 
+ * This can be used to wrap sinks that can only handle one concurrent write
+ * operation at a time but are written to by multiple independent sources.
+ */
+template<size_t NSlots>
+class AsyncStreamSinkMultiplexer : public AsyncStreamSink {
+public:
+    AsyncStreamSinkMultiplexer(AsyncStreamSink& sink) : sink_(sink) {}
+
+    void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final {
+        for (size_t i = 0; i < NSlots; ++i) {
+            auto& [slot_in_use, slot_buf, slot_completer] = slots_[i];
+            if (!__atomic_exchange_n(&slot_in_use, true, __ATOMIC_SEQ_CST)) {
+                slot_buf = buffer;
+                slot_completer = completer;
+
+                if (handle) {
+                    *handle = i + 1; // returning a valid handle of 0 is not a good idea
+                }
+
+                // If the underlying sink wasn't busy, start it now.
+                if (active_slot_ == 0) {
+                    active_slot_ = i + 1;
+                    sink_.start_write(slot_buf, &transfer_handle_, MEMBER_CB(this, on_write_complete));
+                }
+
+                return;
+            }
+        }
+
+        if (handle) {
+            *handle = 0;
+        }
+        completer.invoke({kStreamError, buffer.begin()});
+    }
+
+    void cancel_write(TransferHandle transfer_handle) final {
+        if (transfer_handle == active_slot_) {
+            // This transfer is the one that the underlying sink is busy with.
+            sink_.cancel_write(transfer_handle_);
+        } else {
+            // This transfer is only enqueued but not yet started.
+            auto& [slot_in_use, slot_buf, slot_completer] = slots_[transfer_handle - 1];
+            auto completer = slot_completer;
+            auto end = slot_buf.end();
+            slot_in_use = false;
+            completer.invoke_and_clear({kStreamCancelled, end});
+        }
+    }
+
+private:
+    void on_write_complete(fibre::WriteResult result) {
+        transfer_handle_ = 0;
+
+        auto& [slot_in_use, slot_buf, slot_completer] =  slots_[active_slot_ - 1];
+        (void) slot_buf;
+        auto completer = slot_completer;
+        slot_in_use = false;
+
+        completer.invoke_and_clear(result);
+
+        // Select new slot before announcing completion of the old
+        size_t active_slot = 0;
+        for (size_t i = 0; i < NSlots; ++i) {
+            auto& [slot_in_use, slot_buf, slot_completer] = slots_[i];
+            (void) slot_buf;
+            (void) slot_completer;
+            if (slot_in_use) {
+                active_slot = i + 1;
+                break;
+            }
+        }
+
+        // Start next slot
+        active_slot_ = active_slot;
+        if (active_slot) {
+            auto& [slot_in_use, slot_buf, slot_completer] = slots_[active_slot - 1];
+            (void) slot_in_use;
+            (void) slot_completer;
+            sink_.start_write(slot_buf, &transfer_handle_, MEMBER_CB(this, on_write_complete));
+        }
+    }
+    
+    AsyncStreamSink& sink_;
+    std::tuple<bool, cbufptr_t, Callback<void, WriteResult>> slots_[NSlots];
+    size_t active_slot_ = 0;
+    TransferHandle transfer_handle_ = 0;
+};
+
+}
+
+#endif // __FIBRE_STREAM_UTILS_HPP

ODrive-fw-v0.5.6/Firmware/fibre-cpp/type_info_template.j2

@@ -0,0 +1,50 @@
+/*[# This is the original template, thus the warning below does not apply to this file #]
+ * ============================ WARNING ============================
+ * ==== This is an autogenerated file.                          ====
+ * ==== Any changes to this file will be lost when recompiling. ====
+ * =================================================================
+ *
+ * This file contains support functions for the ODrive ASCII protocol.
+ *
+ * TODO: might generalize this as an approach to runtime introspection.
+ */
+
+#include <fibre/introspection.hpp>
+
+#pragma GCC push_options
+#pragma GCC optimize ("s")
+
+[% for intf in interfaces.values() %][% if not intf.builtin %]
+template<typename T>
+struct [[intf.fullname | to_pascal_case]]TypeInfo : TypeInfo {
+    using TypeInfo::TypeInfo;
+    static const PropertyInfo property_table[];
+    static const [[intf.fullname | to_pascal_case]]TypeInfo<T> singleton;
+    static Introspectable make_introspectable(T& obj) { return TypeInfo::make_introspectable(&obj, &singleton); }
+
+    introspectable_storage_t get_child(introspectable_storage_t obj, size_t idx) const override {
+        T* ptr = *(T**)&obj;
+        introspectable_storage_t res;
+        switch (idx) {
+[%- for property in intf.get_all_attributes().values() %]
+            case [[loop.index0]]: *(decltype([[intf.c_name]]::get_[[property.name]](std::declval<T*>()))*)(&res) = [[intf.c_name]]::get_[[property.name]](ptr); break;
+[%- endfor %]
+        }
+        return res;
+    }
+};
+[% endif %][% endfor %]
+
+[% for intf in interfaces.values() %][% if not intf.builtin %]
+template<typename T>
+const PropertyInfo [[intf.fullname | to_pascal_case]]TypeInfo<T>::property_table[] = {
+[%- for property in intf.get_all_attributes().values() %]
+    {"[[property.name]]", &[[(property.type.purename or property.type.fullname) | to_pascal_case]]TypeInfo<std::remove_reference_t<decltype(*[[intf.c_name]]::get_[[property.name]](std::declval<T*>()))>>::singleton},
+[%- endfor %]
+};
+template<typename T>
+const [[intf.fullname | to_pascal_case]]TypeInfo<T> [[intf.fullname | to_pascal_case]]TypeInfo<T>::singleton{[[intf.fullname | to_pascal_case]]TypeInfo<T>::property_table, sizeof([[intf.fullname | to_pascal_case]]TypeInfo<T>::property_table) / sizeof([[intf.fullname | to_pascal_case]]TypeInfo<T>::property_table[0])};
+
+[% endif %][% endfor %]
+
+#pragma GCC pop_options