diff --git a/Firmware/fibre-cpp/.gitattributes b/Firmware/fibre-cpp/.gitattributes new file mode 100644 index 00000000..4a382938 --- /dev/null +++ b/Firmware/fibre-cpp/.gitattributes @@ -0,0 +1,3 @@ +libfibre*.so filter=lfs diff=lfs merge=lfs -text +libfibre*.dll filter=lfs diff=lfs merge=lfs -text +libfibre*.dylib filter=lfs diff=lfs merge=lfs -text diff --git a/Firmware/fibre-cpp/Makefile b/Firmware/fibre-cpp/Makefile new file mode 100644 index 00000000..8ae93b2c --- /dev/null +++ b/Firmware/fibre-cpp/Makefile @@ -0,0 +1,55 @@ + +# pkgconf --cflags libusb +# pkgconf --libs libusb + +# Prerequisites: +# Ubuntu: libusb-dev + +ifeq ($(OS),Windows_NT) + target = windows + ifeq ($(PROCESSOR_ARCHITEW6432),AMD64) + outname = libfibre-windows-amd64.dll + else + $(error unsupported platform) + endif +else ifeq ($(shell uname -s),Linux) + target = linux + ifeq ($(shell uname -m),x86_64) + outname = libfibre-linux-amd64.so + else ifeq (($(shell uname -m),armv7l) + outname = libfibre-linux-armhf.so + else + $(error unsupported platform) + endif +else ifeq ($(shell uname -s),Darwin) + target = macos + ifeq ($(shell uname -m),x86_64) + outname = libfibre-macos-multiarch.dylib + else + $(error unsupported platform) + endif +endif + + +FILES=libfibre.cpp \ + platform_support/libusb_transport.cpp \ + legacy_protocol.cpp \ + legacy_object_client.cpp \ + logging.cpp + +linux: + g++ -shared -o $(outname) -fPIC -std=c++11 -I/usr/include/libusb-1.0 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \ + $(FILES) \ + -lusb-1.0 + +linux-cross: + arm-linux-gnueabihf-g++ -march=armv7 -shared -o libfibre.so -fPIC -std=c++11 -I/usr/include/libusb-1.0 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \ + $(FILES) \ + -lusb + +windows: + g++ -shared -o $(outname) -fPIC -std=c++11 -I./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \ + $(FILES) \ + -static-libgcc -Wl,-Bstatic -lstdc++ ./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a -Wl,-Bdynamic + +# nm -D libfibre.so | grep T diff --git a/Firmware/fibre-cpp/README.md b/Firmware/fibre-cpp/README.md new file mode 100644 index 00000000..33be1144 --- /dev/null +++ b/Firmware/fibre-cpp/README.md @@ -0,0 +1,38 @@ + +## Platform Compatibility + +| | Windows | macOS [1] | Linux | +|-----|---------|-----------|-------| +| USB | yes | yes | yes | + + - [1] macOS 10.9 (Mavericks) or later + +## `libfibre` API + +Refer to [libfibre.h](include/fibre/libfibre.h) for documentation of the API. + +## Build instructions + +### 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 libusb-1.0-dev` + 2. Navigate to this directory and run `make` + +### macOS + 1. `brew install libusb` + 2. Navigate to this directory and run `make` + +### Cross-compile `libfibre` on Linux for all other platforms +The file `./compile_for_all_platforms.sh` cross-compiles libfibre for all supported platforms. This is mainly intended for CI to generate releases. It written to run on Arch Linux only. Check the script to see which packages need to be installed first. + + +## 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`) diff --git a/Firmware/fibre-cpp/async_stream.hpp b/Firmware/fibre-cpp/async_stream.hpp new file mode 100644 index 00000000..bb233e00 --- /dev/null +++ b/Firmware/fibre-cpp/async_stream.hpp @@ -0,0 +1,188 @@ +#ifndef __FIBRE_ASYNC_STREAM_HPP +#define __FIBRE_ASYNC_STREAM_HPP + +#include "include/fibre/bufptr.hpp" // TODO: move this header +#include + +namespace fibre { + +enum StreamStatus { + kStreamOk, + kStreamCancelled, + kStreamClosed, + kStreamError +}; + +template +class Completer { +public: + virtual void complete(TResults ... result) = 0; + + static Completer& get_dummy() { + static struct DummyCompleter : Completer { + void complete(TResults ... result) {} + } dummy; + return dummy; + } +}; + +/** + * @brief Safe wrapper around Completer::complete. + * + * This function takes a reference to a completer pointer and only invokes the + * completer if it's not null. Before invoking the completer, the pointer is + * cleared. + */ +template +static void safe_complete(Completer*& completer, TResults ... results) { + Completer* tmp = completer; + completer = nullptr; + if (tmp) { + tmp->complete(results...); + } +} + +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; +}; + +struct WriteCompleter : Completer { + virtual void on_write_finished(WriteResult result) = 0; + + void complete(WriteResult result) final { + on_write_finished(result); + } +}; + +struct ReadCompleter : Completer { + virtual void on_read_finished(ReadResult result) = 0; + + void complete(ReadResult result) final { + on_read_finished(result); + } +}; + + +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, Completer& 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, Completer& 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 \ No newline at end of file diff --git a/Firmware/fibre-cpp/compile_for_all_platforms.sh b/Firmware/fibre-cpp/compile_for_all_platforms.sh new file mode 100755 index 00000000..ca01651d --- /dev/null +++ b/Firmware/fibre-cpp/compile_for_all_platforms.sh @@ -0,0 +1,178 @@ +#!/bin/bash +set -euo pipefail + +# Prerequisites: +# Arch Linux: +# gcc binutils +# arm-linux-gnueabihf-gcc arm-linux-gnueabihf-binutils +# mingw-w64-gcc mingw-w64-binutils +# p7zip +# apple-darwin-osxcross + +# TODO: support C++11 + +mkdir -p third_party + +function download_deb_pkg() { + 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 +} + +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 +} + +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" +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" +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" +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" +download_deb_pkg libusb-armhf "http://mirrordirector.raspbian.org/raspbian/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.23-2_armhf.deb" +download_deb_pkg libusb-dev-armhf "http://mirrordirector.raspbian.org/raspbian/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.23-2_armhf.deb" +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" + #compile_libusb 'x86_64-apple-darwin' # fails with "sys/sysctl.h: No such file or directory" + +_architectures=( + #'arm-linux-gnueabihf' + #'x86_64-apple-darwin' + #'x86_64-pc-linux-gnu' + #'i686-w64-mingw32' + #'x86_64-w64-mingw32' + ) + + +FILES=('libfibre.cpp' + 'platform_support/libusb_transport.cpp' + 'legacy_protocol.cpp' + 'legacy_object_client.cpp' + 'logging.cpp') + +### Raspberry Pi + +#arm-linux-gnueabihf-g++ -shared -o libfibre-linux-armhf.so -fPIC -std=c++11 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \ +# -I./third_party/libusb-dev-armhf/usr/include/libusb-1.0 \ +# "${FILES[@]}" \ +# ./third_party/libusb-armhf/lib/arm-linux-gnueabihf/libusb-1.0.so.0.2.0 \ +# -lpthread \ +# -L./third_party/libstdc++-linux-armhf/usr/lib/gcc-cross/arm-linux-gnueabihf/10 \ +# -Wl,--unresolved-symbols=ignore-in-shared-libs -static-libstdc++ + + +### Windows + +mkdir -p "third_party/libusb-windows" +pushd "third_party/libusb-windows" > /dev/null +if [ ! -f libusb-1.0.23.7z ]; then + wget "https://github.com/libusb/libusb/releases/download/v1.0.23/libusb-1.0.23.7z" +fi +if [ ! -f "libusb-1.0.23/libusb-1.0.def" ]; then + 7z x -o"libusb-1.0.23" "libusb-1.0.23.7z" +fi +popd > /dev/null + +#x86_64-w64-mingw32-g++ -shared -o libfibre-windows-amd64.dll -fPIC -std=c++11 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \ +# -I./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0 \ +# "${FILES[@]}" \ +# -static-libgcc \ +# -Wl,-Bstatic \ +# -lstdc++ \ +# ./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a \ +# -Wl,-Bdynamic + +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)" + +# 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 + +export PATH="/opt/osxcross/bin:$PATH" +export LD_LIBRARY_PATH="/opt/osxcross/lib" +export CFLAGS='-I/opt/osxcross/SDK/MacOSX10.13.sdk/usr/include -arch i386 -arch x86_64' +export MACOSX_DEPLOYMENT_TARGET='10.9' +CC='o64-clang' \ + compile_libusb 'macos-amd64' 'x86_64-apple-darwin17' + +#mkdir -p "third_party/libusb-src" +#pushd "third_party/libusb-src" > /dev/null +#if [ ! -f v1.0.23.tar.gz ]; then +# wget "https://github.com/libusb/libusb/archive/v1.0.23.tar.gz" +#fi +#if [ ! -f "libusb-1.0.23/README.md" ]; then +# tar -xvf "v1.0.23.tar.gz" +#fi +#export CC=o64-clang++ +#mkdir -p "third_party/libusb-src/build-macos-amd64" +#../configure +#pushd "third_party/libusb-src" > /dev/null +#./configure +#popd > /dev/null + + +o64-clang++ -shared -o libfibre-macos-x86.dylib -fPIC -std=c++11 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \ + -I./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0 \ + -arch x86_64 -arch i386 \ + "${FILES[@]}" \ + -static-libstdc++ \ + ./third_party/libusb-1.0.23/build-macos-amd64/libusb/.libs/libusb-1.0.a \ + -framework CoreFoundation -framework IOKit + + #"${FILES[@]}" \ + + #-arch i386 \ + #-Wl,-Bstatic \ + #./third_party/libusb-1.0.23/build-macos-amd64/libusb/libusb-1.0.la \ + #-Wl,-Bdynamic + + + # \ + #-Wl,-Bstatic \ + #-lgcc \ + #-lstdc++ \ + #./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a \ + #-Wl,-Bdynamic + diff --git a/Firmware/fibre-cpp/endpoints_template.j2 b/Firmware/fibre-cpp/endpoints_template.j2 new file mode 100644 index 00000000..4e467894 --- /dev/null +++ b/Firmware/fibre-cpp/endpoints_template.j2 @@ -0,0 +1,91 @@ +/*[# 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 +#include + +// 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(PROTOCOL_VERSION, embedded_json, embedded_json_length); +const uint32_t json_version_id_ = (json_crc_ << 16) | calc_crc16(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(property.get_type_info()); + return type_info && type_info->set_float(property, value); +} + +} + +#pragma GCC pop_options + +#endif // __FIBRE_ENDPOINTS_HPP \ No newline at end of file diff --git a/Firmware/fibre-cpp/event_loop.hpp b/Firmware/fibre-cpp/event_loop.hpp new file mode 100644 index 00000000..e4e6a144 --- /dev/null +++ b/Firmware/fibre-cpp/event_loop.hpp @@ -0,0 +1,45 @@ +#ifndef __FIBRE_EVENT_LOOP_HPP +#define __FIBRE_EVENT_LOOP_HPP + +#include + +struct EventLoopTimer; + +/** + * @brief Base class for event loops. + * + * Thread-safety: The functions of this class 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. + */ + virtual int post(void (*callback)(void*), void *ctx) = 0; + + virtual int register_event(int event_fd, uint32_t events, void (*callback)(void*), void* ctx) = 0; + virtual int deregister_event(int event_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, void (*callback)(void*), void *ctx) = 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 int cancel_timer(EventLoopTimer* timer) = 0; +}; + +#endif // __FIBRE_EVENT_LOOP_HPP \ No newline at end of file diff --git a/Firmware/fibre-cpp/function_stubs_template.j2 b/Firmware/fibre-cpp/function_stubs_template.j2 new file mode 100644 index 00000000..fb44fdaa --- /dev/null +++ b/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 + +[% 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 %].value()[% 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]].value()->[[func.name]]([% for arg in func.in.values() | skip_first %]in_[[arg.name]][% if not arg.optional %].value()[% endif %][[', ' 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 %] + diff --git a/Firmware/fibre-cpp/include/fibre/bufptr.hpp b/Firmware/fibre-cpp/include/fibre/bufptr.hpp new file mode 100644 index 00000000..7726ed2b --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/bufptr.hpp @@ -0,0 +1,100 @@ +#ifndef __FIBRE_BUFPTR_HPP +#define __FIBRE_BUFPTR_HPP + +#include +#include + +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 +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 + generic_bufptr_t(T (&begin)[I]) : generic_bufptr_t(begin, I) {} + + generic_bufptr_t(std::vector::type>& vector) + : generic_bufptr_t(vector.data(), vector.size()) {} + + generic_bufptr_t(const std::vector::type>& vector) + : generic_bufptr_t(vector.data(), vector.size()) {} + + generic_bufptr_t(const generic_bufptr_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; +using bufptr_t = generic_bufptr_t; + +} + +#endif // __FIBRE_BUFPTR_HPP diff --git a/Firmware/fibre-cpp/include/fibre/cpp_utils.hpp b/Firmware/fibre-cpp/include/fibre/cpp_utils.hpp new file mode 100644 index 00000000..285af103 --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/cpp_utils.hpp @@ -0,0 +1,1264 @@ +/* + +## Advanced C++ Topics + +This is an overview of some of the more obscure C++ techniques used in this project. +This assumes you're already familiar with templates in C++. + +### Template recursion + +[TODO] + +### Almost perfect template forwarding + +This is adapted from https://akrzemi1.wordpress.com/2013/10/10/too-perfect-forwarding/ + +Suppose you have a inner class, with a couple of constructors: +``` +class InnerClass { +public: + InnerClass(int arg1, int arg2); + InnerClass(int arg1); + InnerClass(); +}; +``` + +Now you want to create a wrapper class. This wrapper class should provide the exact same constructors as `InnerClass`, so you use perfect forwarding: +``` +class WrapperClass { +public: + template + WrapperClass(Args&& ... args) + : inner_object(std::forward(args)...) + {} + InnerClass inner_object; +}; +``` + +Now you can almost use the wrapper class as expected, but only almost: +``` +void make_wrappers(void) { + WrapperClass wrapper1; // ok, maps to InnerClass() + WrapperClass wrapper2(1); // ok, maps to InnerClass(int arg1) + WrapperClass wrapper3(1,2); // ok, maps to InnerClass(int arg1, arg2) + WrapperClass wrapper4 = wrapper1; // does not compile +} +``` + +The last assignment fails. What _you_ obviously wanted, is to use the copy constructor of WrapperClass. +However the compiler will use the perfect forwarding constructor of WrapperClass for this assignment. +So after template expansion it would try to use the following constructor: + +``` + WrapperClass(InnerClass& arg) + : inner_object(arg) + {} +``` + +Clearly this is not what we wanted and in this case it will fail because the exists no +constructor of the form `InnerClass(WrapperClass& arg)`. + +And thus we need to make the perfect forwarding a little less perfect, by telling it +"only enable this constructor if the first argument of the argument list is not of type WrapperClass". + +The modified version thus looks like this: +``` +class WrapperClass { +public: + template::template first_is_not())> + WrapperClass(Args&& ... args) + : inner_object(std::forward(args)...) + {} + InnerClass inner_object; +}; +``` + +*/ + +#ifndef __CPP_UTILS_HPP +#define __CPP_UTILS_HPP + +#include +#include +#include +#include +//#include + +/* Backport features from C++14 and C++17 ------------------------------------*/ + +#if __cplusplus < 201402L +namespace std { + template< class T > + using underlying_type_t = typename underlying_type::type; + + // source: http://en.cppreference.com/w/cpp/types/enable_if + template< bool B, class T = void > + using enable_if_t = typename enable_if::type; + + // source: https://en.cppreference.com/w/cpp/types/conditional + template< bool B, class T, class F > + using conditional_t = typename conditional::type; + + // source: http://en.cppreference.com/w/cpp/utility/tuple/tuple_element + template + using tuple_element_t = typename tuple_element::type; + + // source: https://en.cppreference.com/w/cpp/types/remove_cv + template< class T > + using remove_cv_t = typename remove_cv::type; + template< class T > + using remove_const_t = typename remove_const::type; + template< class T > + using remove_volatile_t = typename remove_volatile::type; + template< class T > + using remove_reference_t = typename remove_reference::type; + + template< class T > + using decay_t = typename decay::type; + + // integer_sequence implementation adapted from + // https://stackoverflow.com/questions/17424477/implementation-c14-make-integer-sequence + + /// Class template integer_sequence + template + struct integer_sequence { + using type = integer_sequence; + typedef _Tp value_type; + static constexpr size_t size() noexcept { return sizeof...(_Idx); } + }; + + template + struct _merge_and_renumber; + + template + struct _merge_and_renumber, integer_sequence<_Tp, I2...>> + : integer_sequence<_Tp, I1..., (sizeof...(I1)+I2)...> + { }; + + template + struct make_integer_sequence + : _merge_and_renumber::type, + typename make_integer_sequence<_Tp, N - N/2>::type> + { }; + + template struct make_integer_sequence<_Tp, 0> : integer_sequence<_Tp> { }; + template struct make_integer_sequence<_Tp, 1> : integer_sequence<_Tp, 0> { }; + + /// Alias template index_sequence + template + using index_sequence = integer_sequence; + + /// Alias template make_index_sequence + template + using make_index_sequence = typename make_integer_sequence::type; +} +#endif + +namespace fibre { + // Creates the index sequence { IFrom, IFrom + 1, IFrom + 2, ..., ITo - 1 } + template + struct make_integer_sequence_from_to_impl { + using type = typename make_integer_sequence_from_to_impl<_Tp, IFrom, ITo - 1, ITo - 1, I...>::type; + }; + + template + struct make_integer_sequence_from_to_impl<_Tp, IFrom, IFrom, I...> { + using type = std::index_sequence; + }; + + template + using make_integer_sequence_from_to = typename make_integer_sequence_from_to_impl<_Tp, IFrom, ITo>::type; +} + +#if __cplusplus < 201703L +namespace std { +//template>{}, int> = 0> +//using enable_ + +template struct invoke_result_impl; + +template +struct invoke_result_impl>{}>, + Fn, Args...> { + typedef decltype(std::mem_fn(std::declval())(std::declval()...)) type; +}; + +template +struct invoke_result_impl>{}>, + Fn, Args...> { + typedef decltype(std::declval()(std::declval()...)) type; +}; + +template +using invoke_result = invoke_result_impl; + +template +using invoke_result_t = typename invoke_result::type; + +template>{}, int> = 0 > +constexpr invoke_result_t invoke(Fn&& f, Args&&... args) + noexcept(noexcept(std::mem_fn(f)(std::forward(args)...))) +{ + return std::mem_fn(f)(std::forward(args)...); +} + +template>{}, int> = 0> +constexpr invoke_result_t invoke(Fn&& f, Args&&... args) + noexcept(noexcept(std::forward(f)(std::forward(args)...))) +{ + return std::forward(f)(std::forward(args)...); +} +} + +namespace std { +namespace detail { +template +struct apply_result_impl; + +// TODO: apply_result is not part of C++17, therefore we should move this out of +// the #if block +template +struct apply_result_impl> { + //typedef std::invoke_result_t...> type; + typedef std::invoke_result_t(std::declval()))...> type; +}; + +template +using apply_result = apply_result_impl>::value>>; + +template +using apply_result_t = typename apply_result::type; + +template +constexpr apply_result_t apply_impl( F&& f, Tuple&& t, std::index_sequence ) +{ + return std::invoke(std::forward(f), std::get(std::forward(t))...); +} +} // namespace detail + +template +constexpr detail::apply_result_t apply(F&& f, Tuple&& t) +{ + return detail::apply_impl(std::forward(f), std::forward(t), + std::make_index_sequence>::value>{}); +} +} + + +namespace std { + +template +struct identity { using type = T; }; + +template +struct overload_resolver; + +template<> +struct overload_resolver<> { void operator()() const; }; + +template +struct overload_resolver : overload_resolver { + using overload_resolver::operator(); + identity operator()(T) const; +}; + +template +struct index_of : integral_constant::value + 1)> {}; + +template +struct index_of : integral_constant {}; + +/** + * @brief Heavily simplified version of the C++17 std::variant. + * Whatever compiles should work as one would expect from the C++17 variant. + */ +template +class variant; + +// Empty variant is ill-formed. Only used for clean recursion here. +template<> +class variant<> { +public: + using storage_t = char[0]; + storage_t content_; + + static void selective_destructor(char* storage, size_t index) { + throw; + } + + static void selective_copy_constuctor(char* target, const char* source, size_t index) { + throw; + } + + static bool selective_eq(const char* lhs, const char* rhs, size_t index) { + throw; + } + + static bool selective_neq(const char* lhs, const char* rhs, size_t index) { + throw; + } + + template + static void selective_invoke_const(const char* content, size_t index, TFunc functor, TArgs&&... args) { + throw; + } + + template + static void selective_invoke(const char* content, size_t index, TFunc functor, TArgs&&... args) { + throw; + } +}; + +template +class variant { +public: + using storage_t = char[sizeof(T) > sizeof(typename variant::storage_t) ? sizeof(T) : sizeof(typename variant::storage_t)]; + + static void selective_copy_constuctor(char* target, const char* source, size_t index) { + if (index == 0) { + new ((T*)target) T{*(T*)source}; // in-place construction using first type's copy constructor + } else { + variant::selective_copy_constuctor(target, source, index - 1); + } + } + + static void selective_destructor(char* storage, size_t index) { + if (index == 0) { + ((T*)storage)->~T(); + } else { + variant::selective_destructor(storage, index - 1); + } + } + + static bool selective_eq(const char* lhs, const char* rhs, size_t index) { + if (index == 0) { + return ((*(T*)lhs) == (*(T*)rhs)); + } else { + return variant::selective_eq(lhs, rhs, index - 1); + } + } + + static bool selective_neq(const char* lhs, const char* rhs, size_t index) { + if (index == 0) { + return ((*(T*)lhs) != (*(T*)rhs)); + } else { + return variant::selective_neq(lhs, rhs, index - 1); + } + } + + template + static void selective_invoke_const(const char* content, size_t index, TFunc functor, TArgs&&... args) { + if (index == 0) { + functor(*(T*)content, std::forward(args)...); + } else { + variant::selective_invoke_const(content, index - 1, functor, std::forward(args)...); + } + } + + template + static void selective_invoke(char* content, size_t index, TFunc functor, TArgs&&... args) { + if (index == 0) { + functor(*(T*)content, std::forward(args)...); + } else { + variant::selective_invoke(content, index - 1, functor, std::forward(args)...); + } + } + + variant() : index_(0) { + new ((T*)content_) T{}; // in-place construction using first type's default constructor + } + + variant(const variant & other) : index_(other.index_) { + selective_copy_constuctor(content_, other.content_, index_); + } + + variant(variant&& other) : index_(other.index_) { + // TODO: implement + selective_copy_constuctor(content_, other.content_, index_); + } + + // Find the best match out of `T, Ts...` with `TArg` as the argument. + template + using best_match = decltype(overload_resolver()(std::declval())); + + template::type> //, typename=typename std::enable_if_t, variant>::value)>, typename TTarget=decltype(indicator_func(std::forward(std::declval()))), typename TIndex=index_of> + variant(TArg&& arg) { + new ((TTarget*)content_) TTarget{std::forward(arg)}; + index_ = index_of::value; + } + + ~variant() { + selective_destructor(content_, index_); + } + + inline variant& operator=(const variant & other) { + selective_destructor(content_, index_); + index_ = other.index_; + selective_copy_constuctor(content_, other.content_, index_); + return *this; + } + + inline bool operator==(const variant& rhs) const { + return (index_ == rhs.index_) && selective_eq(this->content_, rhs.content_, index_); + } + + inline bool operator!=(const variant& rhs) const { + return (index_ != rhs.index_) || selective_neq(this->content_, rhs.content_, index_); + } + + template + void invoke(TFunc functor, TArgs&&... args) const { + selective_invoke_const(content_, index_, functor, std::forward(args)...); + } + + template + void invoke(TFunc functor, TArgs&&... args) { + selective_invoke(content_, index_, functor, std::forward(args)...); + } + + storage_t content_; + size_t index_; + + size_t index() const { return index_; } +}; + +template +std::tuple_element_t>& get(std::variant& val) { + if (val.index() != I) + throw; + using T = std::tuple_element_t>; + return *((T*)val.content_); +} + +template +T& get(std::variant& val) { + constexpr size_t index = std::index_of::value; + return std::get(val); +} + + +/// Tag type to disengage optional objects. +struct nullopt_t { + // Do not user-declare default constructor at all for + // optional_value = {} syntax to work. + // nullopt_t() = delete; + + // Used for constructing nullopt. + enum class _Construct { _Token }; + + // Must be constexpr for nullopt_t to be literal. + explicit constexpr nullopt_t(_Construct) { } +}; + +constexpr nullopt_t nullopt { nullopt_t::_Construct::_Token }; + +template +class optional { +public: + using storage_t = char[sizeof(T)]; + + optional() : has_value_(false) {} + optional(nullopt_t val) : has_value_(false) {} + + optional(const optional & other) : has_value_(other.has_value_) { + if (has_value_) + new ((T*)content_) T{*(T*)other.content_}; + } + + optional(optional&& other) : has_value_(other.has_value_) { + if (has_value_) + new ((T*)content_) T{*(T*)other.content_}; + } + + optional(T& arg) { + new ((T*)content_) T{arg}; + has_value_ = true; + } + + optional(T&& arg) { + new ((T*)content_) T{std::forward(arg)}; + has_value_ = true; + } + + ~optional() { + if (has_value_) + ((T*)content_)->~T(); + } + + inline optional& operator=(const optional & other) { + ~*this(); + new (this) optional{other}; + return *this; + } + + inline bool operator==(const optional& rhs) const { + return (!has_value_ && !rhs.has_value_) || (*(T*)content_ == *(T*)rhs.content_); + } + + inline bool operator!=(const optional& rhs) const { + return !(*this == rhs); + } + + inline T& operator*() { + return *(T*)content_; + } + + inline T& operator->() { + return *(T*)content_; + } + + storage_t content_; + size_t has_value_; + + size_t has_value() const { return has_value_; } +}; + +template +optional make_optional(T&& val) { + return optional{std::forward(val)}; +} + +} // namespace std + +#endif + +/* Stuff that should be in the STL but isn't ---------------------------------*/ + +// source: https://en.cppreference.com/w/cpp/experimental/to_array +namespace detail { +template +constexpr std::array, N> + to_array_impl(T (&a)[N], std::index_sequence) +{ + return { {a[I]...} }; +} + +template +constexpr std::array, N> to_array(T (&a)[N]) +{ + return detail::to_array_impl(a, std::make_index_sequence{}); +} +} + + + +/* Custom utils --------------------------------------------------------------*/ + +// @brief Supports various queries on a list of types +template +class TypeChecker; + +template +class TypeChecker { +public: + using DecayedT = typename std::decay::type; + + // @brief Returns false if type T is equal to U or inherits from U. Returns true otherwise. + template + constexpr static inline bool first_is_not() { + return !std::is_same::value + && !std::is_base_of::value; + } + + // @brief Returns true if all types [T, Ts...] are either equal to U or inherit from U. + template + constexpr static inline bool all_are() { + return std::is_base_of::value + && TypeChecker::template all_are(); + } + constexpr static const size_t count = TypeChecker::count + 1; +}; + +template<> +class TypeChecker<> { +public: + template + constexpr static inline bool first_is_not() { + return std::true_type::value; + } + template + constexpr static inline bool all_are() { + return std::true_type::value; + } + constexpr static const size_t count = 0; +}; + +template +TypeChecker make_type_checker(Ts ...) { + return TypeChecker(); +} + +#include +#define ENABLE_IF(...) \ + typename = typename std::enable_if_t<__VA_ARGS__> + +#define ENABLE_IF_SAME(a, b, type) \ + template typename std::enable_if_t::value, type> + +template M get_member_type(M T:: *); +#define GET_TYPE_OF(mem) decltype(get_member_type(mem)) + + +//#include +// @brief Statically asserts that T is derived from type BaseType +#define EXPECT_TYPE(T, BaseType) static_assert(std::is_base_of::type>::value || std::is_convertible::type, BaseType>::value, "expected template argument of type " #BaseType) +//#define EXPECT_TYPE(T, BaseType) static_assert(, "expected template argument of type " #BaseType) + + + + +template +class function_traits { +public: + template + static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args, TUnpackedArgs ... args) { + return invoke(obj, func_ptr, packed_args, std::forward(args)..., std::get(packed_args)); + } + + template + static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args, TArgs ... args) { + return (obj.*func_ptr)(std::forward(args)...); + } +}; + + +/* @brief return_type::type represents the C++ native return type +* of a function returning 0 or more arguments. +* +* For an empty TypeList, the return type is void. For a list with +* one type, the return type is equal to that type. For a list with +* more than one items, the return type is a tuple. +*/ +template +struct return_type; + +template<> +struct return_type<> { typedef void type; }; +template +struct return_type { typedef T type; }; +template +struct return_type { typedef std::tuple type; }; + + + +template +struct static_function_traits; + +// TODO: All invoke-related functions should be superseeded by a proper std::apply implementation +#if 0 +template +struct static_function_traits, std::tuple> { + using TRet = typename return_type::type; + + //template + //static std::tuple invoke(std::tuple packed_args, TUnpackedInputs ... args) { + // return invoke(packed_args, args..., std::get(packed_args)); + //} + + template + static std::tuple invoke(std::tuple& packed_args) { + return invoke_impl(packed_args, std::make_index_sequence()); + } + + template + static std::tuple invoke_impl(std::tuple packed_args, std::index_sequence) { + return invoke_impl_2(std::get(packed_args)...); + } + + //template + //static std::enable_if_t<(sizeof...(TOutputs) == 0), std::tuple> + template>*/> + static std::enable_if_t<(IOutputs == 0), std::tuple> + invoke_impl_2(TInputs ... args) { + Function(args...); + return std::make_tuple<>(); + } + + //template + //static std::enable_if_t<(sizeof...(TOutputs) == 1), std::tuple> + template>*/> + static std::enable_if_t<(IOutputs == 1), std::tuple> + invoke_impl_2(TInputs ... args) { + return std::make_tuple(Function(args...)); + } +// +// template= 2)> +// static /* std::enable_if_t= 2, */ std::tuple //> +// invoke_impl_2(std::tuple packed_args, TInputs ... args) { +// return Function(args...); +// } +}; + +/* @brief Invoke a class member function with a variable number of arguments that are supplied as a tuple + +Example usage: + +class MyClass { +public: + int MyFunction(int a, int b) { + return 0; + } +}; + +MyClass my_object; +std::tuple my_args(3, 4); // arguments are supplied as a tuple +int result = invoke_function_with_tuple(my_object, &MyClass::MyFunction, my_args); +*/ +template +TRet invoke_function_with_tuple(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args) { + return function_traits::template invoke<0>(obj, func_ptr, packed_args); +} + +template(*Function)(TIn...)> +std::tuple invoke_with_tuples(std::tuple inputs) { + static_function_traits::template invoke<0>(inputs); +} +#endif + + +template +struct sum_impl; +template +struct sum_impl { static constexpr TInt value = 0; }; +template +struct sum_impl { static constexpr TInt value = I + sum_impl::value; }; + +template +using sum = sum_impl; + + +// source: https://akrzemi1.wordpress.com/2017/05/18/asserts-in-constexpr-functions/ +#if defined NDEBUG +# define X_ASSERT(CHECK) void(0) +#else +# define X_ASSERT(CHECK) \ + ( (CHECK) ? void(0) : []{assert(!#CHECK);}() ) +#endif + +template +struct for_each_in_tuple_result_impl; + +template +struct for_each_in_tuple_result_impl> { + typedef std::tuple(std::declval())(std::get(std::declval())))...> type; +}; + +template +using for_each_in_tuple_result = for_each_in_tuple_result_impl>::value>>; + +template +using for_each_in_tuple_result_t = typename for_each_in_tuple_result::type; + +template +for_each_in_tuple_result_t for_each_in_tuple_impl(Fn&& f, Tuple&& t, std::index_sequence) { + return for_each_in_tuple_result_t(std::forward(f)(std::get(t))...); +} + +template +for_each_in_tuple_result_t for_each_in_tuple(Fn&& f, Tuple&& t) { + return for_each_in_tuple_impl(std::forward(f), std::forward(t), std::make_index_sequence>::value>{}); +} +//template +//for_each_in_tuple_result_t for_each_in_tuple(Fn&& f, Tuple&& t) { +// return 5; +//} + + +/* constexpr strings --------------------------------------------------------*/ +/* adapted from: +* https://akrzemi1.wordpress.com/2017/06/28/compile-time-string-concatenation/ +*/ + + +// TODO: the functionality +// sstring::substring, sstring::get_last_part and sstring::after_last_index_of and sstring::last_index_of +// was removed during refactoring. Add again if needed. + +/** + * @brief Represents a string that is known at compile time by encoding it as a + * type. + */ +template +struct sstring { + static constexpr const char chars[] = {CHARS..., 0}; + static constexpr const char* c_str() { return chars; } + static constexpr size_t size() { return sizeof...(CHARS); } + static constexpr std::array as_array() { return {CHARS...}; } + + template + constexpr bool operator==(const sstring & other) const { + return as_array() == other.as_array(); + } +}; +template +constexpr const char sstring::chars[/*sizeof...(CHARS) + 1*/]; + +template +struct sstring_concat_impl; + +template +struct sstring_concat_impl, sstring> { + using type = sstring; +}; + +/** @brief Represents the result type of concatenating two static strings */ +template +using sstring_concat_t = typename sstring_concat_impl::type; + +/** @brief Concatenates two static strings */ +template +constexpr sstring operator+(sstring, sstring) { + return {}; +} + + +/** @brief Helper class for the MAKE_SSTRING macro */ +template +struct sstring_builder; + +template +struct sstring_builder<0, CHAR, CHARS...> { + using type = sstring<>; +}; + +template +struct sstring_builder { + using type = sstring_concat_t, typename sstring_builder::type>; +}; + +template +using sstring_builder_t = typename sstring_builder::type; + +#define MACRO_GET_1(str, i) \ + (sizeof(str) > (i) ? str[(i)] : 0) + +#define MACRO_GET_4(str, i) \ + MACRO_GET_1(str, i+0), \ + MACRO_GET_1(str, i+1), \ + MACRO_GET_1(str, i+2), \ + MACRO_GET_1(str, i+3) + +#define MACRO_GET_16(str, i) \ + MACRO_GET_4(str, i+0), \ + MACRO_GET_4(str, i+4), \ + MACRO_GET_4(str, i+8), \ + MACRO_GET_4(str, i+12) + +#define MACRO_GET_64(str, i) \ + MACRO_GET_16(str, i+0), \ + MACRO_GET_16(str, i+16), \ + MACRO_GET_16(str, i+32), \ + MACRO_GET_16(str, i+48) + +/** + * @brief Builds a compile-time string type from a string literal. + * + * Passing more than 64 characters will prune the string. + * + * Usage: + * MAKE_SSTRING("hello world") my_str{}; + * or + * auto my_str = MAKE_SSTRING("hello world"){}; + * + * Both examples create a compile-time variable "my_str" of which the type + * itself stores the content "hello world". + */ +#define MAKE_SSTRING(literal) sstring_builder_t + +/*namespace std { +template +static std::ostream& operator<<(std::ostream& stream, const sstring& val) { + stream << val.chars; + return stream; +} +}*/ + +template +struct join_sstring_impl; + +template +struct join_sstring_impl> { + using type = sstring<>; +}; + +template +struct join_sstring_impl, sstring> { + using type = sstring; +}; + +template +struct join_sstring_impl, sstring, TStr...> { + using type = sstring_concat_t, typename join_sstring_impl, TStr...>::type>; +}; + +template +using join_sstring_t = typename join_sstring_impl::type; + +template +constexpr join_sstring_t join_sstring(const TDelimiter& delimiter, const TStr& ... str) { + return {}; +} + +template +using sstring_arr = std::tuple...>; + + +// source: https://stackoverflow.com/questions/40159732/return-other-value-if-key-not-found-in-the-map +template +TValue& get_or(std::unordered_map& m, const TKey& key, TValue& default_value) { + auto it = m.find(key); + if (it == m.end()) { + return default_value; + } else { + return it->second; + } +} +template +TValue* get_ptr(std::unordered_map& m, const TKey& key) { + auto it = m.find(key); + if (it == m.end()) + return nullptr; + else + return &(it->second); +} + +template +std::true_type is_complete_impl(T *); +std::false_type is_complete_impl(...); + +/** @brief is_complete resolves to std::true_type if T is complete + * and to std::false_type otherwise. This can be used to check if a certain template + * specialization exists. + **/ +template +using is_complete = decltype(is_complete_impl(std::declval())); + +template +struct dynamic_get_impl { + template + static TRet* get(size_t i, TTuple& t) { + if (i == I::value) + return &static_cast(std::get(t)); + else if (i > I::value) + return dynamic_get_impl, TRet, Ts...>::get(i, t); + return nullptr; // this should not happen + } +}; + +template +struct dynamic_get_impl, TRet, Ts...> { + static TRet* get(size_t i, const std::tuple& t) { + return nullptr; + } +}; + +template +TRet* dynamic_get(size_t i, std::tuple& t) { + return dynamic_get_impl, TRet, Ts...>::get(i, t); +} + +template +TRet* dynamic_get(size_t i, const std::tuple& t) { + return dynamic_get_impl, TRet, Ts...>::get(i, t); +} + + +template +class simple_iterator : std::iterator { + TDereferenceable *container_; + size_t i_; +public: + using reference = TResult; + explicit simple_iterator(TDereferenceable& container, size_t pos) : container_(&container), i_(pos) {} + simple_iterator& operator++() { ++i_; return *this; } + simple_iterator operator++(int) { simple_iterator retval = *this; ++(*this); return retval; } + bool operator==(simple_iterator other) const { return (container_ == other.container_) && (i_ == other.i_); } + bool operator!=(simple_iterator other) const { return !(*this == other); } + bool operator<(simple_iterator other) const { return i_ < other.i_; } + bool operator>(simple_iterator other) const { return i_ > other.i_; } + bool operator<=(simple_iterator other) const { return (*this < other) || (*this == other); } + bool operator>=(simple_iterator other) const { return (*this > other) || (*this == other); } + TResult operator*() const { return (*container_)[i_]; } +}; + + + +/** + * @brief Extracts the argument types of a function signature and provides them + * as a std::tuple. + * TODO: if an STL alternative exists, use that + */ +template +struct args_of; + +template +struct args_of { + using type = std::tuple; +}; + +//template +//struct args_of<_Mem_fn> { +// using type = std::tuple; +//}; + +template +struct args_of { + using type = std::tuple; +}; + +template +struct args_of : public args_of {}; + +template +using args_of_t = typename args_of::type; + +/** + * @brief Extracts the return type of a function signature + * + * This is provided because std::result_of is deprecated since C++17 + */ +template +struct result_of; + +template +struct result_of { + using type = TRet; +}; + +template +struct result_of { + using type = TRet; +}; + +template +struct result_of { + using type = TRet; +}; + +template +using result_of_t = typename result_of::type; + + +/** + * @brief Returns the type that results when concatenating multiple tuples + */ +template +using tuple_cat_t = decltype(std::tuple_cat(std::declval()...)); + +template +constexpr std::array array_cat_impl(std::array arr1, std::array arr2, std::index_sequence, std::index_sequence) { + return { arr1[PACK1]..., arr2[PACK2]... }; +} + +template +constexpr std::array array_cat(std::array arr1, std::array arr2) { + return array_cat_impl(arr1, arr2, std::make_index_sequence(), std::make_index_sequence()); +} + +/** + * @brief Returns the type that results when concatenating multiple tuples + */ +template +using tuple_cat_t = decltype(std::tuple_cat(std::declval()...)); + + +/** + * @brief Ensures that a given type is wrapped in a tuple + */ +template +struct as_tuple { + using type = std::tuple; +}; + +template<> +struct as_tuple { + using type = std::tuple<>; +}; + +template +struct as_tuple> { + using type = std::tuple; +}; + +template +using as_tuple_t = typename as_tuple::type; + +/** + * @brief Removes a reference OR pointer from the given type. + * + * This is similar to std::remove_reference, however it can also remove a + * pointer and it does not work for types that are neither a reference or + * a pointer. + */ +template +struct remove_ref_or_ptr { + static_assert(std::is_reference() || std::is_pointer(), "the type T is neither a reference or a pointer"); +}; + +template +struct remove_ref_or_ptr { using type = T; }; + +template +struct remove_ref_or_ptr { using type = T; }; + +template +using remove_ref_or_ptr_t = typename remove_ref_or_ptr::type; + +/** + * @brief Applies remove_ref_or_ptr_t to every type of a tuple type + */ +template +struct remove_refs_or_ptrs_from_tuple; + +template +struct remove_refs_or_ptrs_from_tuple> { + using type = std::tuple...>; +}; + +template +using remove_refs_or_ptrs_from_tuple_t = typename remove_refs_or_ptrs_from_tuple::type; + +/** + * @brief The convert(val) function returns a reference or a pointer to val + * depending on TTo. + * TODO: this could be a functor + */ +template +struct add_ref_or_ptr; + +template +struct add_ref_or_ptr { + static T& convert(T& value) { + return value; + } +}; + +template +struct add_ref_or_ptr { + static T* convert(T& value) { + return &value; + } +}; + + +/** + * @brief The convert() function turns a given tuple of values into a tuple of + * pointers or references based on the template argument TTo. + */ +template +struct add_ref_or_ptr_to_tuple; + +template +struct add_ref_or_ptr_to_tuple> { + template + static std::tuple convert_impl(std::tuple&& t, std::index_sequence) { + using to_type = std::tuple; + to_type result(add_ref_or_ptr>::convert(std::get(t))...); + return result; + } + + template + static std::tuple convert(std::tuple&& t) { + static_assert(sizeof...(TFrom) == sizeof...(TTo), "both tuples must have the same size"); + return convert_impl(std::forward>(t), std::make_index_sequence()); + } +}; + +template +struct add_ptrs_to_tuple_type; + +template +struct add_ptrs_to_tuple_type> { + using type = std::tuple; +}; + +template +using add_ptrs_to_tuple_t = typename add_ptrs_to_tuple_type::type; + +template +struct add_refs_to_tuple_type; + +template +struct add_refs_to_tuple_type> { + using type = std::tuple; +}; + +template +using add_refs_to_tuple_t = typename add_refs_to_tuple_type::type; + + +template struct is_tuple: std::false_type {}; +template struct is_tuple>: std::true_type {}; + + +template +struct tuple_select_type_impl; + +template +struct tuple_select_type_impl, TTuple> { + using type = std::tuple...>; +}; + +template +typename tuple_select_type_impl, TTuple>::type +tuple_select_impl(TTuple tuple, std::index_sequence) { + return typename tuple_select_type_impl, TTuple>::type(std::get(tuple)...); +}; + + +template +struct tuple_take_type { + static_assert(I <= std::tuple_size::value, "cannot take more elements than tuple size"); + using type = typename tuple_select_type_impl, TTuple>::type; +}; + +template +using tuple_take_t = typename tuple_take_type::type; + +/** + * @brief Returns the first I elements from the tuple as a tuple. + * The resulting type is tuple_take_t. + * See also: tuple_skip + */ +template +tuple_take_t tuple_take(TTuple tuple) { + return tuple_select_impl(tuple, std::make_index_sequence{}); +}; + + +template +struct tuple_skip_type { + static_assert(I <= std::tuple_size::value, "cannot skip more elements than tuple size"); + using type = typename tuple_select_type_impl::value>, TTuple>::type; +}; + +template +using tuple_skip_t = typename tuple_skip_type::type; + +/** + * @brief Returns all but the first I elements from the tuple as a tuple. + * The resulting type is tuple_skip_t. + * See also: tuple_take + */ +template +tuple_skip_t tuple_skip(TTuple tuple) { + return tuple_select_impl(tuple, fibre::make_integer_sequence_from_to::value>{}); +}; + +template +struct repeat_type_impl { + using type = typename repeat_type_impl::type; +}; + +template +struct repeat_type_impl<0, T, Ts...> { + using type = std::tuple; +}; + +template +using repeat_t = typename repeat_type_impl::type; + +#endif // __CPP_UTILS_HPP diff --git a/Firmware/fibre-cpp/include/fibre/crc.hpp b/Firmware/fibre-cpp/include/fibre/crc.hpp new file mode 100644 index 00000000..ed321612 --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/crc.hpp @@ -0,0 +1,56 @@ +#ifndef __CRC_HPP +#define __CRC_HPP + +#include +#include + +// Calculates an arbitrary CRC for one byte. +// Adapted from https://barrgroup.com/Embedded-Systems/How-To/CRC-Calculation-C-Code +template +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 +static T calc_crc(T remainder, const uint8_t* buffer, size_t length) { + while (length--) + remainder = calc_crc(remainder, *(buffer++)); + return remainder; +} + +template +static uint8_t calc_crc8(uint8_t remainder, uint8_t value) { + return calc_crc(remainder, value); +} + +template +static uint16_t calc_crc16(uint16_t remainder, uint8_t value) { + return calc_crc(remainder, value); +} + +template +static uint8_t calc_crc8(uint8_t remainder, const uint8_t* buffer, size_t length) { + return calc_crc(remainder, buffer, length); +} + +template +static uint16_t calc_crc16(uint16_t remainder, const uint8_t* buffer, size_t length) { + return calc_crc(remainder, buffer, length); +} + +#endif /* __CRC_HPP */ diff --git a/Firmware/fibre-cpp/include/fibre/decoders.hpp b/Firmware/fibre-cpp/include/fibre/decoders.hpp new file mode 100644 index 00000000..6d5d69fb --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/decoders.hpp @@ -0,0 +1,336 @@ + +#ifndef __DECODERS_HPP +#define __DECODERS_HPP + +#include "protocol.hpp" +#include "crc.hpp" +#include "cpp_utils.hpp" +#include + + +/* Base classes --------------------------------------------------------------*/ + +// @brief Base class for stream based decoders. +// A stream based decoder is a decoder that processes arbitrary length data blocks. +class StreamDecoder : public StreamSink { +public: + // @brief Returns 0 if no error ocurred, otherwise a non-zero error code. + // Once process_bytes returned an error, subsequent calls to get_status must return the same error. + // If the decoder is in an error state, the behavior of get_expected_bytes and process_bytes is undefined. + virtual int get_status() = 0; + + // @brief Returns the minimum number of bytes that are still needed to complete this decoder. + // If 0, the decoder is considered complete and any subsequent call to process_bytes must process + // exactly 0 bytes. + // process_bytes() must always process all provided bytes unless the decoder expects no more bytes + // afterwards + virtual size_t get_expected_bytes() = 0; +}; + +// @brief Base class for a decoder that is fed in a block-wise fashion. +// This base class is provided for convenience when implementing certain types of decoders. +// A StreamDecoder can be obtained from a BlockDecoder by using StreamDecoder_from_BlockDecoder. +template +class BlockDecoder { +public: + typedef std::integral_constant block_size; + + virtual int get_status() = 0; + virtual size_t get_expected_blocks() = 0; + virtual int process_block(const uint8_t block[BLOCKSIZE]) = 0; +private: +}; + +// @brief Base class for a decoder that is fed in a byte-wise fashion +// This base class is provided for convenience when implementing certain types of decoders. +// A StreamDecoder can be obtained from a ByteDecoder by using StreamDecoder_from_ByteDecoder. +class ByteDecoder { +public: + virtual int get_status() = 0; + virtual size_t get_expected_bytes() = 0; + virtual int process_byte(uint8_t byte) = 0; +}; + +/* Converter classes ---------------------------------------------------------*/ + +// @brief Encapsulates a BlockDecoder to make it look like a StreamDecoder +// @tparam T The encapsulated BlockDecoder type. +// Must inherit from BlockDecoder. +template::template all_are>())> +class StreamDecoder_from_BlockDecoder : public StreamDecoder { +public: + // @brief Imitates the constructor signature of the encapsulated type. + template::template first_is_not())> + explicit StreamDecoder_from_BlockDecoder(Args&& ... args) + : block_decoder_(std::forward(args)...) { + EXPECT_TYPE(T, BlockDecoder); + } + + inline int get_status() final { + return block_decoder_.get_status(); + } + + inline size_t get_expected_bytes() final { + size_t expected_bytes = block_decoder_.get_expected_blocks() * T::block_size::value; + return expected_bytes - std::min(expected_bytes, buffer_pos_); + } + + inline int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) final { + while (!get_status() && get_expected_bytes() && length) { + // use the incoming bytes to fill internal buffer to get a complete block + size_t n_copy = std::min(length, T::block_size::value - buffer_pos_); + memcpy(buffer_ + buffer_pos_, buffer, n_copy); + buffer += n_copy; + length -= n_copy; + if (processed_bytes) (*processed_bytes) += n_copy; + buffer_pos_ += n_copy; + + // if we have a full block, process it + if (buffer_pos_ == T::block_size::value) { + block_decoder_.process_block(buffer_); + buffer_pos_ = 0; + } + } + return get_status(); + } + + size_t get_free_space() { return SIZE_MAX; } // TODO: deprecate +private: + T block_decoder_; + size_t buffer_pos_ = 0; + uint8_t buffer_[T::block_size::value]; +}; + +// @brief Encapsulates a ByteDecoder to make it look like a BlockDecoder +// @tparam T The encapsulated ByteDecoder type. +// Must inherit from ByteDecoder. +template::template all_are())> +class BlockDecoder_from_ByteDecoder : public BlockDecoder<1> { +public: + // @brief Imitates the constructor signature of the encapsulated type. + template::template first_is_not())> + BlockDecoder_from_ByteDecoder(Args&& ... args) + : byte_decoder_(std::forward(args)...) { + EXPECT_TYPE(T, ByteDecoder); + } + + inline int get_status() final { + return byte_decoder_.get_status(); + } + inline size_t get_expected_blocks() final { + return byte_decoder_.get_expected_bytes(); + } + inline int process_block(const uint8_t block[1]) final { + int status = byte_decoder_.process_byte(*block); + return status; + } +private: + T byte_decoder_; +}; + +// @brief Encapsulates a ByteDecoder to make it look like a StreamDecoder +// @tparam T The encapsulated ByteDecoder type. +// Must inherit from ByteDecoder. +template::template all_are())> +class StreamDecoder_from_ByteDecoder : public StreamDecoder { +public: + // @brief Imitates the constructor signature of the encapsulated type. + template::template first_is_not())> + StreamDecoder_from_ByteDecoder(Args&& ... args) + : byte_decoder_(std::forward(args)...) { + EXPECT_TYPE(T, ByteDecoder); + } + + inline int get_status() final { + return byte_decoder_.get_status(); + } + inline size_t get_expected_bytes() final { + return byte_decoder_.get_expected_bytes(); + } + inline size_t get_free_space() { return SIZE_MAX; } + inline int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) final { + while (!byte_decoder_.get_status() && byte_decoder_.get_expected_bytes() && length) { + length--; + if (processed_bytes) (*processed_bytes)++; + byte_decoder_.process_byte(*(buffer++)); + } + return byte_decoder_.get_status(); + } +private: + T byte_decoder_; +}; + +/* Decoder implementations ---------------------------------------------------*/ + +template +class VarintByteDecoder : public ByteDecoder { +public: + static constexpr T BIT_WIDTH = (CHAR_BIT * sizeof(T)); + + VarintByteDecoder(T& state_variable) : + state_variable_(state_variable) + { + } + + size_t get_expected_bytes() final { + return done_ ? 0 : 1; + } + + int get_status() final { + return status_; + } + + int process_byte(uint8_t input_byte) final { + if (bit_pos_ == 0) { + LOG_FIBRE("start decoding varint, with 0x%02x => %zx\n", input_byte, (uintptr_t)&state_variable_); + state_variable_ = 0; + } + LOG_FIBRE("varint: decode %02x << %zu at %zx\n", input_byte, bit_pos_, &bit_pos_); + // we assume bit_pos_ < BIT_WIDTH + state_variable_ |= (static_cast(input_byte & 0x7f) << bit_pos_); + if (((state_variable_ >> bit_pos_) & 0x7f) != static_cast(input_byte & 0x7f)) { + LOG_FIBRE("varint overflow: tried to add %02x << %zu\n", input_byte, bit_pos_); + return (status_ = -1); // overflow + } + bit_pos_ += 7; + done_ = !(input_byte & 0x80); + return (status_ = (done_ || bit_pos_ < BIT_WIDTH) ? 0 : -1); + } + +private: + T& state_variable_; + // At all times where status_ != 0 the following statement holds: + // (done_ || bit_pos_ < BIT_WIDTH) + //size_t bit_pos_ = 0; // bit position + size_t bit_pos_ = 0; // bit position + int status_ = 0; + bool done_ = false; + int data[1024] = {0}; +}; + +template +using VarintStreamDecoder = StreamDecoder_from_ByteDecoder>; + +// This double nested type should work identically but makes it way harder for the compiler to optimize +//template +//using VarintBlockDecoder = BlockDecoder_from_ByteDecoder>; +//template +//using VarintStreamDecoder = StreamDecoder_from_BlockDecoder>; + +template +inline VarintStreamDecoder make_varint_decoder(T& variable) { + return VarintStreamDecoder(variable); +} + +inline VarintStreamDecoder make_endpoint_id_decoder(ReceiverState& state) { + return make_varint_decoder(state.endpoint_id); +} +inline VarintStreamDecoder make_length_decoder(ReceiverState& state) { + return make_varint_decoder(state.length); +} + + + +template::template all_are())> +class CRC8BlockDecoder : public BlockDecoder { +public: + CRC8BlockDecoder(TDecoder&& inner_decoder) : + inner_decoder_(std::forward(inner_decoder)) { + } + + int get_status() final { + return status_; + } + + size_t get_expected_blocks() final { + return (inner_decoder_.get_expected_bytes() + CRC8_BLOCKSIZE - 2) / (CRC8_BLOCKSIZE - 1); + } + + int process_block(const uint8_t input_block[4]) final { + current_crc_ = calc_crc8(current_crc_, input_block, CRC8_BLOCKSIZE - 1); + if (current_crc_ != input_block[CRC8_BLOCKSIZE - 1]) + return status_ = -1; + return status_ = inner_decoder_.process_bytes(input_block, CRC8_BLOCKSIZE - 1, nullptr); + } +private: + TDecoder inner_decoder_; + int status_ = 0; + uint8_t current_crc_ = INIT; +}; + +template +using CRC8StreamDecoder = StreamDecoder_from_BlockDecoder>; + +template +inline CRC8StreamDecoder make_crc8_decoder(TDecoder&& decoder) { + return CRC8StreamDecoder(std::forward(decoder)); +} + +// TODO: ENABLE_IF(TypeChecker::template all_are()) +template +class DecoderChain; + +template<> +class DecoderChain<> : public StreamDecoder { +public: + size_t get_expected_bytes() { return 0; } + int get_status() { return 0; } + int process_bytes(const uint8_t *input, size_t length, size_t* processed_bytes) { return 0; } + size_t get_free_space() { return SIZE_MAX; } // TODO: deprecate +}; + +template +class DecoderChain : public StreamDecoder { +public: + DecoderChain(TDecoder&& this_decoder, TDecoders&& ... subsequent_decoders) : + this_decoder_(std::forward(this_decoder)), + subsequent_decoders_(std::forward(subsequent_decoders)...) + { + EXPECT_TYPE(TDecoder, StreamDecoder); + } + + int get_status() final { + // If this decoder or any of the subsequent decoders failed, return error code. + int this_status = this_decoder_.get_status(); + int subsequent_status = subsequent_decoders_.get_status(); + if (this_status) + return this_status; + else if (subsequent_status) + return subsequent_status; + else + return 0; + } + + size_t get_expected_bytes() final { + return this_decoder_.get_expected_bytes() + subsequent_decoders_.get_expected_bytes(); + } + + int process_bytes(const uint8_t *input, size_t length, size_t* processed_bytes) final { + if (this_decoder_.get_expected_bytes()) { + LOG_FIBRE("decoder chain: process %zu bytes in segment %s\n", length, typeid(TDecoder).name()); + size_t chunk = 0; + int status = this_decoder_.process_bytes(input, length, &chunk); + input += chunk; + length -= chunk; + if (processed_bytes) (*processed_bytes) += chunk; + if (status) + return status; + if (!length) + return 0; + } + return subsequent_decoders_.process_bytes(input, length, processed_bytes); + } + + size_t get_free_space() { return SIZE_MAX; } // TODO: deprecate +private: + TDecoder this_decoder_; + DecoderChain subsequent_decoders_; +}; + +template +inline DecoderChain make_decoder_chain(TDecoders&& ... decoders) { + return DecoderChain(std::forward(decoders)...); +} + +#endif // __DECODERS_HPP diff --git a/Firmware/fibre-cpp/include/fibre/encoders.hpp b/Firmware/fibre-cpp/include/fibre/encoders.hpp new file mode 100644 index 00000000..e296eeb0 --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/encoders.hpp @@ -0,0 +1,323 @@ + +#ifndef __ENCODERS_HPP +#define __ENCODERS_HPP + +#include "protocol.hpp" +#include "crc.hpp" +#include "cpp_utils.hpp" +#include + +struct Request { + endpoint_id_t endpoint_id; + size_t length; +}; + +/* Base classes --------------------------------------------------------------*/ + +// @brief Base class for all stream encoders +// A stream based encoder is an encoder that generates arbitrary length data blocks. +class StreamEncoder : public StreamSource { +public: + // @brief Returns 0 if no error ocurred, otherwise a non-zero error code. + // Once get_bytes returned an error, subsequent calls to get_status must return the same error. + // If the encoder is in an error state, the behavior of get_available_bytes and get_bytes is undefined. + virtual int get_status() = 0; + + // @brief Returns the minimum number of bytes that will still be generated by this encoder. + // If 0, the encoder is considered complete and any subsequent call to get_bytes must generate + // exactly 0 bytes. + // get_bytes() must always generate as many bytes as requested unless the encoder generates no more bytes + // afterwards + virtual size_t get_available_bytes() = 0; +}; + +// @brief Base class for an encoder that is fed in a block-wise fashion. +// This base class is provided for convenience when implementing certain types of encoders. +// A StreamEncoder can be obtained from a BlockEncoder by using StreamEncoder_from_BlockEncoder. +template +class BlockEncoder { +public: + typedef std::integral_constant block_size; + + virtual int get_status() = 0; + virtual size_t get_available_blocks() = 0; + virtual int get_block(uint8_t block[BLOCKSIZE]) = 0; +private: +}; + +// @brief Base class for an encoder that is fed in a byte-wise fashion +// This base class is provided for convenience when implementing certain types of encoders. +// A StreamEncoder can be obtained from a ByteEncoder by using StreamEncoder_from_ByteEncoder. +class ByteEncoder { +public: + virtual int get_status() = 0; + virtual size_t get_available_bytes() = 0; + virtual int get_byte(uint8_t *output_byte) = 0; +}; + +/* Converter classes ---------------------------------------------------------*/ + +// @brief Encapsulates a BlockEncoder to make it look like a StreamEncoder +// @tparam T The encapsulated BlockEncoder type. +// Must inherit from to BlockEncoder. +template::template all_are>())> +class StreamEncoder_from_BlockEncoder : public StreamEncoder { +public: + // @brief Imitates the constructor signature of the encapsulated type. + template::template first_is_not())> + explicit StreamEncoder_from_BlockEncoder(Args&& ... args) + : block_encoder_(std::forward(args)...) { + EXPECT_TYPE(T, BlockEncoder); + } + + inline int get_status() final { + return buffered_bytes_ ? 0 : block_encoder_.get_status(); + } + + inline size_t get_available_bytes() final { + size_t available_bytes = block_encoder_.get_available_blocks() * T::block_size::value; + return available_bytes + buffered_bytes_; + } + + inline int get_bytes(uint8_t* buffer, size_t length, size_t* generated_bytes) final { + while (!get_status() && get_available_bytes() && length) { + // if the buffer is empty, retrieve a new block from the encode + if (!buffered_bytes_) { + block_encoder_.get_block(buffer_); + buffered_bytes_ = T::block_size::value; + } + + // hand the buffered bytes to the encoder + size_t n_copy = std::min(buffered_bytes_, length); + memcpy(buffer, buffer_ + T::block_size::value - n_copy, n_copy); + length -= n_copy; + buffer += n_copy; + if (generated_bytes) (*generated_bytes) += n_copy; + buffered_bytes_ -= n_copy; + } + return get_status(); + } +private: + T block_encoder_; + size_t buffered_bytes_ = 0; + uint8_t buffer_[T::block_size::value]; +}; + +// @brief Encapsulates a ByteEncoder to make it look like a BlockEncoder +// @tparam T The encapsulated ByteEncoder type. +// Must inherit from ByteEncoder. +template::template all_are())> +class BlockEncoder_from_ByteEncoder : public BlockEncoder<1> { +public: + // @brief Imitates the constructor signature of the encapsulated type. + template::template first_is_not())> + BlockEncoder_from_ByteEncoder(Args&& ... args) + : byte_encoder_(std::forward(args)...) { + EXPECT_TYPE(T, ByteEncoder); + } + + inline int get_status() final { + return byte_encoder_.get_status(); + } + inline size_t get_available_blocks() final { + return byte_encoder_.get_available_bytes(); + } + inline int get_block(uint8_t block[1]) final { + int status = byte_encoder_.get_byte(*block); + return status; + } +private: + T byte_encoder_; +}; + +// @brief Encapsulates a ByteEncoder to make it look like a StreamEncoder +// @tparam T The encapsulated ByteEncoder type. +// Must inherit from ByteEncoder. +template::template all_are())> +class StreamEncoder_from_ByteEncoder : public StreamEncoder { +public: + // @brief Imitates the constructor signature of the encapsulated type. + template::template first_is_not())> + StreamEncoder_from_ByteEncoder(Args&& ... args) + : byte_encoder_(std::forward(args)...) { + EXPECT_TYPE(T, ByteEncoder); + } + + inline int get_status() final { + return byte_encoder_.get_status(); + } + inline size_t get_available_bytes() final { + return byte_encoder_.get_available_bytes(); + } + inline int get_bytes(uint8_t* buffer, size_t length, size_t* generated_bytes) final { + while (!byte_encoder_.get_status() && byte_encoder_.get_available_bytes() && length) { + length--; + if (generated_bytes) (*generated_bytes)++; + byte_encoder_.get_byte(buffer++); + } + return byte_encoder_.get_status(); + } +private: + T byte_encoder_; +}; + +/* Encoder implementations ---------------------------------------------------*/ + +template +class VarintByteEncoder : public ByteEncoder { +public: + static constexpr T BIT_WIDTH = (CHAR_BIT * sizeof(T)); + + VarintByteEncoder(const T& state_variable) : + state_variable_(state_variable) + {} + + size_t get_available_bytes() final { + return done_ ? 0 : 1; + } + + int get_status() final { + return 0; + } + + int get_byte(uint8_t *output_byte) final { + if (bit_pos_ == 0) + LOG_FIBRE("start encoding varint, from pos %d\n", bit_pos_); + *output_byte = (state_variable_ >> bit_pos_) & 0x7f; + bit_pos_ += 7; + if (bit_pos_ < BIT_WIDTH && (state_variable_ >> bit_pos_)) { + LOG_FIBRE("remainder: %x\n", state_variable_ >> bit_pos_); + *output_byte |= 0x80; + }else + done_ = true; + return 0; + } + +private: + const T& state_variable_; + size_t bit_pos_ = 0; // bit position + int status_ = 0; + bool done_ = false; +}; + +template +using VarintStreamEncoder = StreamEncoder_from_ByteEncoder>; + +template +VarintStreamEncoder make_varint_encoder(const T& variable) { + return VarintStreamEncoder(variable); +} + +VarintStreamEncoder make_endpoint_id_encoder(const Request& request) { + return make_varint_encoder(request.endpoint_id); +} +VarintStreamEncoder make_length_encoder(const Request& request) { + return make_varint_encoder(request.length); +} + +template::template all_are())> +class CRC8BlockEncoder : public BlockEncoder { +public: + CRC8BlockEncoder(TEncoder&& inner_encoder) + : inner_encoder_(std::forward(inner_encoder)) {} + + int get_status() final { + return status_; + } + + size_t get_available_blocks() final { + return (inner_encoder_.get_available_bytes() + CRC8_BLOCKSIZE - 2) / (CRC8_BLOCKSIZE - 1); + } + + int get_block(uint8_t block[4]) final { + size_t generated_bytes = 0; + status_ = inner_encoder_.get_bytes(block, CRC8_BLOCKSIZE - 1, &generated_bytes); + if (status_) + return status_; + + // zero out unused end of the block + while (generated_bytes < CRC8_BLOCKSIZE) + block[generated_bytes++] = 0; + + block[CRC8_BLOCKSIZE - 1] = current_crc_ = calc_crc8(current_crc_, block, CRC8_BLOCKSIZE - 1); + return 0; + } +private: + TEncoder inner_encoder_; + int status_ = 0; + uint8_t current_crc_ = INIT; +}; + +template +using CRC8StreamEncoder = StreamEncoder_from_BlockEncoder>; + +template +CRC8StreamEncoder make_crc8_encoder(TEncoder&& encoder) { + return CRC8StreamEncoder(std::forward(encoder)); +} + +template +class EncoderChain; + +template<> +class EncoderChain<> : public StreamEncoder { +public: + size_t get_available_bytes() final { return 0; } + int get_status() final { return 0; } + int get_bytes(uint8_t *output, size_t length, size_t* generated_bytes) final { return 0; } +}; + +template +class EncoderChain : public StreamEncoder { +public: + EncoderChain(TEncoder&& this_encoder, TEncoders&& ... subsequent_encoders) : + this_encoder_(std::forward(this_encoder)), + subsequent_encoders_(std::forward(subsequent_encoders)...) + { + EXPECT_TYPE(TEncoder, StreamEncoder); + } + + size_t get_available_bytes() final { + return this_encoder_.get_available_bytes() + subsequent_encoders_.get_available_bytes(); + } + + int get_status() final { + // If this encoder or any of the subsequent encoders failed, return error code. + int this_status = this_encoder_.get_status(); + int subsequent_status = subsequent_encoders_.get_status(); + if (this_status) + return this_status; + else if (subsequent_status) + return subsequent_status; + else + return 0; + } + + int get_bytes(uint8_t *output, size_t length, size_t* generated_bytes) final { + if (this_encoder_.get_available_bytes()) { + LOG_FIBRE("encoder chain: generate %zu bytes in segment %s\n", length, typeid(TEncoder).name()); + size_t chunk = 0; + int status = this_encoder_.get_bytes(output, length, &chunk); + if (status) + return status; + output += chunk; + length -= chunk; + if (generated_bytes) *generated_bytes += chunk; + if (!length) + return 0; + } + return subsequent_encoders_.get_bytes(output, length, generated_bytes); + } + +private: + TEncoder this_encoder_; + EncoderChain subsequent_encoders_; +}; + +template +EncoderChain make_encoder_chain(TEncoders&& ... encoders) { + return EncoderChain(std::forward(encoders)...); +} + +#endif // __ENCODERS_HPP diff --git a/Firmware/fibre-cpp/include/fibre/introspection.hpp b/Firmware/fibre-cpp/include/fibre/introspection.hpp new file mode 100644 index 00000000..f7e43f68 --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/introspection.hpp @@ -0,0 +1,219 @@ +#ifndef __FIBRE_INTROSPECTION_HPP +#define __FIBRE_INTROSPECTION_HPP + +#include +#include +#include + +#pragma GCC push_options +#pragma GCC optimize ("s") + +class TypeInfo; +class Introspectable; +using introspectable_storage_t = std::aligned_storage<16, 4>::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 static T& as(Introspectable& obj); + template static const T& as(const Introspectable& obj); + template 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 T& TypeInfo::as(Introspectable& obj) { + static_assert(sizeof(T) <= sizeof(obj.storage_)); + return *(T*)&obj.storage_; +} +template const T& TypeInfo::as(const Introspectable& obj) { + static_assert(sizeof(T) <= sizeof(obj.storage_)); + return *(const T*)&obj.storage_; +} +template Introspectable TypeInfo::make_introspectable(T obj, const TypeInfo* type_info) { + Introspectable introspectable; + as(introspectable) = obj; + introspectable.type_info_ = type_info; + return introspectable; +} + + +// maybe_underlying_type_t resolves to the underlying type of T if T is an enum type or otherwise to T itself. +template::value> struct maybe_underlying_type; +template struct maybe_underlying_type { typedef std::underlying_type_t type; }; +template struct maybe_underlying_type { typedef T type; }; +template using maybe_underlying_type_t = typename maybe_underlying_type::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 +struct FibrePropertyTypeInfo; + +// readonly property +template +struct FibrePropertyTypeInfo> : StringConvertibleTypeInfo, TypeInfo { + using TypeInfo::TypeInfo; + static const PropertyInfo property_table[]; + static const FibrePropertyTypeInfo> 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>(as>(obj).read()), buffer, length, 0); + } +}; + +template +const PropertyInfo FibrePropertyTypeInfo>::property_table[] = {}; +template +const FibrePropertyTypeInfo> FibrePropertyTypeInfo>::singleton{FibrePropertyTypeInfo>::property_table, sizeof(FibrePropertyTypeInfo>::property_table) / sizeof(FibrePropertyTypeInfo>::property_table[0])}; + +// readwrite property +template +struct FibrePropertyTypeInfo> : FloatSettableTypeInfo, StringConvertibleTypeInfo, TypeInfo { + using TypeInfo::TypeInfo; + static const PropertyInfo property_table[]; + static const FibrePropertyTypeInfo> singleton; + static const Introspectable make_introspectable(Property 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>(as>(obj).read()), buffer, length, 0); + } + + bool set_string(const Introspectable& obj, char* buffer, size_t length) const override { + maybe_underlying_type_t value; + if (!from_string(buffer, length, &value, 0)) { + return false; + } + as>(obj).exchange(static_cast(value)); + return true; + } + + bool set_float(const Introspectable& obj, float val) const override { + maybe_underlying_type_t value; + if (!conversion::set_from_float(val, &value)) { + return false; + } + as>(obj).exchange(static_cast(value)); + return true; + } +}; + +template +const PropertyInfo FibrePropertyTypeInfo>::property_table[] = {}; +template +const FibrePropertyTypeInfo> FibrePropertyTypeInfo>::singleton{FibrePropertyTypeInfo>::property_table, sizeof(FibrePropertyTypeInfo>::property_table) / sizeof(FibrePropertyTypeInfo>::property_table[0])}; + +#pragma GCC pop_options + +#endif // __FIBRE_INTROSPECTION_HPP \ No newline at end of file diff --git a/Firmware/fibre-cpp/include/fibre/libfibre.h b/Firmware/fibre-cpp/include/fibre/libfibre.h new file mode 100644 index 00000000..dd60832f --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/libfibre.h @@ -0,0 +1,348 @@ +/** + * @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 +#include + +#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 +# define FIBRE_PUBLIC DLL_EXPORT +# 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; + +enum FibreStatus { + kFibreOk, + kFibreCancelled, + kFibreClosed, + kFibreInvalidArgument, + kFibreInternalError +}; + +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*), 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); + +/** + * @brief construct_object callback type for libfibre_open(). + * + * @param ctx: The user data that was passed to libfibre_open(). + * @param obj: An object handle. This handle is valid until the invokation of + * destroy_object(). It is unique at any point in time but can be reused + * after destroy_object(). + * @param intf: A handle for the interface that this object implements. + * The handle may be identical to an interface handle announced for a + * previous object. + * The interface handle is valid until the last object that implements it + * is destroyed. + * @param intf_name: The ASCII-encoded name of the interface. Can be NULL be for + * anonymous interfaces. If not NULL, it is only valid for the duration + * of the callback and must not be freed by the application. + */ +typedef void (*construct_object_cb_t)(void* ctx, LibFibreObject* obj, LibFibreInterface* intf, const char* intf_name, size_t intf_name_length); + +typedef void (*destroy_object_cb_t)(void* ctx, LibFibreObject* obj); +typedef void (*on_found_object_cb_t)(void*, LibFibreObject*); +typedef void (*on_stopped_cb_t)(void*, FibreStatus); +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*); +typedef void (*on_function_added_cb_t)(void*, LibFibreFunction*, 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 Completion callback type for libfibre_start_call(). + * + * @param ctx: The user data that was passed to libfibre_start_call(). + * @param status: The status of the function call. + * kFibreOk indicates successful completion of the function call. All + * other error codes make no guarantee whether the call was executed or + * not. + * kFibreClosed indicates that the underlying object was lost during the + * function call. The call may or may not have succeeded. + * @param rx_end: Points to the address after the last byte written to the + * output buffer. This pointer always points to a valid position in the + * buffer (or the end of the buffer), even if the call failed. However if + * the status is not kFibreOk then the pointer may not precisely indicate + * the received data range. + */ +typedef void (*on_call_completed_cb_t)(void* ctx, FibreStatus 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. + */ +const struct LibFibreVersion* libfibre_get_version(); + +/** + * @brief Opens and initializes a Fibre context. + * + * @param post: 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. + * @param register_event: TODO: this is a Linux specific callback. Need to use + * IOCP on Windows. + * @param deregister_event: TODO: this is a Linux specific callback. Need to use + * IOCP on Windows. + * @param call_later: Called by libfibre to ask the application to call a + * certain callback after a certain amount of time 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. + * @param cancel_timer: Called by libfibre to ask the application to cancel a + * callback timer previously enqueued with call_later(). + * @param construct_object: Called by libfibre for every remote object that is + * allocated. This can be a root object that was just discovered or an + * object that was created as a result of operations like + * libfibre_get_attribute(). The application can use this to construct + * a corresponding object in the application's environment. + * @param destroy_object: Called by libfibre when a remote object is lost, for + * instance because all channels that provided connection to the object + * broke down. + * An object pointer must no longer be used during or after the call to + * destroy_object(). + * @param cb_ctx: Arbitrary user data passed to construct_object() and + * destroy_object(). + */ +FIBRE_PUBLIC struct LibFibreCtx* libfibre_open( + post_cb_t post, + register_event_cb_t register_event, + deregister_event_cb_t deregister_event, + call_later_cb_t call_later, + cancel_timer_cb_t cancel_timer, + construct_object_cb_t construct_object, + destroy_object_cb_t destroy_object, + void* cb_ctx); + +/** + * @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 Starts looking for Fibre objects that match the specifications. + * + * TODO: specify if specs needs to remain valid for the duration of discovery. + * + * @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 duration of the discovery. + * + * The specification has the format: + * "transport_provider1:args1;transport_provider2:args2" + * Transport providers are for example "usb", "serial", etc. + * Refer to the transport provider's documentation to see what arguments + * it takes. + * + * Example: + * "usb:idVendor=0x1209,idVendor=0x0d32;serial:path=/dev/ttyACM0" + * This will look for channels on USB devices with VID:PID 1209:0d32 and + * on the serial port /dev/ttyACM0. + * @param on_found_object: Invoked for every object that is found. Objects are + * first passed to the construct_object() callback of libfibre_open() + * before they are passed to this callback. An application should use + * the destroy_object() callback of the libfibre_open() function to + * detect the loss of objects. + * @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(LibFibreCtx* ctx, const char* specs, size_t specs_len, struct LibFibreDiscoveryCtx** handle, + on_found_object_cb_t on_found_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(LibFibreCtx* ctx, LibFibreDiscoveryCtx* discovery_ctx); + +/** + * @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. The name and intf_name buffers are only valid for the + * duration of the callback and must not be freed by the application. + * The attribute handle remains valid until the corresponding call to + * on_attribute_removed(). + * @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 until the next + * iteration of the libfibre event loop or until any other libfibre + * function (other than libfibre_ref_obj()) is invoked. If the + * application intends to keep the object handle around it must call + * libfibre_ref_obj() immediately. + * @returns: kFibreOk or kFibreInvalidArgument + */ +FIBRE_PUBLIC FibreStatus libfibre_get_attribute(LibFibreObject* parent_obj, LibFibreAttribute* attr, LibFibreObject** child_obj_ptr); + +/** + * @brief Starts the invokation of a function call. + * + * Once the call has completed (whether successful, failed or cancelled), the + * provided callback will be invoked. + * Until then, the ongoing call can be aborted with libfibre_cancel_call(). + * + * @param obj: An object handle that was obtained in the callback of + * libfibre_start_discovery() or from a call to libfibre_get_attribute(). + * @param func: A function handle that was obtained in the on_function_added() + * callback of libfibre_subscribe_to_interface(). + * @param input: The buffer that contains the input arguments. Must remain valid + * until on_completed() is called. + * @param output: The buffer where the output arguments will be written. Must + * remain valid until on_completed() is called. + * @param handle: The variable being pointed to by this argument is set to a + * handle that can be passed to libfibre_cancel_call() to cancel the + * started call. If on_complete() is invoked directly during + * libfibre_start_call() then the handle variable is not updated later + * than this invokation. + * @param on_completed: Called when the operation completes, whether successful + * or not. + */ +FIBRE_PUBLIC void libfibre_start_call(LibFibreObject* obj, LibFibreFunction* func, const uint8_t *input, size_t input_length, uint8_t *output, size_t output_length, LibFibreCallContext** handle, on_call_completed_cb_t on_completed, void* ctx); + +/** + * @brief Cancels an ongoing function call. + * + * This must not be called twice for the same call and must not be called once + * the completion callback of the function call is invoked. + * + * The completion callback associated with this call will still be invoked after + * the call is cancelled. Until then, the call must still be considered in + * progress. + * + * After calling this function, the function call that was cancelled may or may + * not still go into effect. + * + * @param handle: The function call handle that was obtained by a call to + * libfibre_cancel_call(). + */ +FIBRE_PUBLIC void libfibre_cancel_call(LibFibreCallContext* handle); + +#ifdef __cplusplus +} +#endif + +#endif // __LIBFIBRE_H \ No newline at end of file diff --git a/Firmware/fibre-cpp/include/fibre/posix_tcp.hpp b/Firmware/fibre-cpp/include/fibre/posix_tcp.hpp new file mode 100644 index 00000000..3f6a7a07 --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/posix_tcp.hpp @@ -0,0 +1,4 @@ + +#include "protocol.hpp" + +int serve_on_tcp(unsigned int port); diff --git a/Firmware/fibre-cpp/include/fibre/posix_udp.hpp b/Firmware/fibre-cpp/include/fibre/posix_udp.hpp new file mode 100644 index 00000000..7022f9de --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/posix_udp.hpp @@ -0,0 +1,4 @@ + +#include "protocol.hpp" + +int serve_on_udp(unsigned int port); diff --git a/Firmware/fibre-cpp/include/fibre/protocol.hpp b/Firmware/fibre-cpp/include/fibre/protocol.hpp new file mode 100644 index 00000000..9c039e7d --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/protocol.hpp @@ -0,0 +1,301 @@ +/* +see protocol.md for the protocol specification +*/ + +#ifndef __PROTOCOL_HPP +#define __PROTOCOL_HPP + +#include +#include +#include +//#include +#include +#include +#include +#include +#include "crc.hpp" +#include "cpp_utils.hpp" +#include "bufptr.hpp" +#include "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 +struct Codec { + static std::optional decode(cbufptr_t* buffer) { return std::nullopt; } +}; + +template<> struct Codec { + static std::optional 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::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(int8_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(uint8_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(int16_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(uint16_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(int32_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(uint32_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(int64_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { return SimpleSerializer::read(&(buffer->begin()), buffer->end()); } + static bool encode(uint64_t value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { + std::optional int_val = Codec::decode(buffer); + return int_val.has_value() ? std::optional(*reinterpret_cast(&*int_val)) : std::nullopt; + } + static bool encode(float value, bufptr_t* buffer) { + void* ptr = &value; + return Codec::encode(*reinterpret_cast(ptr), buffer); + } +}; +template +struct Codec::value>> { + static std::optional decode(cbufptr_t* buffer) { + std::optional int_val = SimpleSerializer::read(&(buffer->begin()), buffer->end()); + return int_val.has_value() ? std::make_optional(static_cast(*int_val)) : std::nullopt; + } + static bool encode(T value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } +}; +template<> struct Codec { + static std::optional decode(cbufptr_t* buffer) { + std::optional val0 = SimpleSerializer::read(&(buffer->begin()), buffer->end()); + std::optional val1 = SimpleSerializer::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::write(value.endpoint_id, &(buffer->begin()), buffer->end()) + && SimpleSerializer::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 +struct format_traits_t; + +// template<> struct format_traits_t { using type = void; +// static constexpr const char * fmt = "%f"; +// static constexpr const char * fmtp = "%f"; +// }; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%lld"; + static constexpr const char * fmtp = "%lld"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%llu"; + static constexpr const char * fmtp = "%llu"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%ld"; + static constexpr const char * fmtp = "%ld"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%lu"; + static constexpr const char * fmtp = "%lu"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%d"; + static constexpr const char * fmtp = "%d"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%ud"; + static constexpr const char * fmtp = "%ud"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%hd"; + static constexpr const char * fmtp = "%hd"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%hu"; + static constexpr const char * fmtp = "%hu"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%hhd"; + static constexpr const char * fmtp = "%d"; +}; +template<> struct format_traits_t { using type = void; + static constexpr const char * fmt = "%hhu"; + static constexpr const char * fmtp = "%u"; +}; + +template::type> +static bool to_string(const T& value, char * buffer, size_t length, int) { + snprintf(buffer, length, format_traits_t::fmtp, value); + return true; +} +// Special case for float because printf promotes float to double, and we get warnings +template +static bool to_string(const float& value, char * buffer, size_t length, int) { + snprintf(buffer, length, "%f", (double)value); + return true; +} +template +static bool to_string(const bool& value, char * buffer, size_t length, int) { + buffer[0] = value ? '1' : '0'; + buffer[1] = 0; + return true; +} +template +static bool to_string(const T& value, char * buffer, size_t length, ...) { + return false; +} + +template::type> +static bool from_string(const char * buffer, size_t length, T* property, int) { + // Note for T == uint8_t: Even though we supposedly use the correct format + // string sscanf treats our pointer as pointer-to-int instead of + // pointer-to-uint8_t. To avoid an unexpected memory access we first read + // into a union. + union { T t; int i; } val; + if (sscanf(buffer, format_traits_t::fmt, &val.t) == 1) { + *property = val.t; + return true; + } else { + return false; + } +} +// Special case for float because printf promotes float to double, and we get warnings +template +static bool from_string(const char * buffer, size_t length, float* property, int) { + return sscanf(buffer, "%f", property) == 1; +} +template +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 +static bool from_string(const char * buffer, size_t length, T* property, ...) { + return false; +} + + +//template +//bool set_from_float_ex(float value, T* property) { +// return false; +//} + +namespace conversion { +//template +template +bool set_from_float_ex(float value, float* property, int) { + return *property = value, true; +} +template +bool set_from_float_ex(float value, bool* property, int) { + return *property = (value >= 0.0f), true; +} +template::value && !std::is_const::value>> +bool set_from_float_ex(float value, T* property, int) { + return *property = static_cast(std::round(value)), true; +} +template +bool set_from_float_ex(float value, T* property, ...) { + return false; +} +template +bool set_from_float(float value, T* property) { + return set_from_float_ex(value, property, 0); +} +} + + +template +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 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 +struct Property { + Property(void* ctx, T(*getter)(void*)) + : ctx_(ctx), getter_(getter) {} + Property(const T* ctx) + : ctx_(const_cast(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 diff --git a/Firmware/fibre-cpp/include/fibre/simple_serdes.hpp b/Firmware/fibre-cpp/include/fibre/simple_serdes.hpp new file mode 100644 index 00000000..2fc11077 --- /dev/null +++ b/Firmware/fibre-cpp/include/fibre/simple_serdes.hpp @@ -0,0 +1,126 @@ +#ifndef __FIBRE_SIMPLE_SERDES +#define __FIBRE_SIMPLE_SERDES + +#include "cpp_utils.hpp" +#include "limits.h" +#include // TODO: make C++11 backport of this +#include + +template +struct SimpleSerializer; +template +using LittleEndianSerializer = SimpleSerializer; +template +using BigEndianSerializer = SimpleSerializer; + + +/* @brief Serializer/deserializer for arbitrary integral number types */ +// TODO: allow reading an arbitrary number of bits +template +struct SimpleSerializer::value>> { + static constexpr size_t BIT_WIDTH = std::numeric_limits::digits; + static constexpr size_t BYTE_WIDTH = (BIT_WIDTH + 7) / 8; + + template + static std::optional 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(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(byte) << (i << 3); + } + } + return result; + } + + template + 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((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((value >> (i << 3)) & 0xff); + **begin = byte; + } + } + return true; + } +}; + +template +inline std::optional read_le(fibre::cbufptr_t* buffer) { + static_assert(is_complete>(), "no LittleEndianSerializer is defined for type T"); + return LittleEndianSerializer::read(&buffer->begin(), buffer->end()); +} + +template +inline bool write_le(T value, fibre::bufptr_t* buffer) { + static_assert(is_complete>(), "no LittleEndianSerializer is defined for type T"); + return LittleEndianSerializer::write(value, &buffer->begin(), buffer->end()); +} + +template::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 std::enable_if_t::value, size_t> +write_le(T value, uint8_t* buffer) { + return write_le>(value, buffer); +} + +template<> +inline size_t write_le(float value, uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + uint32_t value_as_uint32; + std::memcpy(&value_as_uint32, &value, sizeof(uint32_t)); + return write_le(value_as_uint32, buffer); +} + +template +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* value, const uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + return read_le(reinterpret_cast(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 +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 \ No newline at end of file diff --git a/Firmware/fibre-cpp/interfaces_template.j2 b/Firmware/fibre-cpp/interfaces_template.j2 new file mode 100644 index 00000000..cbf7e72f --- /dev/null +++ b/Firmware/fibre-cpp/interfaces_template.j2 @@ -0,0 +1,100 @@ +/*[# 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 + +#include + +#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 -%] +[% for arg in func.out.values() %][[arg.type]][[', ' if not loop.last]][% endfor %] +[%- endif -%] +[%- endmacro %] + +[%- macro render_interface(intf) %] +class [[intf.name | to_pascal_case]]Intf { +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 + 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 static inline auto get_[[property.name]](T* obj) { return [[property.type.c_name]]{&obj->[[property.c_name]]}; } + template 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 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 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 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 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 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 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 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 static auto get_[[func.name | to_snake_case]]_out_[[arg.name]]_(T* obj) { return Property{&obj->[[func.name | to_snake_case]]_out_[[arg.name]]_}; } + template static void get_[[func.name | to_snake_case]]_out_[[arg.name]]_(T* obj, void* ptr) { new (ptr) Property{&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>(a) | static_cast>(b)); } +inline [[enum.c_name]] operator & ([[enum.c_name]] a, [[enum.c_name]] b) { return static_cast<[[enum.c_name]]>(static_cast>(a) & static_cast>(b)); } +inline [[enum.c_name]] operator ^ ([[enum.c_name]] a, [[enum.c_name]] b) { return static_cast<[[enum.c_name]]>(static_cast>(a) ^ static_cast>(b)); } +inline [[enum.c_name]]& operator |= ([[enum.c_name]] &a, [[enum.c_name]] b) { return reinterpret_cast<[[enum.c_name]]&>(reinterpret_cast&>(a) |= static_cast>(b)); } +inline [[enum.c_name]]& operator &= ([[enum.c_name]] &a, [[enum.c_name]] b) { return reinterpret_cast<[[enum.c_name]]&>(reinterpret_cast&>(a) &= static_cast>(b)); } +inline [[enum.c_name]]& operator ^= ([[enum.c_name]] &a, [[enum.c_name]] b) { return reinterpret_cast<[[enum.c_name]]&>(reinterpret_cast&>(a) ^= static_cast>(b)); } +inline [[enum.c_name]] operator ~ ([[enum.c_name]] a) { return static_cast<[[enum.c_name]]>(~static_cast>(a)); } +[%- endif %] +[%- endfor %] + + + +#pragma GCC pop_options + +#endif // __FIBRE_INTERFACES_HPP \ No newline at end of file diff --git a/Firmware/fibre-cpp/legacy_object_client.cpp b/Firmware/fibre-cpp/legacy_object_client.cpp new file mode 100644 index 00000000..abc1262d --- /dev/null +++ b/Firmware/fibre-cpp/legacy_object_client.cpp @@ -0,0 +1,483 @@ + +#include "legacy_object_client.hpp" +#include "legacy_protocol.hpp" +#include "include/fibre/simple_serdes.hpp" +#include "logging.hpp" +#include "print_utils.hpp" +#include "include/fibre/crc.hpp" +#include +#include + +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>; +using json_dict = std::vector, std::shared_ptr>>; +using json_value_variant = std::variant; + +struct json_value : json_value_variant { + //json_value(const json_value_variant& v) : json_value_variant{v} {} + template json_value(T&& arg) : json_value_variant{std::forward(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(key), std::make_shared(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(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>& 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; +} + +std::unordered_map codecs = { + {"bool", 1}, + {"int8", 1}, + {"uint8", 1}, + {"int16", 2}, + {"uint16", 2}, + {"int32", 4}, + {"uint32", 4}, + {"int64", 6}, + {"uint64", 6}, + {"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 parse_arglist(const json_value& list_val) { + std::vector 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), + (size_t)json_as_int(id_val), + get_codec_size(json_as_str(type_val)) + }); + } + + return arglist; +} + +void LegacyObjectClient::start(Completer>& on_found_root_object, Completer& 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(); +} + +void LegacyObjectClient::start_call(size_t ep_num, LegacyFibreFunction* func, cbufptr_t input, bufptr_t output, CallContext** handle, Completer& completer) { + CallContext* call = new CallContext(); + call->ep_num = ep_num; + call->tx_buf = input; + call->rx_buf = output; + call->func = func; + call->completer = &completer; + + if (op_handle_) { + FIBRE_LOG(D) << "Call in progress. Enqueuing this call."; + // An operation is already in progress. Enqueue this one. + pending_calls_.push_back(call); + } else { + // No endpoint operation is in progress. Start this call immediately + FIBRE_LOG(D) << "No call in progress. Starting call now."; + call_ = call; + complete({kStreamOk, nullptr}); + } +} + +void LegacyObjectClient::cancel_call(CallContext* handle) { + if (call_ == handle) { + protocol_->cancel_endpoint_operation(op_handle_); + } else { + auto it = std::find(pending_calls_.begin(), pending_calls_.end(), handle); + if (it != pending_calls_.end()) { + CallContext* call = *it; + pending_calls_.erase(it); + safe_complete(call->completer, {kFibreCancelled, call->rx_buf.end()}); + delete call; + } + } +} + +std::shared_ptr 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; + } + + FibreInterface intf; + size_t size = get_codec_size(codec); + + if (!size) { + FIBRE_LOG(W) << "unknown size for codec " << codec; + } + + intf.name = std::string{} + "fibre.Property<" + (write ? "readwrite" : "readonly") + " " + codec + ">"; + intf.functions["read"] = {0, {}, {{"value", codec, 0, size}}}; + if (write) { + intf.functions["exchange"] = {0, {{"newval", codec, 0, size}}, {{"oldval", codec, 0, size}}}; + } + + return dict[codec] = std::make_shared(intf); +} + +std::shared_ptr LegacyObjectClient::load_object(json_value list_val) { + FibreInterface intf; + + if (!json_is_list(list_val)) { + FIBRE_LOG(W) << "interface members must be a list"; + return nullptr; + } + + 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 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[name] = { + (size_t)json_as_int(id), + 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(subobj); + objects_.push_back(subobj_ptr); + intf.attributes[name] = {subobj_ptr}; + + } else { + FIBRE_LOG(W) << "unsupported codec"; + } + } + + LegacyObject obj{ + .client = this, + .ep_num = 0, + .intf = std::make_shared(intf), + .known_to_application = false + }; + auto obj_ptr = std::make_shared(obj); + objects_.push_back(obj_ptr); + return obj_ptr; +} + +void LegacyObjectClient::receive_more_json() { + write_le(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_, *this); +} + +void LegacyObjectClient::complete(EndpointOperationResult result) { + op_handle_ = 0; + + if (result.status == kStreamCancelled) { + if (call_) { + auto call = call_; + call_ = nullptr; + safe_complete(call->completer, {kFibreCancelled, call->rx_buf.end()}); + delete call; + } + return; + } else if (result.status == kStreamClosed) { + if (call_) { + auto call = call_; + call_ = nullptr; + safe_complete(call->completer, {kFibreClosed, call->rx_buf.end()}); + delete call; + } + return; + } else if (result.status != kStreamOk) { + FIBRE_LOG(W) << "endpoint operation failed"; // TODO: add retry logic + if (call_) { + auto call = call_; + call_ = nullptr; + safe_complete(call->completer, {kFibreInternalError, call->rx_buf.end()}); + delete call; + } + return; + } + + if (call_) { + // The endpoint operation that completed belongs to the active call + + LegacyFibreFunction* func = call_->func; + + if (call_->ep_num && !call_->progress) { + // Read/write/exchange property + FIBRE_LOG(D) << "starting property transaction on " << call_->ep_num << " with tx buf len " << call_->tx_buf.size() << " and rx len " << call_->rx_buf.size(); + call_->progress++; + protocol_->start_endpoint_operation(call_->ep_num, call_->tx_buf, call_->rx_buf, &op_handle_, *this); + + } else if (!call_->ep_num && call_->progress < func->inputs.size()) { + // Write input arg + size_t argnum = call_->progress; + call_->progress++; + cbufptr_t current_buf = call_->tx_buf.take(func->inputs[argnum].size); + call_->tx_buf = call_->tx_buf.skip(func->inputs[argnum].size); + protocol_->start_endpoint_operation(func->inputs[argnum].ep_num, current_buf, call_->rx_buf.take(0), &op_handle_, *this); + + } else if (!call_->ep_num && call_->progress == func->inputs.size()) { + // Trigger + // call_->tx_buf should be empty by now + call_->progress++; + protocol_->start_endpoint_operation(func->ep_num, call_->tx_buf, call_->rx_buf.take(0), &op_handle_, *this); + + } else if (!call_->ep_num && call_->progress < func->inputs.size() + 1 + func->outputs.size()) { + // Read output arg + size_t argnum = call_->progress - func->inputs.size() - 1; + call_->progress++; + bufptr_t current_buf = call_->rx_buf.take(func->outputs[argnum].size); + call_->rx_buf = call_->rx_buf.skip(func->outputs[argnum].size); + protocol_->start_endpoint_operation(func->outputs[argnum].ep_num, call_->tx_buf, current_buf, &op_handle_, *this); + + } else { + CallContext* call = call_; + call_ = nullptr; + FIBRE_LOG(D) << "call completed!"; + safe_complete(call->completer, {kFibreOk, call->rx_buf.end()}); + delete call; + } + + } else { + // The endpoint operation that completed belongs to the JSON fetch process + + 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(); + return; + + } 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(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(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(PROTOCOL_VERSION, json_.data(), json_.size()); + if (root_obj_) { + safe_complete(on_found_root_object_, this, root_obj_); + } + } + } + + // Start next call in the queue if any + // It's possible that the next function call was already started on one of + // the callbacks above. + if (!call_ && pending_calls_.size()) { + call_ = pending_calls_[0]; + pending_calls_.erase(pending_calls_.begin()); + complete({kStreamOk, nullptr}); + } +} \ No newline at end of file diff --git a/Firmware/fibre-cpp/legacy_object_client.hpp b/Firmware/fibre-cpp/legacy_object_client.hpp new file mode 100644 index 00000000..1f2e8453 --- /dev/null +++ b/Firmware/fibre-cpp/legacy_object_client.hpp @@ -0,0 +1,107 @@ +#ifndef __FIBRE_LEGACY_OBJECT_MODEL_HPP +#define __FIBRE_LEGACY_OBJECT_MODEL_HPP + +//#include "legacy_protocol.hpp" +#include "async_stream.hpp" +#include +#include +#include +#include +#include + +struct json_value; + +namespace fibre { + +struct EndpointOperationResult { + StreamStatus status; + uint8_t* rx_end; +}; + +using EndpointOperationHandle = uint32_t; + +struct LegacyProtocolPacketBased; + +struct LegacyFibreArg { + std::string name; + std::string codec; + size_t ep_num; + size_t size; +}; + +struct LegacyFibreFunction { + size_t ep_num; // 0 for property read/write/exchange functions + std::vector inputs; + std::vector outputs; +}; + +struct FibreInterface; +struct LegacyObjectClient; +struct LegacyObject; + +struct LegacyFibreAttribute { + std::shared_ptr object; +}; + +struct FibreInterface { + std::string name; + std::unordered_map functions; + std::unordered_map attributes; +}; + +struct LegacyObject { + LegacyObjectClient* client; + size_t ep_num; + std::shared_ptr intf; + bool known_to_application; +}; + +class LegacyObjectClient : Completer { +public: + struct CallResult { + FibreStatus status; + uint8_t* end; + }; + struct CallContext { + size_t progress = 0; + size_t ep_num; + cbufptr_t tx_buf; + bufptr_t rx_buf; + LegacyFibreFunction* func; + Completer* completer; + }; + + LegacyObjectClient(LegacyProtocolPacketBased* protocol) : protocol_(protocol) {} + + void start(Completer>& on_found_root_object, Completer& on_lost_root_object); + + void start_call(size_t ep_num, LegacyFibreFunction* func, cbufptr_t input, bufptr_t output, CallContext** handle, Completer& completer); + void cancel_call(CallContext* handle); + + // For direct access by LegacyProtocolPacketBased and libfibre.cpp + uint16_t json_crc_ = 0; + Completer* on_lost_root_object_; + std::shared_ptr root_obj_; + std::vector> objects_; + void* user_data_; // used by libfibre to store the libfibre context pointer + +private: + std::shared_ptr get_property_interfaces(std::string codec, bool write); + std::shared_ptr load_object(json_value list_val); + void receive_more_json(); + void complete(EndpointOperationResult result); + + LegacyProtocolPacketBased* protocol_; + Completer>* on_found_root_object_; + uint8_t tx_buf_[4] = {0xff, 0xff, 0xff, 0xff}; + EndpointOperationHandle op_handle_ = 0; + std::vector json_; + CallContext* call_ = nullptr; // active call + std::vector pending_calls_; + std::unordered_map> rw_property_interfaces; + std::unordered_map> ro_property_interfaces; +}; + +} + +#endif // __FIBRE_LEGACY_OBJECT_MODEL_HPP \ No newline at end of file diff --git a/Firmware/fibre-cpp/legacy_protocol.cpp b/Firmware/fibre-cpp/legacy_protocol.cpp new file mode 100644 index 00000000..f9f052bf --- /dev/null +++ b/Firmware/fibre-cpp/legacy_protocol.cpp @@ -0,0 +1,520 @@ + + +#include "legacy_protocol.hpp" + +#include +#include +#include "logging.hpp" +#include "print_utils.hpp" +#include "async_stream.hpp" +#include +#include + +DEFINE_LOG_TOPIC(LEGACY_PROTOCOL); +USE_LOG_TOPIC(LEGACY_PROTOCOL); + +using namespace fibre; + + +/* PacketWrapper -------------------------------------------------------------*/ + +void PacketWrapper::start_write(cbufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(this); + } + + if (state_ != kStateIdle) { + completer.complete({kStreamError, buffer.begin()}); + } + + // TODO: support buffer size >= 128 + if (buffer.size() >= 128) { + completer.complete({kStreamError, buffer.begin()}); + } + + completer_ = &completer; + + header_buf_[0] = CANONICAL_PREFIX; + header_buf_[1] = static_cast(buffer.size()); + header_buf_[2] = calc_crc8(CANONICAL_CRC8_INIT, header_buf_, 2); + + payload_buf_ = buffer; + + uint16_t crc16 = calc_crc16(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_, *this); +} + +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; + safe_complete(completer_, {kStreamCancelled, payload_buf_.begin()}); + return; + } + + if (result.status != kStreamOk) { + state_ = kStateIdle; + safe_complete(completer_, {result.status, payload_buf_.begin()}); + return; + } + + if (result.end < expected_tx_end_) { + tx_channel_->start_write({result.end, expected_tx_end_}, &inner_transfer_handle_, *this); + return; + } + + if (state_ == kStateSendingHeader) { + state_ = kStateSendingPayload; + expected_tx_end_ = payload_buf_.end(); + tx_channel_->start_write(payload_buf_, &inner_transfer_handle_, *this); + + } else if (state_ == kStateSendingPayload) { + state_ = kStateSendingTrailer; + expected_tx_end_ = trailer_buf_ + 2; + tx_channel_->start_write(trailer_buf_, &inner_transfer_handle_, *this); + + } else if (state_ == kStateSendingTrailer) { + state_ = kStateIdle; + safe_complete(completer_, {kStreamOk, payload_buf_.end()}); + } +} + + +/* PacketUnwrapper -----------------------------------------------------------*/ + +void PacketUnwrapper::start_read(bufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(this); + } + + if (state_ != kStateIdle) { + completer.complete({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_, *this); +} + +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; + safe_complete(completer_, {kStreamCancelled, payload_buf_.begin()}); + return; + } + + if (result.status != kStreamOk) { + state_ = kStateIdle; + safe_complete(completer_, {result.status, payload_buf_.begin()}); + return; + } + + if (result.end < expected_rx_end_) { + rx_channel_->start_read({result.end, expected_rx_end_}, &inner_transfer_handle_, *this); + 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_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_, *this); + 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_, *this); + + } else if (state_ == kStateReceivingPayload) { + expected_rx_end_ = rx_buf_ + 2; + state_ = kStateReceivingTrailer; + rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, *this); + + } else if (state_ == kStateReceivingTrailer) { + uint16_t crc = calc_crc16(CANONICAL_CRC16_INIT, payload_buf_.begin(), payload_length_); + crc = calc_crc16(crc, rx_buf_, 2); + + if (!crc) { + state_ = kStateIdle; + safe_complete(completer_, {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_, *this); + } + } +} + + +/* LegacyProtocolPacketBased -------------------------------------------------*/ + +#ifdef 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, Completer& completer) { + if (tx_buf.size() + 8 >= tx_mtu_) { + FIBRE_LOG(E) << "packet too large"; + completer.complete({kStreamError, rx_buf.begin()}); + } + + if (rx_buf.size() > 0xffff) { + FIBRE_LOG(E) << "receive size larger than 65535 currently not supported"; + completer.complete({kStreamError, rx_buf.begin()}); + } + + 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, + .completer = &completer + }; + + 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 + if (pending_operation_.completer) { + // Previous endpoint operation was not yet sent. We don't support + // enqueuing multiple endpoint operations while the first didn't send yet. + FIBRE_LOG(E) << "previous endpoint operation still not sent"; + completer.complete({kStreamError, rx_buf.begin()}); + } else { + // Control is returned to start_endpoint_operation once TX completes + pending_operation_ = op; + } + return; + } + + start_endpoint_operation(op); +} + +void LegacyProtocolPacketBased::start_endpoint_operation(EndpointOperation op) { + write_le(op.seqno, tx_buf_); + write_le(op.endpoint_id | 0x8000, tx_buf_ + 2); + write_le(op.rx_buf.size(), tx_buf_ + 4); + + memcpy(tx_buf_ + 6, op.tx_buf.begin(), op.tx_buf.size()); + + uint16_t trailer = (op.endpoint_id & 0x7fff) == 0 ? + PROTOCOL_VERSION : client_.json_crc_; + + write_le(trailer, tx_buf_ + 6 + op.tx_buf.size()); + + expected_acks_[op.seqno] = op; + transmitting_op_ = op.seqno | 0xffff0000; + tx_channel_->start_write(cbufptr_t{tx_buf_}.take(8 + op.tx_buf.size()), &tx_handle_, *static_cast(this)); +} + + +void LegacyProtocolPacketBased::cancel_endpoint_operation(EndpointOperationHandle handle) { + if (!handle) { + return; + } + + uint16_t seqno = static_cast(handle & 0xffff); + + Completer* completer; + uint8_t* rx_end = nullptr; + + if (pending_operation_.seqno == seqno) { + completer = pending_operation_.completer; + rx_end = pending_operation_.rx_buf.begin(); + pending_operation_ = {}; + } + + auto it = expected_acks_.find(handle); + + if (it != expected_acks_.end()) { + completer = it->second.completer; + rx_end = it->second.rx_buf.begin(); + expected_acks_.erase(it); + } + + 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. + safe_complete(completer, {kStreamCancelled, rx_end}); + } +} + +#endif + +#ifdef 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 offset = read_le(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(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 FIBRE_ENABLE_CLIENT + if (transmitting_op_) { + uint16_t seqno = transmitting_op_ & 0xffff; + transmitting_op_ = 0; + + // If the TX task was a remote endpoint operation but didn't succeed + // we terminate that operation + if (result.status != kStreamOk) { + auto it = expected_acks_.find(seqno); + auto completer = it->second.completer; + auto rx_end = it->second.rx_buf.begin(); + expected_acks_.erase(it); + safe_complete(completer, {result.status, rx_end}); + } + } +#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}); + return; + } +#endif + +#if FIBRE_ENABLE_CLIENT + if (pending_operation_.completer) { + // There is a write operation pending from the client side (i.e. an + // outgoing remote endpoint operation). + EndpointOperation op = pending_operation_; + pending_operation_ = {}; + start_endpoint_operation(op); + return; + } +#endif +} + +void LegacyProtocolPacketBased::on_read_finished(ReadResult result) { + TransferHandle dummy; + + if (result.status == kStreamClosed) { + FIBRE_LOG(D) << "RX stream closed."; + on_closed(kStreamClosed); + return; + } else if (result.status == kStreamCancelled) { + FIBRE_LOG(W) << "RX operation cancelled."; + // TODO: close stream + 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_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 seq_no = read_le(&rx_buf); + + if (!seq_no.has_value()) { + FIBRE_LOG(W) << "packet too short"; + + } else if (*seq_no & 0x8000) { + +#ifdef 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); + uint8_t* rx_end = it->second.rx_buf.begin() + n_copy; + auto completer = it->second.completer; + expected_acks_.erase(it); + safe_complete(completer, {kStreamOk, rx_end}); + } + +#else + FIBRE_LOG(W) << "received ack but client support is not compiled in"; +#endif + + } else { + +#ifdef FIBRE_ENABLE_SERVER + if (rx_buf.size() < 6) { + FIBRE_LOG(W) << "packet too short"; + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); + 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(&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, *static_cast(this)); + 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(&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(*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_, *static_cast(this)); + } +#else + FIBRE_LOG(W) << "received request but server support is not compiled in"; +#endif + } + + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); +} + +void LegacyProtocolPacketBased::on_closed(StreamStatus status) { + +#ifdef FIBRE_ENABLE_CLIENT + // Cancel all ongoing endpoint operations + for (auto& item: expected_acks_) { + if (item.second.completer) + (*item.second.completer).complete({status, item.second.rx_buf.begin()}); + } + expected_acks_.clear(); + + // Report that the root object was lost + if (client_.on_lost_root_object_ && client_.root_obj_) { + client_.root_obj_ = nullptr; + client_.on_lost_root_object_->complete(&client_); + } +#endif + safe_complete(on_stopped_, this, status); +} + +#if FIBRE_ENABLE_CLIENT +void LegacyProtocolPacketBased::start(Completer>& on_found_root_object, Completer& on_lost_root_object, Completer& on_stopped) { +#else +void LegacyProtocolPacketBased::start(Completer& on_stopped) { +#endif + on_stopped_ = &on_stopped; + TransferHandle dummy; + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); + +#if FIBRE_ENABLE_CLIENT + if (on_stopped_) { + client_.start(on_found_root_object, on_lost_root_object); + } +#endif +} diff --git a/Firmware/fibre-cpp/legacy_protocol.hpp b/Firmware/fibre-cpp/legacy_protocol.hpp new file mode 100644 index 00000000..61a16cc1 --- /dev/null +++ b/Firmware/fibre-cpp/legacy_protocol.hpp @@ -0,0 +1,165 @@ +#ifndef __FIBRE_LEGACY_PROTOCOL_HPP +#define __FIBRE_LEGACY_PROTOCOL_HPP + +#include "async_stream.hpp" + +#ifdef FIBRE_ENABLE_CLIENT +#include "legacy_object_client.hpp" +#include +#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, Completer { +public: + PacketWrapper(AsyncStreamSink* tx_channel) + : tx_channel_(tx_channel) {} + + void start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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}; + Completer* completer_; + + enum { + kStateIdle, + kStateCancelling, + kStateSendingHeader, + kStateSendingPayload, + kStateSendingTrailer + } state_ = kStateIdle; +}; + + +class PacketUnwrapper : public AsyncStreamSource, Completer { +public: + PacketUnwrapper(AsyncStreamSource* rx_channel) + : rx_channel_(rx_channel) {} + + void start_read(bufptr_t buffer, TransferHandle* handle, Completer& 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}; + Completer* completer_; + + enum { + kStateIdle, + kStateCancelling, + kStateReceivingHeader, + kStateReceivingPayload, + kStateReceivingTrailer + } state_ = kStateIdle; +}; + + +struct LegacyProtocolPacketBased : ReadCompleter, WriteCompleter { +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 + + Completer* on_stopped_ = nullptr; + +#ifdef FIBRE_ENABLE_CLIENT + void start_endpoint_operation(uint16_t endpoint_id, cbufptr_t tx_buf, bufptr_t rx_buf, EndpointOperationHandle* handle, Completer& completer); + void cancel_endpoint_operation(EndpointOperationHandle handle); + + LegacyObjectClient client_{this}; +#endif + +#ifdef FIBRE_ENABLE_CLIENT + void start(Completer>& on_found_root_object, Completer& on_lost_root_object, Completer& on_stopped); +#else + void start(Completer& on_stopped); +#endif + +private: + +#ifdef FIBRE_ENABLE_CLIENT + struct EndpointOperation { + uint16_t seqno; + uint16_t endpoint_id; + cbufptr_t tx_buf; + bufptr_t rx_buf; + Completer* completer; + }; + + void start_endpoint_operation(EndpointOperation op); + + uint16_t outbound_seq_no_ = 0; + EndpointOperation pending_operation_{.completer = nullptr}; // operation that is waiting for TX + EndpointOperationHandle transmitting_op_ = 0; // operation that is in TX + std::unordered_map expected_acks_; // operations that are waiting for RX +#endif + + void on_write_finished(WriteResult result); + void on_read_finished(ReadResult result); + void on_closed(StreamStatus status); +}; + + +struct LegacyProtocolStreamBased { +public: + LegacyProtocolStreamBased(AsyncStreamSource* rx_channel, AsyncStreamSink* tx_channel) + : unwrapper_(rx_channel), wrapper_(tx_channel) {} + + +#ifdef FIBRE_ENABLE_CLIENT + void start(Completer>& on_found_root_object, Completer& on_lost_root_object, Completer& on_stopped) { + inner_protocol_.start(on_found_root_object, on_lost_root_object, on_stopped); + } +#else + void start(Completer& on_stopped) { inner_protocol_.start(on_stopped); } +#endif + +private: + PacketUnwrapper unwrapper_; + PacketWrapper wrapper_; + LegacyProtocolPacketBased inner_protocol_{&unwrapper_, &wrapper_, 127}; +}; + +} + +#endif // __FIBRE_LEGACY_PROTOCOL_HPP diff --git a/Firmware/fibre-cpp/libfibre-macos-x86.dylib b/Firmware/fibre-cpp/libfibre-macos-x86.dylib new file mode 100644 index 00000000..b881a087 --- /dev/null +++ b/Firmware/fibre-cpp/libfibre-macos-x86.dylib @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad86cbdadd274245b9220f9728c1fc05ae05a1f196510a35e78e37309b3d6adf +size 2195160 diff --git a/Firmware/fibre-cpp/libfibre.cpp b/Firmware/fibre-cpp/libfibre.cpp new file mode 100644 index 00000000..edf09689 --- /dev/null +++ b/Firmware/fibre-cpp/libfibre.cpp @@ -0,0 +1,511 @@ + +#include +#include "platform_support/libusb_transport.hpp" +#include "logging.hpp" +#include "print_utils.hpp" +#include "legacy_protocol.hpp" +#include "legacy_object_client.hpp" +#include "stdio.h" // TODO: remove +#include "string.h" +#include +#include "fibre/simple_serdes.hpp" + +DEFINE_LOG_TOPIC(LIBFIBRE); +USE_LOG_TOPIC(LIBFIBRE); + +static const struct LibFibreVersion libfibre_version = { 0, 1, 0 }; + +class FIBRE_PRIVATE ExternalEventLoop : public EventLoop { +public: + ExternalEventLoop(post_cb_t post, + register_event_cb_t register_event, + deregister_event_cb_t deregister_event, + call_later_cb_t call_later, + cancel_timer_cb_t cancel_timer) : + post_(post), + register_event_(register_event), + deregister_event_(deregister_event), + call_later_(call_later), + cancel_timer_(cancel_timer) {} + + int post(void (*callback)(void*), void *ctx) final { + return (*post_)(callback, ctx); + } + + int register_event(int event_fd, uint32_t events, void (*callback)(void*), void* ctx) final { + return (*register_event_)(event_fd, events, callback, ctx); + } + + int deregister_event(int event_fd) final { + return (*deregister_event_)(event_fd); + } + + struct EventLoopTimer* call_later(float delay, void (*callback)(void*), void *ctx) final { + return (*call_later_)(delay, callback, ctx); + } + + int cancel_timer(struct EventLoopTimer* timer) final { + return (*cancel_timer_)(timer); + } + +private: + post_cb_t post_; + register_event_cb_t register_event_; + deregister_event_cb_t deregister_event_; + call_later_cb_t call_later_; + cancel_timer_cb_t cancel_timer_; +}; + +struct FIBRE_PRIVATE LibFibreCtx { + ExternalEventLoop* event_loop; + construct_object_cb_t on_construct_object; + destroy_object_cb_t on_destroy_object; + void* cb_ctx; + fibre::LibusbDiscoverer libusb_discoverer; + size_t n_discoveries = 0; +}; + +struct FIBRE_PRIVATE LibFibreDiscoveryCtx : + fibre::Completer, + fibre::Completer>, + fibre::Completer, + fibre::Completer +{ + void complete(fibre::ChannelDiscoveryResult result) final; + void complete(fibre::LegacyObjectClient* obj_client, std::shared_ptr intf) final; + void complete(fibre::LegacyObjectClient* obj_client) final; + void complete(fibre::LegacyProtocolPacketBased* protocol, fibre::StreamStatus status) final; + + fibre::LibusbDiscoverer::ChannelDiscoveryContext* libusb_discovery_ctx = nullptr; + on_found_object_cb_t on_found_object; + void* cb_ctx; + LibFibreCtx* ctx; + std::vector protocol_instances; +}; + + + +// Callback for start_channel_discovery() +void LibFibreDiscoveryCtx::complete(fibre::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; + } + + const size_t mtu = 64; // TODO: get MTU from channel specific data + + auto protocol = new fibre::LegacyProtocolPacketBased(result.rx_channel, result.tx_channel, mtu); + protocol->client_.user_data_ = ctx; + protocol_instances.push_back(protocol); + protocol->start(*this, *this, *this); +} + +// on_found_root_object callback for LegacyProtocolPacketBased::start() +void LibFibreDiscoveryCtx::complete(fibre::LegacyObjectClient* obj_client, std::shared_ptr obj) { + auto obj_cast = reinterpret_cast(obj.get()); // corresponding reverse cast in libfibre_get_attribute() + auto intf_cast = reinterpret_cast(obj->intf.get()); // corresponding reverse cast in libfibre_subscribe_to_interface() + + for (auto& obj: obj_client->objects_) { + // If the callback handler calls libfibre_get_attribute() before + // all objects were announced to the application then it's possible + // that during that function call some objects are already announced + // on-demand. + if (!obj->known_to_application) { + obj->known_to_application = true; + //FIBRE_LOG(D) << "constructing root object " << fibre::as_hex(reinterpret_cast(obj.get())); + if (ctx->on_construct_object) { + (*ctx->on_construct_object)(ctx->cb_ctx, + reinterpret_cast(obj.get()), + reinterpret_cast(obj->intf.get()), + obj->intf->name.size() ? obj->intf->name.data() : nullptr, obj->intf->name.size()); + } + } + } + + if (on_found_object) { + FIBRE_LOG(D) << "announcing root object " << fibre::as_hex(reinterpret_cast(obj_cast)); + (*on_found_object)(cb_ctx, obj_cast); + } +} + +// on_lost_root_object for LegacyProtocolPacketBased::start() +void LibFibreDiscoveryCtx::complete(fibre::LegacyObjectClient* obj_client) { + if (ctx->on_destroy_object) { + for (auto obj: obj_client->objects_) { + auto obj_cast = reinterpret_cast(obj.get()); + //FIBRE_LOG(D) << "destroying subobject " << fibre::as_hex(reinterpret_cast(obj_cast)); + (*ctx->on_destroy_object)(ctx->cb_ctx, obj_cast); + } + + obj_client->objects_.clear(); + } +} + +// on_stopped callback for LegacyProtocolPacketBased::start() +void LibFibreDiscoveryCtx::complete(fibre::LegacyProtocolPacketBased* protocol, fibre::StreamStatus status) { + delete protocol; +} + +const struct LibFibreVersion* libfibre_get_version() { + return &libfibre_version; +} + +LibFibreCtx* libfibre_open( + post_cb_t post, + register_event_cb_t register_event, + deregister_event_cb_t deregister_event, + call_later_cb_t call_later, + cancel_timer_cb_t cancel_timer, + construct_object_cb_t construct_object, + destroy_object_cb_t destroy_object, + void* cb_ctx) +{ + if (!register_event || !deregister_event) { + FIBRE_LOG(E) << "invalid argument"; + return nullptr; + } + + LibFibreCtx* ctx = new LibFibreCtx(); + ctx->event_loop = new ExternalEventLoop(post, register_event, deregister_event, call_later, cancel_timer); + ctx->on_construct_object = construct_object; + ctx->on_destroy_object = destroy_object; + ctx->cb_ctx = cb_ctx; + + if (ctx->libusb_discoverer.init(ctx->event_loop) != 0) { + delete ctx; + FIBRE_LOG(E) << "failed to init libusb transport layer"; + return nullptr; + } + + FIBRE_LOG(D) << "opened (" << fibre::as_hex((uintptr_t)ctx) << ")"; + return ctx; +} + +void libfibre_close(LibFibreCtx* ctx) { + if (ctx->n_discoveries) { + FIBRE_LOG(W) << "there are still discovery processes ongoing"; + } + + ctx->libusb_discoverer.deinit(); + delete ctx->event_loop; + delete ctx; + + FIBRE_LOG(D) << "closed (" << fibre::as_hex((uintptr_t)ctx) << ")"; +} + +void libfibre_start_discovery(LibFibreCtx* ctx, const char* specs, size_t specs_len, struct LibFibreDiscoveryCtx** handle, + on_found_object_cb_t on_found_object, on_stopped_cb_t on_stopped, void* cb_ctx) { + if (!ctx) { + FIBRE_LOG(E) << "invalid argument"; + if (on_stopped) { + (*on_stopped)(cb_ctx, kFibreInvalidArgument); + } + return; + } + + const char* prev_delim = specs; + + FIBRE_LOG(D) << "starting discovery with path \"" << std::string(specs, specs_len) << "\""; + + LibFibreDiscoveryCtx* discovery_ctx = new LibFibreDiscoveryCtx(); + discovery_ctx->on_found_object = on_found_object; + discovery_ctx->cb_ctx = cb_ctx; + discovery_ctx->ctx = ctx; + + if (handle) { + *handle = discovery_ctx; + } + + while (prev_delim < specs + specs_len) { + const char* next_delim = std::find(prev_delim, specs + specs_len, ';'); + const char* colon = std::find(prev_delim, next_delim, ':'); + const char* colon_end = std::min(colon + 1, next_delim); + + if ((colon - prev_delim) == strlen("usb") && std::equal(prev_delim, colon, "usb")) { + ctx->libusb_discoverer.start_channel_discovery(colon_end, next_delim - colon_end, + &discovery_ctx->libusb_discovery_ctx, *discovery_ctx); + } else { + FIBRE_LOG(W) << "transport layer \"" << std::string(prev_delim, colon - prev_delim) << "\" not implemented"; + } + + prev_delim = std::min(next_delim + 1, specs + specs_len); + } + + ctx->n_discoveries++; +} + +void libfibre_stop_discovery(LibFibreCtx* ctx, LibFibreDiscoveryCtx* discovery_ctx) { + if (!ctx->n_discoveries) { + FIBRE_LOG(W) << "stopping a discovery process but none is active"; + } else { + ctx->n_discoveries--; + } + + if (discovery_ctx->libusb_discovery_ctx) { + // TODO: implement "stopped" callback + ctx->libusb_discoverer.stop_channel_discovery(discovery_ctx->libusb_discovery_ctx); + } + + delete discovery_ctx; +} + +const char* transform_codec(std::string& codec) { + if (codec == "endpoint_ref") { + return "object_ref"; + } else { + return codec.data(); + } +} + +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(interface); // corresponding reverse cast in LibFibreDiscoveryCtx::complete() and libfibre_subscribe_to_interface() + + for (auto& func: intf->functions) { + std::vector input_names; + std::vector input_codecs; + std::vector output_names; + std::vector output_codecs; + for (auto& arg: func.second.inputs) { + input_names.push_back(arg.name.data()); + input_codecs.push_back(transform_codec(arg.codec)); + } + for (auto& arg: func.second.outputs) { + output_names.push_back(arg.name.data()); + output_codecs.push_back(transform_codec(arg.codec)); + } + 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, + reinterpret_cast(&func.second), // corresponding reverse cast in libfibre_start_call() + 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(&attr.second), // corresponding reverse cast in libfibre_get_attribute() + attr.first.data(), attr.first.size(), + reinterpret_cast(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() + ); + } + } +} + +FibreStatus 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(parent_obj); + fibre::LegacyFibreAttribute* attr_cast = reinterpret_cast(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& 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(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(child_obj)); + (*libfibre_ctx->on_construct_object)(libfibre_ctx->cb_ctx, + reinterpret_cast(child_obj), + reinterpret_cast(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(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& 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())); + } +} + +void transcode(fibre::LegacyObjectClient* client, std::vector& buffer, const std::vector& args, bool to) { + size_t offset = 0; + + for (auto& arg: args) { + if (to && arg.codec == "endpoint_ref") { + fibre::cbufptr_t orig_range = fibre::cbufptr_t{buffer}.skip(offset).take(sizeof(uintptr_t)); + + uintptr_t val = *reinterpret_cast(orig_range.begin()); + + resize_at(buffer, offset, (ssize_t)4 - (ssize_t)sizeof(uintptr_t)); + fibre::bufptr_t transcoded_range = fibre::bufptr_t{buffer}.skip(offset).take(4); + + auto obj = reinterpret_cast(val); + write_le(obj ? obj->ep_num : 0, &transcoded_range); + write_le(obj ? obj->client->json_crc_ : 0, &transcoded_range); + + offset += 4; + + } else if (!to && arg.codec == "endpoint_ref") { + fibre::cbufptr_t orig_range = fibre::cbufptr_t{buffer}.skip(offset).take(4); + + uint16_t ep_num = *read_le(&orig_range); + uint16_t json_crc = *read_le(&orig_range); + + resize_at(buffer, offset, (ssize_t)sizeof(uintptr_t) - (ssize_t)4); + fibre::bufptr_t transcoded_range = fibre::bufptr_t{buffer}.skip(offset).take(sizeof(uintptr_t)); + + fibre::LegacyObject* obj_ptr = nullptr; + + if (ep_num && json_crc == client->json_crc_) { + for (auto& known_obj: client->objects_) { + if (known_obj->ep_num == ep_num) { + obj_ptr = known_obj.get(); + } + } + } + + FIBRE_LOG(D) << "placing transcoded ptr " << reinterpret_cast(obj_ptr); + *reinterpret_cast(transcoded_range.begin()) = reinterpret_cast(obj_ptr); + + offset += sizeof(uintptr_t); + + } else { + offset += arg.size; + } + } +} + +struct FIBRE_PRIVATE LibFibreCallContext : fibre::Completer { + void complete(fibre::LegacyObjectClient::CallResult output) final { + + FIBRE_LOG(D) << "received " << (output.end - rx_vec.data()) << " bytes "; + + // Prune vector to the end of valid data + rx_vec.erase(rx_vec.begin() + (output.end - rx_vec.data()), rx_vec.end()); + + transcode(obj->client, rx_vec, func->outputs, false); + + size_t len = std::min(rx_vec.size(), rx_buf.size()); + FIBRE_LOG(D) << "copying " << rx_vec.size() << " or " << rx_buf.size() << " bytes to output"; + std::copy(rx_vec.begin(), rx_vec.begin() + len, rx_buf.begin()); + FIBRE_LOG(D) << "result is " << as_hex(rx_buf); + + if (on_completed) { + (*on_completed)(ctx, output.status, rx_buf.begin() + len); + } + delete this; + } + + fibre::LegacyObject* obj = nullptr; + fibre::LegacyFibreFunction* func = nullptr; + void (*on_completed)(void*, FibreStatus, uint8_t*); + void* ctx; + fibre::cbufptr_t tx_buf; // application-owned buffer + fibre::bufptr_t rx_buf; // application-owned buffer + std::vector tx_vec; // libfibre-owned buffer used after transcoding from application buffer + std::vector rx_vec; // libfibre-owned buffer used before transcoding to application buffer + fibre::LegacyObjectClient::CallContext* handle; +}; + +void libfibre_start_call(LibFibreObject* obj, LibFibreFunction* func, + const uint8_t *input, size_t input_length, + uint8_t *output, size_t output_length, + LibFibreCallContext** handle, + on_call_completed_cb_t on_completed, void* cb_ctx) { + if (!obj || !func || !input || !output) { + if (on_completed) { + (*on_completed)(cb_ctx, kFibreInvalidArgument, output); + } + return; + } + + fibre::LegacyObject* obj_cast = reinterpret_cast(obj); + fibre::LegacyFibreFunction* func_cast = reinterpret_cast(func); + + bool is_member = std::find_if(obj_cast->intf->functions.begin(), obj_cast->intf->functions.end(), + [&](std::pair& kv) { + return &kv.second == func_cast; + }) != obj_cast->intf->functions.end(); + + if (!is_member) { + FIBRE_LOG(W) << "attempt to invoke function on an object that does not implement it"; + if (on_completed) { + (*on_completed)(cb_ctx, kFibreInvalidArgument, output); + } + return; + } + + std::vector tx_vec; + tx_vec.insert(tx_vec.begin(), input, input + input_length); + + + auto completer = new LibFibreCallContext(); + completer->obj = obj_cast; + completer->func = func_cast; + completer->on_completed = on_completed; + completer->ctx = cb_ctx; + completer->tx_buf = fibre::cbufptr_t{input, input + input_length}; + completer->rx_buf = fibre::bufptr_t{output, output + output_length}; + + completer->tx_vec.insert(completer->tx_vec.begin(), completer->tx_buf.begin(), completer->tx_buf.end()); + transcode(obj_cast->client, completer->tx_vec, func_cast->inputs, true); + + size_t output_size = 0; + for (auto& arg: func_cast->outputs) + output_size += arg.size; + FIBRE_LOG(D) << "sizing output to " << output_size; + completer->rx_vec.resize(output_size); + + if (handle) { + *handle = completer; + } + + obj_cast->client->start_call(obj_cast->ep_num, func_cast, + fibre::cbufptr_t{completer->tx_vec}, fibre::bufptr_t{completer->rx_vec}, + &completer->handle, *completer); +} + +void libfibre_cancel_call(LibFibreCallContext* handle) { + if (handle && handle->on_completed) { + handle->obj->client->cancel_call(handle->handle); + } +} diff --git a/Firmware/fibre-cpp/logging.cpp b/Firmware/fibre-cpp/logging.cpp new file mode 100644 index 00000000..31f37113 --- /dev/null +++ b/Firmware/fibre-cpp/logging.cpp @@ -0,0 +1,20 @@ + +#include "logging.hpp" + +#if !defined(_WIN32) && !defined(_WIN64) && !defined(__linux__) && !defined(__APPLE__) + +namespace std { +StdoutStream cerr; +} + +#endif + +namespace fibre { + +Logger logger{}; + +Logger* get_logger() { + return &logger; +} + +} diff --git a/Firmware/fibre-cpp/logging.hpp b/Firmware/fibre-cpp/logging.hpp new file mode 100644 index 00000000..a4aeac62 --- /dev/null +++ b/Firmware/fibre-cpp/logging.hpp @@ -0,0 +1,339 @@ +/** + * @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 + +// TODO: support lite-version of logging on embedded systems +#if defined(_WIN32) || defined(_WIN64) || defined(__linux__) || defined(__APPLE__) + +#include + +#include +#include + +#if defined(_WIN32) || defined(_WIN64) +#include "windows.h" +#endif + +#if defined(_WIN32) || defined(_WIN64) || defined(__linux__) || defined(__APPLE__) +#include +#include +#else + +// We don't want 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 +using TMutex = std::mutex; +using TLock = std::unique_lock; + +#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( \ + 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() { return (default_verbosity); } \ +template<> constexpr log_level_t get_max_log_verbosity() { 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(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 +constexpr log_level_t get_default_log_verbosity() { return FIBRE_DEFAULT_LOG_VERBOSITY; } + +template +constexpr log_level_t get_max_log_verbosity() { return 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 +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(); + if (var_val) { + unsigned long num = strtoul(var_val, nullptr, 10); + log_level = (log_level_t)num; + } + + if (log_level > get_max_log_verbosity()) { + log_level = get_max_log_verbosity(); + } + return log_level; +} + + + +/* +template +void send_to_stream(TStream&& stream); + +template +void send_to_stream(TStream&& stream) { } + +template +void send_to_stream(TStream&& stream, T&& value, Ts&&... values) { + send_to_stream(std::forward(stream) << std::forward(value), std::forward(values)...); +}*/ + + +Logger* get_logger(); // defined in logging.cpp + +template +Logger::Entry make_log_entry(const char *filename, size_t line_no, const char *funcname) { + if (get_current_log_verbosity() < LEVEL) { + return {}; + } else { + Logger* logger = get_logger(); + return { std::cerr, LEVEL, TOPIC::get_label(), filename, line_no, funcname, logger->mutex_ }; + } +} + +template +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) +} + +} + +/** + * @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 {}; + +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 NullStream& operator<<(T val) { return *this; } +}; + +#define FIBRE_LOG(level) NullStream() + +#endif + +#endif // __FIBRE_LOGGING_HPP diff --git a/Firmware/fibre-cpp/package.lua b/Firmware/fibre-cpp/package.lua new file mode 100644 index 00000000..53101e1a --- /dev/null +++ b/Firmware/fibre-cpp/package.lua @@ -0,0 +1,8 @@ + +tup.include('../tupfiles/build.lua') + +fibre_package = define_package{ + sources={'protocol.cpp', 'posix_tcp.cpp', 'posix_udp.cpp'}, + libs={'pthread'}, + headers={'include'} +} diff --git a/Firmware/fibre-cpp/platform_support/libusb_transport.cpp b/Firmware/fibre-cpp/platform_support/libusb_transport.cpp new file mode 100644 index 00000000..4ed00dbc --- /dev/null +++ b/Firmware/fibre-cpp/platform_support/libusb_transport.cpp @@ -0,0 +1,660 @@ +/** + * @brief Transport provider: libusb + * + * Platform Compatibility: Linux, Windows, macOS + */ + +#include "libusb_transport.hpp" +#include "../logging.hpp" +#include "../print_utils.hpp" + +#include + +using namespace fibre; + +DEFINE_LOG_TOPIC(USB); +USE_LOG_TOPIC(USB); + +constexpr unsigned int kBulkTimeoutMs = 2000; + + +/* 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. + */ +int LibusbDiscoverer::init(EventLoop* event_loop) { + if (!event_loop) + return -1; + event_loop_ = event_loop; + + if (libusb_init(&libusb_ctx_) != LIBUSB_SUCCESS) { + FIBRE_LOG(E) << "libusb_init() failed: " << sys_err(); + return deinit(0), -1; + } + + // 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), -1; + } + + 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), -1; + } + + } 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 0; +} + +int 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 0; +} + +bool try_parse_key(const char* begin, const char* end, const char* key, int* val) { + char buf[end - begin + 1]; + memcpy(buf, begin, end - begin); + buf[end - begin] = 0; + + char fmt1[strlen(key) + 6]; + memcpy(fmt1, key, strlen(key)); + memcpy(fmt1 + strlen(key), "=0x%x", 6); + + char fmt2[strlen(key) + 4]; + memcpy(fmt2, key, strlen(key)); + memcpy(fmt2 + strlen(key), "=%d", 4); + + return sscanf(buf, fmt1, val) == 1 + || sscanf(buf, fmt2, val) == 1; +} + +/** + * @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: Specifies the constraints to consider. Must be either empty or + * of the format "key1=val1,key2=val2" where the available keys are: + * + * bus, address, idProduct, idVendor, bInterfaceClass, + * bInterfaceSubClass, bInterfaceProtocol + * + * The value can be either a integer in decimal or hexadecimal notation + * (0x1234). + * Omitted keys are ignored during filtering. + * + * @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(const char* specs, size_t specs_len, ChannelDiscoveryContext** handle, Completer& on_found_channels) { + FIBRE_LOG(D) << "starting discovery with filter \"" << std::string(specs, specs_len) << "\""; + + const char* prev_delim = specs; + + InterfaceSpecs interface_specs; + + while (prev_delim < specs + specs_len) { + const char* next_delim = std::find(prev_delim, specs + specs_len, ','); + + bool success = try_parse_key(prev_delim, next_delim, "bus", &interface_specs.bus) + || try_parse_key(prev_delim, next_delim, "address", &interface_specs.address) + || try_parse_key(prev_delim, next_delim, "idVendor", &interface_specs.vendor_id) + || try_parse_key(prev_delim, next_delim, "idProduct", &interface_specs.product_id) + || try_parse_key(prev_delim, next_delim, "bInterfaceClass", &interface_specs.interface_class) + || try_parse_key(prev_delim, next_delim, "bInterfaceSubClass", &interface_specs.interface_subclass) + || try_parse_key(prev_delim, next_delim, "bInterfaceProtocol", &interface_specs.interface_protocol); + + if (!success) { + FIBRE_LOG(E) << "could not interpret channel discovery specs"; + on_found_channels.complete({kFibreInvalidArgument, nullptr, nullptr}); + return; + } + + prev_delim = std::min(next_delim + 1, specs + specs_len); + } + + ChannelDiscoveryContext* subscription = new ChannelDiscoveryContext{interface_specs, &on_found_channels}; + 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, [](void* ctx) { + ((LibusbDiscoverer*)ctx)->on_event_loop_iteration(); + }, this); + } +} + +/** + * @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, [](void* ctx) { + ((LibusbDiscoverer*)ctx)->on_event_loop_iteration(); + }, this); +} + +/** + * @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; + + for (auto& subscription: subscriptions_) { + consider_device(dev, subscription); + } + + // add empty placeholder to the list of known devices + known_devices_[bus_number << 8 | dev_number] = { + .dev = libusb_ref_device(dev), + .handle = nullptr + }; + + } 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 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); + } + } + + // Call on_hotplug for all lost devices + + std::vector 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(1.0, [](void* ctx) { + ((LibusbDiscoverer*)ctx)->poll_devices_now(); + }, this); + } +} + +void LibusbDiscoverer::consider_device(struct libusb_device *device, ChannelDiscoveryContext* 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; + } + } + + static libusb_device_handle *handle = nullptr; + 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; + } + + EventLoop* event_loop = using_sparate_libusb_thread_ ? event_loop_ : nullptr; + + LibusbBulkInEndpoint* ep_in = new LibusbBulkInEndpoint(); + if (libusb_ep_in && ep_in->init(event_loop, my_dev.handle, libusb_ep_in->bEndpointAddress)) { + my_dev.ep_in.push_back(ep_in); + } else { + delete ep_in; + ep_in = nullptr; + } + + LibusbBulkOutEndpoint* ep_out = new LibusbBulkOutEndpoint(); + if (libusb_ep_out && ep_out->init(event_loop, my_dev.handle, libusb_ep_out->bEndpointAddress)) { + my_dev.ep_out.push_back(ep_out); + } else { + delete ep_out; + ep_out = nullptr; + } + + if (subscription->on_found_channels) { + subscription->on_found_channels->complete({kFibreOk, ep_in, ep_out}); + } + } + } + + libusb_free_config_descriptor(config_desc); + config_desc = nullptr; + } +} + + +/* LibusbBulkEndpoint --------------------------------------------------------*/ + +template +bool LibusbBulkEndpoint::init(EventLoop* event_loop, libusb_device_handle* handle, uint8_t endpoint_id) { + event_loop_ = event_loop; + handle_ = handle; + transfer_ = libusb_alloc_transfer(0); + endpoint_id_ = endpoint_id; + return true; +} + +template +bool LibusbBulkEndpoint::deinit() { + if (completer_) { + FIBRE_LOG(E) << "Transfer still in progress. This is gonna be messy."; + } + + libusb_free_transfer(transfer_); + transfer_ = nullptr; + return true; +} + +template +void LibusbBulkEndpoint::start_transfer(bufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(this); + } + + if (completer_) { + FIBRE_LOG(E) << "transfer already in progress"; + completer.complete({kStreamError, nullptr}); + return; + } + + if (!handle_) { + FIBRE_LOG(E) << "device not open"; + completer.complete({kStreamError, nullptr}); + return; + } + + auto direct_callback = [](struct libusb_transfer* transfer){ + ((LibusbBulkEndpoint*)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){ + ((LibusbBulkEndpoint*)transfer->user_data)->event_loop_->post( + [](void* ctx) { + ((LibusbBulkEndpoint*)ctx)->on_transfer_finished(); + }, transfer->user_data + ); + }; + + //FIBRE_LOG(D) << "transfer of size " << buffer.size(); + libusb_fill_bulk_transfer(transfer_, handle_, endpoint_id_, + buffer.begin(), buffer.size(), + event_loop_ ? indirect_callback : direct_callback, + this, kBulkTimeoutMs); + + completer_ = &completer; + submit_transfer(); +} + +template +void LibusbBulkEndpoint::cancel_transfer(TransferHandle transfer_handle) { + if (!completer_) { + FIBRE_LOG(E) << "transfer not in progress"; + return; + } + + libusb_cancel_transfer(transfer_); +} + +template +void LibusbBulkEndpoint::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); + safe_complete(completer_, {kStreamClosed, nullptr}); + } else { + FIBRE_LOG(W) << "couldn't start USB transfer on EP " << as_hex(endpoint_id_) << ": " << libusb_error_name(result); + safe_complete(completer_, {kStreamError, nullptr}); + } +} + +template +void LibusbBulkEndpoint::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; + } + + // On linux we get LIBUSB_TRANSFER_STALL on the RX pipe when the cable is plugged out + StreamStatus status = LIBUSB_TRANSFER_COMPLETED == transfer_->status ? kStreamOk : + LIBUSB_TRANSFER_CANCELLED == transfer_->status ? kStreamCancelled : + LIBUSB_TRANSFER_STALL == transfer_->status ? kStreamClosed : + LIBUSB_TRANSFER_NO_DEVICE == transfer_->status ? kStreamClosed : + kStreamError; + + (status == kStreamError ? FIBRE_LOG(W) : FIBRE_LOG(T)) + << "USB transfer on EP " << as_hex(endpoint_id_) << " finished with " << libusb_error_name(transfer_->status); + + uint8_t* end = std::max(transfer_->buffer + transfer_->actual_length, transfer_->buffer); + + safe_complete(completer_, {status, end}); +} diff --git a/Firmware/fibre-cpp/platform_support/libusb_transport.hpp b/Firmware/fibre-cpp/platform_support/libusb_transport.hpp new file mode 100644 index 00000000..5d383691 --- /dev/null +++ b/Firmware/fibre-cpp/platform_support/libusb_transport.hpp @@ -0,0 +1,124 @@ +#ifndef __FIBRE_USB_DISCOVERER_HPP +#define __FIBRE_USB_DISCOVERER_HPP + +#include "../event_loop.hpp" +#include "../async_stream.hpp" +#include + +#include +#include +#include +#include + +namespace fibre { + +class LibusbBulkInEndpoint; +class LibusbBulkOutEndpoint; + +struct ChannelDiscoveryResult { + FibreStatus status; + AsyncStreamSource* rx_channel; + AsyncStreamSink* tx_channel; +}; + +class FIBRE_PRIVATE LibusbDiscoverer { +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 ChannelDiscoveryContext { + InterfaceSpecs interface_specs; + Completer* on_found_channels; + }; + + int init(EventLoop* event_loop); + int deinit() { return deinit(INT_MAX); } + void start_channel_discovery(const char* specs, size_t specs_len, ChannelDiscoveryContext** handle, Completer& on_found_channels); + int stop_channel_discovery(ChannelDiscoveryContext* handle); + +private: + struct Device { + struct libusb_device* dev; + struct libusb_device_handle* handle; + std::vector ep_in; + std::vector ep_out; + }; + + int deinit(int stage); + void internal_event_loop(); + void 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, ChannelDiscoveryContext* 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 known_devices_; // key: bus_number << 8 | dev_number + std::vector subscriptions_; +}; + +template +class FIBRE_PRIVATE LibusbBulkEndpoint { +public: + bool init(EventLoop* event_loop, struct libusb_device_handle* handle, uint8_t endpoint_id); + bool deinit(); + +protected: + void start_transfer(bufptr_t buffer, TransferHandle* handle, Completer& completer); + void cancel_transfer(TransferHandle transfer_handle); + +private: + void submit_transfer(); + void on_transfer_finished(); + + EventLoop* event_loop_ = nullptr; // only non-null on Windows where we use a separate libusb thread + struct libusb_device_handle* handle_ = nullptr; + uint8_t endpoint_id_ = 0; + struct libusb_transfer* transfer_ = nullptr; + Completer* completer_ = nullptr; +}; + +class FIBRE_PRIVATE LibusbBulkInEndpoint : public LibusbBulkEndpoint, public AsyncStreamSource { +public: + void start_read(bufptr_t buffer, TransferHandle* handle, Completer& completer) final { + start_transfer(buffer, handle, completer); + } + + void cancel_read(TransferHandle transfer_handle) final { + cancel_transfer(transfer_handle); + } +}; + +class FIBRE_PRIVATE LibusbBulkOutEndpoint : public LibusbBulkEndpoint, public AsyncStreamSink { +public: + void start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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 \ No newline at end of file diff --git a/Firmware/fibre-cpp/posix_tcp.cpp b/Firmware/fibre-cpp/posix_tcp.cpp new file mode 100644 index 00000000..57ce0df9 --- /dev/null +++ b/Firmware/fibre-cpp/posix_tcp.cpp @@ -0,0 +1,108 @@ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +#define TCP_RX_BUF_LEN 512 + +class TCPStreamSink : public StreamSink { +public: + TCPStreamSink(int socket_fd) : + socket_fd_(socket_fd) + {} + + int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) { + int bytes_sent = send(socket_fd_, buffer, length, 0); + if (processed_bytes) + *processed_bytes = (bytes_sent == -1) ? 0 : bytes_sent; + return (bytes_sent == -1) ? -1 : 0; + } + + size_t get_free_space() { return SIZE_MAX; } + +private: + int socket_fd_; +}; + + +int serve_client(int sock_fd) { + uint8_t buf[TCP_RX_BUF_LEN]; + + // initialize output stack for this client + TCPStreamSink tcp_packet_output(sock_fd); + StreamBasedPacketSink packet2stream(tcp_packet_output); + BidirectionalPacketBasedChannel channel(packet2stream); + + StreamToPacketSegmenter stream2packet(channel); + + // now listen for it + for (;;) { + memset(buf, 0, sizeof(buf)); + // returns as soon as there is some data + ssize_t n_received = recv(sock_fd, buf, sizeof(buf), 0); + + // -1 indicates error and 0 means that the client gracefully terminated + if (n_received == -1 || n_received == 0) { + close(sock_fd); + return n_received; + } + + // input processing stack + size_t processed = 0; + stream2packet.process_bytes(buf, n_received, &processed); + } +} + +// function to check if a worker thread handling a single client is done +template +bool future_is_ready(std::future& t){ + return t.wait_for(std::chrono::seconds(0)) == std::future_status::ready; +} + +int serve_on_tcp(unsigned int port) { + struct sockaddr_in6 si_me, si_other; + int s; + + + if ((s=socket(AF_INET6, SOCK_STREAM, IPPROTO_TCP)) == -1) { + return -1; + } + + memset((char *) &si_me, 0, sizeof(si_me)); + si_me.sin6_family = AF_INET6; + si_me.sin6_port = htons(port); + si_me.sin6_flowinfo = 0; + si_me.sin6_addr = in6addr_any; + if (bind(s, reinterpret_cast(&si_me), sizeof(si_me)) == -1) { + return -1; + } + + listen(s, 128); // make this socket a passive socket + std::vector> serv_pool; + for (;;) { + memset(&si_other, 0, sizeof(si_other)); + + socklen_t silen = sizeof(si_other); + // TODO: Add a limit on accepting connections + int client_portal_fd = accept(s, reinterpret_cast(&si_other), &silen); // blocking call + serv_pool.push_back(std::async(std::launch::async, serve_client, client_portal_fd)); + // do a little clean up on the pool + for (std::vector>::iterator it = serv_pool.end()-1; it >= serv_pool.begin(); --it) { + if (future_is_ready(*it)) { + // we can erase this thread + serv_pool.erase(it); + } + } + } + + close(s); +} + diff --git a/Firmware/fibre-cpp/posix_udp.cpp b/Firmware/fibre-cpp/posix_udp.cpp new file mode 100644 index 00000000..0cc9d09c --- /dev/null +++ b/Firmware/fibre-cpp/posix_udp.cpp @@ -0,0 +1,70 @@ + +#include +#include +#include +#include +#include + +#include + +#define UDP_RX_BUF_LEN 512 +#define UDP_TX_BUF_LEN 512 + + +class UDPPacketSender : public PacketSink { +public: + UDPPacketSender(int socket_fd, struct sockaddr_in6 *si_other) : + _socket_fd(socket_fd), + _si_other(si_other) + {} + + size_t get_mtu() { return UDP_TX_BUF_LEN; } + + int process_packet(const uint8_t* buffer, size_t length) { + // cannot send partial packets + if (length > get_mtu()) + return -1; + + int status = sendto(_socket_fd, buffer, length, 0, reinterpret_cast(_si_other), sizeof(*_si_other)); + return (status == -1) ? -1 : 0; + } + +private: + int _socket_fd; + struct sockaddr_in6 *_si_other; +}; + + + +int serve_on_udp(unsigned int port) { + struct sockaddr_in6 si_me, si_other; + int s; + socklen_t slen = sizeof(si_other); + uint8_t buf[UDP_RX_BUF_LEN]; + + if ((s=socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP)) == -1) + return -1; + + memset((char *) &si_me, 0, sizeof(si_me)); + si_me.sin6_family = AF_INET6; + si_me.sin6_port = htons(port); + si_me.sin6_flowinfo = 0; + si_me.sin6_addr= in6addr_any; + if (bind(s, reinterpret_cast(&si_me), sizeof(si_me)) == -1) + return -1; + + for (;;) { + ssize_t n_received = recvfrom(s, buf, sizeof(buf), 0, reinterpret_cast(&si_other), &slen); + if (n_received == -1) + return -1; + //printf("Received packet from %s:%d\nData: %s\n\n", + // inet_ntoa(si_other.sin_addr), ntohs(si_other.sin_port), buf); + + UDPPacketSender udp_packet_output(s, &si_other); + BidirectionalPacketBasedChannel udp_channel(udp_packet_output); + udp_channel.process_packet(buf, n_received); + } + + close(s); +} + diff --git a/Firmware/fibre-cpp/print_utils.hpp b/Firmware/fibre-cpp/print_utils.hpp new file mode 100644 index 00000000..2457494e --- /dev/null +++ b/Firmware/fibre-cpp/print_utils.hpp @@ -0,0 +1,136 @@ +#ifndef __FIBRE_PRINT_UTILS_HPP +#define __FIBRE_PRINT_UTILS_HPP + +#include +#include + +namespace fibre { + +template +constexpr size_t hex_digits() { + return (std::numeric_limits::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 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 +bool hex_string_to_int(const char * str, size_t length, TInt* output) { + constexpr size_t N_DIGITS = hex_digits(); + 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 +bool hex_string_to_int(const char * str, TInt* output) { + return hex_string_to_int(str, hex_digits(), output); +} + +template +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(&str[i * hex_digits()], &output[i])) + return false; + } + return true; +} + +template +bool hex_string_to_int_arr(const char * str, TInt (&output)[ICount]) { + return hex_string_to_int_arr(str, hex_digits() * ICount, output); +} + +// TODO: move to print_utils.hpp +template +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()] = '\0'; + + for (size_t i = 0; i < hex_digits(); ++i) { + str[prefix_length + hex_digits() - 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() + 3]; // 3 additional characters 0x and \0 +}; + +template +std::ostream& operator<<(std::ostream& stream, const HexPrinter& printer) { + // TODO: specialize for char + return stream << printer.to_string(); +} + +template +HexPrinter as_hex(T val, bool prefix = true) { return HexPrinter(val, prefix); } + +template +class HexArrayPrinter { +public: + HexArrayPrinter(T* ptr, size_t length) : ptr_(ptr), length_(length) {} + T* ptr_; + size_t length_; +}; + +template +TStream& operator<<(TStream& stream, const HexArrayPrinter& 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 +HexArrayPrinter as_hex(T (&val)[ILength]) { return HexArrayPrinter(val, ILength); } + +template +HexArrayPrinter as_hex(generic_bufptr_t buffer) { return HexArrayPrinter(buffer.begin(), buffer.size()); } + +} + +#endif // __FIBRE_PRINT_UTILS_HPP \ No newline at end of file diff --git a/Firmware/fibre-cpp/stream_utils.hpp b/Firmware/fibre-cpp/stream_utils.hpp new file mode 100644 index 00000000..fc865442 --- /dev/null +++ b/Firmware/fibre-cpp/stream_utils.hpp @@ -0,0 +1,177 @@ +#ifndef __FIBRE_STREAM_UTILS_HPP +#define __FIBRE_STREAM_UTILS_HPP + +#include "async_stream.hpp" +#include + +namespace fibre { + +template +class BufferedStreamSink : Completer { +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) { + // 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". + + size_t read_idx = (read_idx_ + I - 1) % I; // read_idx_ could change during this function + + 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_, *this); + } else if (read_idx_ > write_idx_) { + is_active_ = true; + sink_.start_write({buffer_ + read_idx_, buffer_ + I}, &transfer_handle_, *this); + } else { + // nothing to do + } + } + +private: + void complete(WriteResult result) final { + 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 +class AsyncStreamSinkMultiplexer : public AsyncStreamSink, Completer { +public: + AsyncStreamSinkMultiplexer(AsyncStreamSink& sink) : sink_(sink) {} + + void start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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_, *this); + } + + return; + } + } + + if (handle) { + *handle = 0; + } + completer.complete({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; + safe_complete(completer, {kStreamCancelled, end}); + } + } + +private: + void complete(fibre::WriteResult result) final { + transfer_handle_ = 0; + + auto& [slot_in_use, slot_buf, slot_completer] = slots_[active_slot_ - 1]; + auto completer = slot_completer; + slot_in_use = false; + + safe_complete(completer, 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]; + 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]; + sink_.start_write(slot_buf, &transfer_handle_, *this); + } + } + + AsyncStreamSink& sink_; + std::tuple*> slots_[NSlots]; + size_t active_slot_ = 0; + TransferHandle transfer_handle_ = 0; +}; + +} + +#endif // __FIBRE_STREAM_UTILS_HPP diff --git a/Firmware/fibre-cpp/type_info_template.j2 b/Firmware/fibre-cpp/type_info_template.j2 new file mode 100644 index 00000000..7e6cda57 --- /dev/null +++ b/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 + +#pragma GCC push_options +#pragma GCC optimize ("s") + +[% for intf in interfaces.values() %][% if not intf.builtin %] +template +struct [[intf.fullname | to_pascal_case]]TypeInfo : TypeInfo { + using TypeInfo::TypeInfo; + static const PropertyInfo property_table[]; + static const [[intf.fullname | to_pascal_case]]TypeInfo 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.attributes.values() %] + case [[loop.index0]]: *(decltype([[intf.c_name]]::get_[[property.name]](std::declval()))*)(&res) = [[intf.c_name]]::get_[[property.name]](ptr); break; +[%- endfor %] + } + return res; + } +}; +[% endif %][% endfor %] + +[% for intf in interfaces.values() %][% if not intf.builtin %] +template +const PropertyInfo [[intf.fullname | to_pascal_case]]TypeInfo::property_table[] = { +[%- for property in intf.attributes.values() %] + {"[[property.name]]", &[[(property.type.purename or property.type.fullname) | to_pascal_case]]TypeInfo()))>>::singleton}, +[%- endfor %] +}; +template +const [[intf.fullname | to_pascal_case]]TypeInfo [[intf.fullname | to_pascal_case]]TypeInfo::singleton{[[intf.fullname | to_pascal_case]]TypeInfo::property_table, sizeof([[intf.fullname | to_pascal_case]]TypeInfo::property_table) / sizeof([[intf.fullname | to_pascal_case]]TypeInfo::property_table[0])}; + +[% endif %][% endfor %] + +#pragma GCC pop_options