├── .github └── workflows │ └── unit_tests.yml ├── .gitignore ├── LICENSE ├── README.md ├── console ├── README.md ├── include │ └── anchor │ │ └── console │ │ ├── console.h │ │ ├── console_config.h │ │ └── console_private_helpers.h ├── src │ └── console.c └── tests │ ├── Makefile │ ├── main.cpp │ ├── test_console_help.cpp │ └── test_console_no_help.cpp ├── fsm ├── README.md ├── include │ └── anchor │ │ └── fsm │ │ └── fsm.h └── src │ └── fsm.c ├── logging ├── README.md ├── include │ └── anchor │ │ └── logging │ │ ├── logging.h │ │ └── logging_internal.h └── src │ └── logging.c └── sonar ├── ProtocolSpecification.md ├── README.md ├── include └── anchor │ └── sonar │ ├── attribute.h │ ├── client.h │ ├── error_types.h │ ├── proto_helpers.h │ └── server.h ├── protobuf ├── sonar_extensions.proto ├── sonar_nanopb.mk └── sonar_proto_gen.py ├── sonar.mk ├── src ├── application_layer │ ├── application_layer.c │ ├── application_layer.h │ └── types.h ├── attribute │ ├── attribute_client.c │ ├── attribute_server.c │ ├── client.h │ ├── control_helpers.h │ └── server.h ├── client.c ├── common │ ├── buffer_chain.c │ ├── buffer_chain.h │ ├── crc16.c │ └── crc16.h ├── link_layer │ ├── link_layer.c │ ├── link_layer.h │ ├── receive.c │ ├── receive.h │ ├── timeouts.h │ ├── transmit.c │ ├── transmit.h │ └── types.h └── server.c └── tests ├── Makefile ├── main.cpp ├── test_application_layer.cpp ├── test_attribute_client.cpp ├── test_attribute_server.cpp ├── test_buffer_chain.cpp ├── test_client.cpp ├── test_common.h ├── test_crc16.cpp ├── test_link_layer.cpp ├── test_link_layer_receive.cpp ├── test_link_layer_transmit.cpp └── test_server.cpp /.github/workflows/unit_tests.yml: -------------------------------------------------------------------------------- 1 | name: Unit Tests 2 | on: 3 | pull_request: 4 | branches: 5 | - master 6 | push: 7 | branches: 8 | - master 9 | jobs: 10 | build_and_test: 11 | runs-on: ubuntu-latest 12 | container: 13 | image: docker://srzzumix/googletest:latest 14 | steps: 15 | - uses: actions/checkout@master 16 | - name: Run Sonar Unit Tests 17 | working-directory: ./sonar/tests 18 | run: make 19 | - name: Run Console Unit Tests 20 | working-directory: ./console/tests 21 | run: make 22 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | **/build/ 3 | *.pyc 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2019-Present Skip Transport, Inc. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 14 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 15 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 16 | IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 17 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 18 | OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE 19 | OR OTHER DEALINGS IN THE SOFTWARE. 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Anchor 2 | 3 | Anchor is a collection of various embedded firmware libraries developed by Skip 4 | Transport, Inc. These libraries were all developed to be easily re-usable and 5 | not dependant on a specific MCU or RTOS. 6 | 7 | # Libraries 8 | 9 | This repository is organized as a collection of libraries which are largely 10 | independent of each other. You are free to pick and chose the libraries which 11 | are most useful in your application, without needing to include them all. Each 12 | library is organized within a subdirectory, with a README providing information 13 | specific to the library. 14 | 15 | In general, including a library is as simple as adding all the `.c` files 16 | within `/src` to your build system as C sources to be compiled, and adding 17 | `/include` to your build system's include path. 18 | 19 | ## Logging 20 | 21 | The logging library makes it easy to write out consistent and useful debug 22 | logs. This is often done over a UART connection, but the library is agnostic to 23 | the protocol being used. 24 | 25 | ``` 26 | 0:00:00.626 WARN system.c:199: Last reset due to software reset 27 | 0:00:00.632 INFO system.c:243: Serial number is 003b001b524b501020353548 28 | 0:00:00.639 INFO system.c:257: Detected DVT hardware 29 | 0:00:00.648 INFO piezo.c:45: is_present=1 30 | 0:00:16.732 ERROR SONAR:link_layer.c:161: Invalid packet: Response sequence number does not match request 31 | ``` 32 | 33 | ## FSM 34 | 35 | The FSM library is a very lightweight set of macros and functions for defining 36 | event-based finite state machines with enter/exit handlers for each state. See 37 | the README within the `fsm` subdirectory for more information and an example. 38 | 39 | ## Console 40 | 41 | The console library can be used to provide a debug interface to your device. 42 | The library is designed to be highly configurable to allow the user to decide 43 | between more features and code-space / runtime efficiencies. 44 | 45 | ``` 46 | > help 47 | Available commands: 48 | help - List all commands, or give details about a specific command 49 | gpio_set - Sets a GPIO pin 50 | gpio_get - Gets a GPIO pin value 51 | > help gpio_get 52 | Gets a GPIO pin value 53 | Usage: gpio_get port pin 54 | port - The port 55 | pin - The pin <0-15> 56 | > gpio_set A 1 1 57 | > gpio_get A 1 58 | value=1 59 | > gpio_set A 1 0 60 | > gpio_get A 1 61 | value=0 62 | ``` 63 | 64 | # Contributing 65 | 66 | If you find a bug, or have an idea for how we can improve this library, please 67 | let us know by creating an issue. Opening up a pull request is even better! 68 | 69 | # License 70 | 71 | These libraries are made available under the MIT license. See the LICENSE file 72 | for more information. 73 | -------------------------------------------------------------------------------- /console/README.md: -------------------------------------------------------------------------------- 1 | # Console 2 | 3 | A command-line console library which can be used to easily expose debug and 4 | test commands via a serial interface. 5 | 6 | ## Features 7 | 8 | The console library supports the following high-level features: 9 | * Built-in validation of command arguments 10 | * Auto-generated "help" command which lists all available commands, and can be 11 | used to display usage information about specific commands (when enabled) 12 | * Support for string and integer positional arguments, including (limited) 13 | optional arguments 14 | * Optimized for embedded environments with static memory allocation and the 15 | ability to store command definitions in program memory (instead of RAM) 16 | * Simple and easy to integrate API 17 | 18 | ## Usage 19 | 20 | The first step is to define a console command and implement its handler. During 21 | program initialization, `console_init()` should be called to initialize the 22 | console library, and each command should be registered with the console library 23 | using `console_command_register()`. 24 | 25 | When the program receives data which should be interpreted by the console 26 | library, it should be passed to `console_process()`. Note that the data passed 27 | may contain a partial, complete, or even multiple commands. As part of this 28 | call, the console library will call the command handlers, as appropriate. 29 | 30 | ### Defining Commands 31 | 32 | To define a new console command, the `CONSOLE_COMMAND_DEF(CMD, DESC, ...)` 33 | macro should be used (omit the `DESC` argument if the `CONSOLE_HELP_COMMAND` 34 | option is not enabled). 35 | * `CMD` is the name of the console command, and must be a valid C symbol name. 36 | * `DESC` should be specified if the `CONSOLE_HELP_COMMAND` option is set, and 37 | should be a string which describes the command for use by the `help` command. 38 | * `...` should consist of 0 or more argument definitions (see below). 39 | 40 | Arguments are defined as part of a console command definition with the help of 41 | one of the following macros (omit the `DESC` argument if the 42 | `CONSOLE_HELP_COMMAND` option is not enabled). The `NAME` argument must be a 43 | valid C symbol name. 44 | * `CONSOLE_INT_ARG_DEF(NAME, DESC)` - Defines a required integer argument. 45 | * `CONSOLE_STR_ARG_DEF(NAME, DESC)` - Defines a required string argument. 46 | * `CONSOLE_OPTIONAL_INT_ARG_DEF(NAME, DESC)` - Defines an optional integer 47 | argument (can only be used for the last argument). 48 | * `CONSOLE_OPTIONAL_STR_ARG_DEF(NAME, DESC)` - Defines an optional string 49 | argument (can only be used for the last argument). 50 | 51 | The `CONSOLE_COMMAND_DEF()` macro will forward declare a handler for the 52 | console command with one of the following prototypes depending on whether or not 53 | the command has arguments: 54 | * `static void _command_handler(const _args_t* args);` 55 | * `static void _command_handler(void);` 56 | 57 | This function will be called whenever the console command is invoked, with the 58 | `args` argument (if applicable) containing the validated, parsed, and 59 | appropriately typed (`intptr_t` / `const char*`) arguments. For optional 60 | arguments, a default value of `-1` / `NULL` will be set for integer / string 61 | arguments respectively. 62 | 63 | ### Initializing the Library and Registering Commands 64 | 65 | The `console_init()` function should be called before registering any commands 66 | to initialize the console library. Console commands can be registered at any 67 | time afterwards using the `console_command_register(CMD)` function. 68 | 69 | ## Compile Options 70 | 71 | Parameters and additional features of the console library can be configured via 72 | the following defines: 73 | * `CONSOLE_MAX_COMMANDS` - The maximum number of console commands which may be 74 | registered (default of 16). 75 | * `CONSOLE_MAX_LINE_LENGTH` - The maximum length of a single line sent to the 76 | console library (default of 128). 77 | * `CONSOLE_PROMPT` - The string displayed to prompt for console input (default 78 | of "> "). 79 | * `CONSOLE_RETURN_KEY` - Specifies the character sequence for the console 80 | return key (default is '\n'). 81 | * `CONSOLE_NEWLINE` - Defines the character sequence for console newline 82 | (default is '\n'). 83 | * `CONSOLE_BUFFER_ATTRIBUTES` - Extra GCC attributes to add to the internal 84 | buffers used by the console library (default of none). As an example, this can 85 | be used in cases where these buffers should be placed in a different linker 86 | section. 87 | * `CONSOLE_HELP_COMMAND` - Enables the built-in help command (default of 1). 88 | This can be disabled to save code space. Disabling this will also remove the 89 | `desc` attribute of commands and arguments. 90 | * `CONSOLE_FULL_CONTROL` - Enables more advanced line control features such as 91 | backspace, left and right arrow keys, and CTRL-C (default of 1). Also enables 92 | the `console_print_line()` function. This setting also results in the console 93 | echo'ing back any valid characters it receives. 94 | * `CONSOLE_TAB_COMPLETE` - Enables tab-completion of console commands (default 95 | of 1). This depends on `CONSOLE_FULL_CONTROL` being enabled. 96 | * `CONSOLE_HISTORY` - Enables history accessible via up/down arrow keys with 97 | the specified command buffer size (default is 0). This depends on 98 | `CONSOLE_FULL_CONTROL` being enabled. 99 | 100 | ## Tests 101 | 102 | The unit tests can be run by running `make` within the `tests` directory. 103 | These tests are built using 104 | [googletest](https://github.com/google/googletest) and depend on the 105 | `gtest` library being available on the system. 106 | 107 | ## Example 108 | 109 | ```c 110 | #include "anchor/console/console.h" 111 | 112 | CONSOLE_COMMAND_DEF(hello_world, "Prints a greeting"); 113 | 114 | CONSOLE_COMMAND_DEF(gpio_set, "Sets a GPIO pin", 115 | CONSOLE_STR_ARG_DEF(port, "The port "), 116 | CONSOLE_INT_ARG_DEF(pin, "The pin <0-15>"), 117 | CONSOLE_INT_ARG_DEF(value, "The value <0-1>") 118 | ); 119 | 120 | CONSOLE_COMMAND_DEF(gpio_get, "Gets a GPIO pin value", 121 | CONSOLE_STR_ARG_DEF(port, "The port "), 122 | CONSOLE_INT_ARG_DEF(pin, "The pin <0-15>") 123 | ); 124 | 125 | static void hello_world_command_handler(void) { 126 | printf("Hello World\n"); 127 | } 128 | 129 | static void gpio_set_command_handler(const gpio_set_args_t* args) { 130 | gpio_write(args->port, args->pin, args->value); 131 | } 132 | 133 | static void gpio_get_command_handler(const gpio_get_args_t* args) { 134 | printf("value=%d\n", gpio_read(args->port, args->pin)); 135 | } 136 | 137 | void main(void) { 138 | const console_init_t init_console = { 139 | .write_function = console_write_function, 140 | }; 141 | console_init(&init_console); 142 | console_command_register(gpio_set); 143 | console_command_register(gpio_get); 144 | 145 | while (true) { 146 | uint8_t buffer[64]; 147 | const uint32_t len = serial_read(buffer, sizeof(buffer)); 148 | console_process(buffer, len); 149 | // ... 150 | } 151 | } 152 | ``` 153 | 154 | ``` 155 | > help 156 | Available commands: 157 | help - List all commands, or give details about a specific command 158 | gpio_set - Sets a GPIO pin 159 | gpio_get - Gets a GPIO pin value 160 | > help gpio_get 161 | Gets a GPIO pin value 162 | Usage: gpio_get port pin 163 | port - The port 164 | pin - The pin <0-15> 165 | > gpio_set A 1 1 166 | > gpio_get A 1 167 | value=1 168 | > gpio_set A 1 0 169 | > gpio_get A 1 170 | value=0 171 | ``` 172 | -------------------------------------------------------------------------------- /console/include/anchor/console/console.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "anchor/console/console_config.h" 4 | #include "anchor/console/console_private_helpers.h" 5 | 6 | #include 7 | #include 8 | 9 | #define CONSOLE_INT_ARG_DEFAULT ((intptr_t)-1) 10 | #define CONSOLE_STR_ARG_DEFAULT ((const char*)NULL) 11 | 12 | // Generic command handler type which is used internally by the console library 13 | typedef void(*console_command_handler_t)(const void*); 14 | typedef void(*console_command_handler_no_args_t)(void); 15 | #if CONSOLE_TAB_COMPLETE 16 | // Handler used for tab-completion 17 | typedef const char*(*console_tab_complete_iterator_t)(bool); 18 | #endif 19 | 20 | typedef enum { 21 | // An integer argument (of type "intptr_t") 22 | CONSOLE_ARG_TYPE_INT, 23 | // A string argument 24 | CONSOLE_ARG_TYPE_STR, 25 | } console_arg_type_t; 26 | 27 | typedef struct { 28 | // The name of the argument 29 | const char* name; 30 | #if CONSOLE_HELP_COMMAND 31 | // A description of the argument for display by the "help" command 32 | const char* desc; 33 | #endif 34 | // The type of the argument 35 | console_arg_type_t type; 36 | // Whether or not the arg is optional (can ONLY be set for the last argument) 37 | bool is_optional; 38 | } console_arg_def_t; 39 | 40 | typedef struct { 41 | // The name of the command (must be a valid C symbol name, consist of only lower-case alphanumeric characters, and be unique) 42 | const char* name; 43 | #if CONSOLE_HELP_COMMAND 44 | // A description of the command for display by the "help" command 45 | const char* desc; 46 | #endif 47 | // The command handler 48 | union { 49 | console_command_handler_t handler; 50 | console_command_handler_no_args_t handler_no_args; 51 | }; 52 | #if CONSOLE_TAB_COMPLETE 53 | // The auto complete iterator 54 | console_tab_complete_iterator_t tab_complete_iter; 55 | #endif 56 | // List of argument definitions 57 | const console_arg_def_t* args; 58 | // The number of arguments 59 | uint32_t num_args; 60 | // A pointer which the console library uses internally to store arguments for this command 61 | void** args_ptr; 62 | } console_command_def_t; 63 | 64 | typedef struct { 65 | // Write function which gets passed a string to be written out 66 | void(*write_function)(const char* str); 67 | } console_init_t; 68 | 69 | // Defines a console command 70 | #if CONSOLE_HELP_COMMAND 71 | #define CONSOLE_COMMAND_DEF(CMD, DESC, ...) _CONSOLE_COMMAND_DEF(CMD, DESC, ##__VA_ARGS__) 72 | #if CONSOLE_TAB_COMPLETE 73 | #define CONSOLE_COMMAND_DEF_WITH_TAB_COMPLETION(CMD, DESC, ...) _CONSOLE_COMMAND_DEF_WITH_TAB_COMPLETION(CMD, DESC, ##__VA_ARGS__) 74 | #endif 75 | #else 76 | #define CONSOLE_COMMAND_DEF(CMD, ...) _CONSOLE_COMMAND_DEF(CMD, NULL, ##__VA_ARGS__) 77 | #if CONSOLE_TAB_COMPLETE 78 | #define CONSOLE_COMMAND_DEF_WITH_TAB_COMPLETION(CMD, ...) _CONSOLE_COMMAND_DEF_WITH_TAB_COMPLETION(CMD, NULL, ##__VA_ARGS__) 79 | #endif 80 | #endif 81 | 82 | // Defines an integer argument of a console command 83 | #if CONSOLE_HELP_COMMAND 84 | #define CONSOLE_INT_ARG_DEF(NAME, DESC) (NAME, CONSOLE_ARG_TYPE_INT, false, intptr_t, DESC) 85 | #else 86 | #define CONSOLE_INT_ARG_DEF(NAME) (NAME, CONSOLE_ARG_TYPE_INT, false, intptr_t) 87 | #endif 88 | 89 | // Defines a string argument of a console command 90 | #if CONSOLE_HELP_COMMAND 91 | #define CONSOLE_STR_ARG_DEF(NAME, DESC) (NAME, CONSOLE_ARG_TYPE_STR, false, const char*, DESC) 92 | #else 93 | #define CONSOLE_STR_ARG_DEF(NAME) (NAME, CONSOLE_ARG_TYPE_STR, false, const char*) 94 | #endif 95 | 96 | // Defines an optional integer argument of a console command (can ONLY be used for the last argument) 97 | // The argument will have a value of `CONSOLE_INT_ARG_DEFAULT` when not specified 98 | #if CONSOLE_HELP_COMMAND 99 | #define CONSOLE_OPTIONAL_INT_ARG_DEF(NAME, DESC) (NAME, CONSOLE_ARG_TYPE_INT, true, intptr_t, DESC) 100 | #else 101 | #define CONSOLE_OPTIONAL_INT_ARG_DEF(NAME) (NAME, CONSOLE_ARG_TYPE_INT, true, intptr_t) 102 | #endif 103 | 104 | // Defines an optional string argument of a console command (can ONLY be used for the last argument) 105 | // The argument will have a value of `CONSOLE_STR_ARG_DEFAULT` when not specified 106 | #if CONSOLE_HELP_COMMAND 107 | #define CONSOLE_OPTIONAL_STR_ARG_DEF(NAME, DESC) (NAME, CONSOLE_ARG_TYPE_STR, true, const char*, DESC) 108 | #else 109 | #define CONSOLE_OPTIONAL_STR_ARG_DEF(NAME) (NAME, CONSOLE_ARG_TYPE_STR, true, const char*) 110 | #endif 111 | 112 | // Initializes the console library 113 | void console_init(const console_init_t* init); 114 | 115 | // Deinitializes the console library 116 | void console_deinit(void); 117 | 118 | // Registers a console command with the console library (returns true on success) 119 | bool console_command_register(const console_command_def_t* cmd); 120 | 121 | // Processes received data 122 | void console_process(const uint8_t* data, uint32_t length); 123 | 124 | #if CONSOLE_FULL_CONTROL 125 | // Prints a string (should end with a '\n') without visibly corrupting the current command line 126 | void console_print_line(const char* str); 127 | #endif 128 | -------------------------------------------------------------------------------- /console/include/anchor/console/console_config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // See the README for documentation on these options. 4 | 5 | #ifndef CONSOLE_MAX_COMMANDS 6 | #define CONSOLE_MAX_COMMANDS 16 7 | #endif 8 | 9 | #ifndef CONSOLE_MAX_LINE_LENGTH 10 | #define CONSOLE_MAX_LINE_LENGTH 128 11 | #endif 12 | 13 | #ifndef CONSOLE_PROMPT 14 | #define CONSOLE_PROMPT "> " 15 | #endif 16 | 17 | #ifndef CONSOLE_RETURN_KEY 18 | #define CONSOLE_RETURN_KEY '\n' 19 | #endif 20 | 21 | #ifndef CONSOLE_NEWLINE 22 | #define CONSOLE_NEWLINE "\n" 23 | #endif 24 | 25 | #ifndef CONSOLE_BUFFER_ATTRIBUTES 26 | #define CONSOLE_BUFFER_ATTRIBUTES 27 | #endif 28 | 29 | #ifndef CONSOLE_HELP_COMMAND 30 | #define CONSOLE_HELP_COMMAND 1 31 | #endif 32 | 33 | #ifndef CONSOLE_FULL_CONTROL 34 | #define CONSOLE_FULL_CONTROL 1 35 | #endif 36 | 37 | #ifndef CONSOLE_TAB_COMPLETE 38 | #define CONSOLE_TAB_COMPLETE 1 39 | #endif 40 | 41 | #if CONSOLE_TAB_COMPLETE && !CONSOLE_FULL_CONTROL 42 | #error "CONSOLE_TAB_COMPLETE requires CONSOLE_FULL_CONTROL to be enabled" 43 | #endif 44 | 45 | #ifndef CONSOLE_HISTORY 46 | #define CONSOLE_HISTORY 0 47 | #endif 48 | 49 | #if CONSOLE_HISTORY && !CONSOLE_FULL_CONTROL 50 | #error "CONSOLE_HISTORY requires CONSOLE_FULL_CONTROL to be enabled" 51 | #endif 52 | -------------------------------------------------------------------------------- /console/include/anchor/console/console_private_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "anchor/console/console_config.h" 4 | 5 | #if !defined(__GNUC__) && !defined(__TI_COMPILER_VERSION__) 6 | #error "Unsupported toolchain" 7 | #endif 8 | 9 | // The _CONSOLE_NUM_ARGS(...) macro returns the number of arguments passed to it (0-10) 10 | #define _CONSOLE_NUM_ARGS(...) _CONSOLE_NUM_ARGS_HELPER1(_, ##__VA_ARGS__, _CONSOLE_NUM_ARGS_SEQ()) 11 | #define _CONSOLE_NUM_ARGS_HELPER1(...) _CONSOLE_NUM_ARGS_HELPER2(__VA_ARGS__) 12 | #define _CONSOLE_NUM_ARGS_HELPER2(_1,_2,_3,_4,_5,_6,_7,_8,_9,_10,N,...) N 13 | #define _CONSOLE_NUM_ARGS_SEQ() 9,8,7,6,5,4,3,2,1,0 14 | _Static_assert(_CONSOLE_NUM_ARGS() == 0, "_CONSOLE_NUM_ARGS() != 0"); 15 | _Static_assert(_CONSOLE_NUM_ARGS(1) == 1, "_CONSOLE_NUM_ARGS(1) != 1"); 16 | _Static_assert(_CONSOLE_NUM_ARGS(1,2,3,4,5,6,7,8,9,10) == 10, "_CONSOLE_NUM_ARGS(<10_args>) != 10"); 17 | 18 | // General helper macro for concatenating two tokens 19 | #define _CONSOLE_CONCAT(A, B) _CONSOLE_CONCAT2(A, B) 20 | #define _CONSOLE_CONCAT2(A, B) A ## B 21 | 22 | // Helper macro for calling a given macro for each of 0-10 arguments 23 | #define _CONSOLE_MAP(X, ...) _CONSOLE_MAP_HELPER(_CONSOLE_CONCAT(_CONSOLE_MAP_, _CONSOLE_NUM_ARGS(__VA_ARGS__)), X, ##__VA_ARGS__) 24 | #define _CONSOLE_MAP_HELPER(C, X, ...) C(X, ##__VA_ARGS__) 25 | #define _CONSOLE_MAP_0(X) 26 | #define _CONSOLE_MAP_1(X,_1) X(_1) 27 | #define _CONSOLE_MAP_2(X,_1,_2) X(_1) X(_2) 28 | #define _CONSOLE_MAP_3(X,_1,_2,_3) X(_1) X(_2) X(_3) 29 | #define _CONSOLE_MAP_4(X,_1,_2,_3,_4) X(_1) X(_2) X(_3) X(_4) 30 | #define _CONSOLE_MAP_5(X,_1,_2,_3,_4,_5) X(_1) X(_2) X(_3) X(_4) X(_5) 31 | #define _CONSOLE_MAP_6(X,_1,_2,_3,_4,_5,_6) X(_1) X(_2) X(_3) X(_4) X(_5) X(_6) 32 | #define _CONSOLE_MAP_7(X,_1,_2,_3,_4,_5,_6,_7) X(_1) X(_2) X(_3) X(_4) X(_5) X(_6) X(_7) 33 | #define _CONSOLE_MAP_8(X,_1,_2,_3,_4,_5,_6,_7,_8) X(_1) X(_2) X(_3) X(_4) X(_5) X(_6) X(_7) X(_8) 34 | #define _CONSOLE_MAP_9(X,_1,_2,_3,_4,_5,_6,_7,_8,_9) X(_1) X(_2) X(_3) X(_4) X(_5) X(_6) X(_7) X(_8) X(_9) 35 | #define _CONSOLE_MAP_10(X,_1,_2,_3,_4,_5,_6,_7,_8,_9,_10) X(_1) X(_2) X(_3) X(_4) X(_5) X(_6) X(_7) X(_8) X(_9) X(_10) 36 | 37 | // Helper macros for defining things differently if the command has args or not 38 | #define _CONSOLE_IF_ARGS(VAL, ...) _CONSOLE_IF_ELSE_NO_ARGS(, VAL, ##__VA_ARGS__) 39 | #define _CONSOLE_IF_NO_ARGS(VAL, ...) _CONSOLE_IF_ELSE_NO_ARGS(VAL,, ##__VA_ARGS__) 40 | #define _CONSOLE_IF_ELSE_NO_ARGS(TRUE, FALSE, ...) _CONSOLE_IF_ELSE_NO_ARGS_HELPER(_CONSOLE_CONCAT(_CONSOLE_IF_ELSE_NO_ARGS_, _CONSOLE_NUM_ARGS(__VA_ARGS__)), TRUE, FALSE, ##__VA_ARGS__) 41 | #define _CONSOLE_IF_ELSE_NO_ARGS_HELPER(C, TRUE, FALSE, ...) C(TRUE, FALSE, ##__VA_ARGS__) 42 | #define _CONSOLE_IF_ELSE_NO_ARGS_0(TRUE, FALSE) TRUE 43 | #define _CONSOLE_IF_ELSE_NO_ARGS_1(TRUE, FALSE, ...) FALSE 44 | #define _CONSOLE_IF_ELSE_NO_ARGS_2(TRUE, FALSE, ...) FALSE 45 | #define _CONSOLE_IF_ELSE_NO_ARGS_3(TRUE, FALSE, ...) FALSE 46 | #define _CONSOLE_IF_ELSE_NO_ARGS_4(TRUE, FALSE, ...) FALSE 47 | #define _CONSOLE_IF_ELSE_NO_ARGS_5(TRUE, FALSE, ...) FALSE 48 | #define _CONSOLE_IF_ELSE_NO_ARGS_6(TRUE, FALSE, ...) FALSE 49 | #define _CONSOLE_IF_ELSE_NO_ARGS_7(TRUE, FALSE, ...) FALSE 50 | #define _CONSOLE_IF_ELSE_NO_ARGS_8(TRUE, FALSE, ...) FALSE 51 | #define _CONSOLE_IF_ELSE_NO_ARGS_9(TRUE, FALSE, ...) FALSE 52 | #define _CONSOLE_IF_ELSE_NO_ARGS_10(TRUE, FALSE, ...) FALSE 53 | 54 | // Helper macros for defining console commands and their arguments 55 | #define _CONSOLE_COMMAND_DEF(CMD, DESC, ...) \ 56 | _CONSOLE_COMMAND_ARGS_AND_HANDLER(CMD, ##__VA_ARGS__) \ 57 | _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_DEFAULT_DEF(CMD) \ 58 | static const console_command_def_t _##CMD##_DEF = { \ 59 | .name = #CMD, \ 60 | _CONSOLE_COMMAND_DEF_DESC_FIELD(DESC) \ 61 | _CONSOLE_IF_ELSE_NO_ARGS( \ 62 | .handler_no_args = (console_command_handler_no_args_t)CMD##_command_handler, \ 63 | .handler = (console_command_handler_t)CMD##_command_handler, \ 64 | ##__VA_ARGS__ \ 65 | ), \ 66 | _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_FIELD(CMD) \ 67 | .args = _##CMD##_ARGS_DEF, \ 68 | .num_args = sizeof(_##CMD##_ARGS_DEF) / sizeof(console_arg_def_t), \ 69 | .args_ptr = _##CMD##_ARGS, \ 70 | }; \ 71 | static const console_command_def_t* const CMD = &_##CMD##_DEF 72 | #define _CONSOLE_COMMAND_DEF_WITH_TAB_COMPLETION(CMD, DESC, ...) \ 73 | _CONSOLE_COMMAND_ARGS_AND_HANDLER(CMD, ##__VA_ARGS__) \ 74 | _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_PROTOTYPE(CMD) \ 75 | static const console_command_def_t _##CMD##_DEF = { \ 76 | .name = #CMD, \ 77 | _CONSOLE_COMMAND_DEF_DESC_FIELD(DESC) \ 78 | _CONSOLE_IF_ELSE_NO_ARGS( \ 79 | .handler_no_args = (console_command_handler_no_args_t)CMD##_command_handler, \ 80 | .handler = (console_command_handler_t)CMD##_command_handler, \ 81 | ##__VA_ARGS__ \ 82 | ), \ 83 | _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_FIELD(CMD) \ 84 | .args = _##CMD##_ARGS_DEF, \ 85 | .num_args = sizeof(_##CMD##_ARGS_DEF) / sizeof(console_arg_def_t), \ 86 | .args_ptr = _##CMD##_ARGS, \ 87 | }; \ 88 | static const console_command_def_t* const CMD = &_##CMD##_DEF 89 | #if CONSOLE_HELP_COMMAND 90 | #define _CONSOLE_COMMAND_DEF_DESC_FIELD(DESC) .desc = DESC, 91 | #else 92 | #define _CONSOLE_COMMAND_DEF_DESC_FIELD(DESC) 93 | #endif 94 | #if CONSOLE_TAB_COMPLETE 95 | #define _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_DEFAULT_DEF(CMD) \ 96 | static const console_tab_complete_iterator_t CMD##_tab_complete_iterator = NULL; 97 | #define _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_PROTOTYPE(CMD) \ 98 | static const char* CMD##_tab_complete_iterator(bool start); 99 | #define _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_FIELD(CMD) .tab_complete_iter = CMD##_tab_complete_iterator, 100 | #else 101 | #define _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_DEFAULT_DEF(CMD) 102 | #define _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_PROTOTYPE(CMD) 103 | #define _CONSOLE_COMMAND_DEF_TAB_COMPLETE_ITER_FIELD(CMD) 104 | #endif 105 | #define _CONSOLE_COMMAND_ARGS_AND_HANDLER(CMD, ...) \ 106 | _CONSOLE_IF_ARGS( \ 107 | typedef struct { \ 108 | _CONSOLE_MAP(_CONSOLE_ARG_TYPE_HELPER, ##__VA_ARGS__) \ 109 | } CMD##_args_t; \ 110 | _Static_assert(sizeof(CMD##_args_t) == _CONSOLE_NUM_ARGS(__VA_ARGS__) * sizeof(void *), "Compiler created *_args_t struct of unexpected size");, \ 111 | ##__VA_ARGS__\ 112 | ) \ 113 | static const console_arg_def_t _##CMD##_ARGS_DEF[] = { \ 114 | _CONSOLE_MAP(_CONSOLE_ARG_DEF_HELPER, ##__VA_ARGS__) \ 115 | }; \ 116 | _Static_assert(sizeof(_##CMD##_ARGS_DEF) / sizeof(console_arg_def_t) == _CONSOLE_NUM_ARGS(__VA_ARGS__), "Internal error in code generation"); \ 117 | static void* _##CMD##_ARGS[_CONSOLE_NUM_ARGS(__VA_ARGS__)]; \ 118 | static void CMD##_command_handler(_CONSOLE_IF_ELSE_NO_ARGS(void, const CMD##_args_t* args, ##__VA_ARGS__)); 119 | #define _CONSOLE_ARG_DEF_HELPER(...) _CONSOLE_ARG_DEF_HELPER2 __VA_ARGS__ 120 | #if CONSOLE_HELP_COMMAND 121 | #define _CONSOLE_ARG_DEF_HELPER2(NAME, ENUM_TYPE, IS_OPTIONAL, C_TYPE, DESC) \ 122 | { .name = #NAME, .desc = DESC, .type = ENUM_TYPE, .is_optional = IS_OPTIONAL }, 123 | #else 124 | #define _CONSOLE_ARG_DEF_HELPER2(NAME, ENUM_TYPE, IS_OPTIONAL, C_TYPE) \ 125 | { .name = #NAME, .type = ENUM_TYPE, .is_optional = IS_OPTIONAL }, 126 | #endif 127 | #define _CONSOLE_ARG_TYPE_HELPER(...) _CONSOLE_ARG_TYPE_HELPER2 __VA_ARGS__ 128 | #define _CONSOLE_ARG_TYPE_HELPER2(NAME, ENUM_TYPE, IS_OPTIONAL, C_TYPE, ...) \ 129 | C_TYPE NAME; 130 | -------------------------------------------------------------------------------- /console/tests/Makefile: -------------------------------------------------------------------------------- 1 | TARGET := test 2 | BUILD_DIR_COMMON := build/ 3 | BUILD_DIR_HELP := $(BUILD_DIR_COMMON)/help 4 | BUILD_DIR_NO_HELP := $(BUILD_DIR_COMMON)/no_help 5 | 6 | C_SOURCES := \ 7 | ../src/console.c 8 | 9 | C_DEFS := 10 | 11 | CXX_SOURCES_HELP := \ 12 | main.cpp \ 13 | test_console_help.cpp 14 | 15 | CXX_SOURCES_NO_HELP := \ 16 | main.cpp \ 17 | test_console_no_help.cpp 18 | 19 | CXX_INCLUDES := \ 20 | -I../include 21 | 22 | CC := gcc 23 | CXX := g++ 24 | 25 | OBJECTS_HELP := $(addprefix $(BUILD_DIR_HELP)/,$(notdir $(CXX_SOURCES_HELP:.cpp=.o))) $(addprefix $(BUILD_DIR_HELP)/,$(notdir $(C_SOURCES:.c=.o))) 26 | OBJECTS_NO_HELP := $(addprefix $(BUILD_DIR_NO_HELP)/,$(notdir $(CXX_SOURCES_NO_HELP:.cpp=.o))) $(addprefix $(BUILD_DIR_NO_HELP)/,$(notdir $(C_SOURCES:.c=.o))) 27 | vpath %.c $(sort $(dir $(C_SOURCES))) 28 | vpath %.cpp $(sort $(dir $(CXX_SOURCES_HELP))) 29 | vpath %.cpp $(sort $(dir $(CXX_SOURCES_NO_HELP))) 30 | 31 | CFLAGS := $(CXX_INCLUDES) -g3 -Werror $(addprefix -D,$(C_DEFS)) 32 | CPP_FLAGS := 33 | LDFLAGS := -lgtest -lpthread 34 | 35 | ifeq ($(OS),Windows_NT) 36 | $(error "Windows is not currently supported") 37 | else 38 | UNAME_S := $(shell uname -s) 39 | ifeq ($(UNAME_S),Darwin) 40 | # Likely using clang 41 | CFLAGS += -Wno-extern-c-compat 42 | CPP_FLAGS += -std=c++14 43 | else 44 | CPP_FLAGS += -D_Static_assert=static_assert 45 | endif 46 | endif 47 | 48 | $(BUILD_DIR_HELP)/%.o: %.c Makefile | $(BUILD_DIR_HELP) 49 | @echo "Compiling $(notdir $@)" 50 | @$(CC) -c $(CFLAGS) -DCONSOLE_HELP_COMMAND=1 -MD -MF"$(@:%.o=%.d)" $< -o $@ 51 | 52 | $(BUILD_DIR_NO_HELP)/%.o: %.c Makefile | $(BUILD_DIR_NO_HELP) 53 | @echo "Compiling $(notdir $@)" 54 | @$(CC) -c $(CFLAGS) -DCONSOLE_HELP_COMMAND=0 -MD -MF"$(@:%.o=%.d)" $< -o $@ 55 | 56 | $(BUILD_DIR_HELP)/%.o: %.cpp Makefile | $(BUILD_DIR_HELP) 57 | @echo "Compiling $(notdir $@)" 58 | @$(CXX) -c $(CFLAGS) -DCONSOLE_HELP_COMMAND=1 $(CPP_FLAGS) -MD -MF"$(@:%.o=%.d)" $< -o $@ 59 | 60 | $(BUILD_DIR_NO_HELP)/%.o: %.cpp Makefile | $(BUILD_DIR_NO_HELP) 61 | @echo "Compiling $(notdir $@)" 62 | @$(CXX) -c $(CFLAGS) -DCONSOLE_HELP_COMMAND=0 $(CPP_FLAGS) -MD -MF"$(@:%.o=%.d)" $< -o $@ 63 | 64 | $(BUILD_DIR_HELP)/$(TARGET): $(OBJECTS_HELP) Makefile | $(BUILD_DIR_HELP) 65 | @echo "Linking $(notdir $@)" 66 | @$(CXX) $(OBJECTS_HELP) -o $@ $(LDFLAGS) 67 | 68 | $(BUILD_DIR_NO_HELP)/$(TARGET): $(OBJECTS_NO_HELP) Makefile | $(BUILD_DIR_NO_HELP) 69 | @echo "Linking $(notdir $@)" 70 | @$(CXX) $(OBJECTS_NO_HELP) -o $@ $(LDFLAGS) 71 | 72 | $(BUILD_DIR_HELP): 73 | @mkdir -p $@ 74 | 75 | $(BUILD_DIR_NO_HELP): 76 | @mkdir -p $@ 77 | 78 | build: $(BUILD_DIR_HELP)/$(TARGET) $(BUILD_DIR_NO_HELP)/$(TARGET) 79 | 80 | test: $(BUILD_DIR_HELP)/$(TARGET) $(BUILD_DIR_NO_HELP)/$(TARGET) 81 | @echo "Running test with help command" 82 | @$(BUILD_DIR_HELP)/$(TARGET) 83 | @echo "Running test without help command" 84 | @$(BUILD_DIR_NO_HELP)/$(TARGET) 85 | 86 | clean: 87 | @echo "Deleting build folder" 88 | @rm -fR $(BUILD_DIR_COMMON) 89 | 90 | -include $(wildcard $(BUILD_DIR_HELP)/*.d) 91 | -include $(wildcard $(BUILD_DIR_NO_HELP)/*.d) 92 | .PHONY: test clean build 93 | .DEFAULT_GOAL := test 94 | -------------------------------------------------------------------------------- /console/tests/main.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | extern "C" { 4 | 5 | #include "anchor/console/console.h" 6 | 7 | }; 8 | 9 | #include 10 | 11 | #define CONSOLE_COMMAND_GET_INT_ARG(NAME) ({ \ 12 | int _arg; \ 13 | const bool _res = console_command_get_int_arg(NAME, &_arg); \ 14 | ASSERT_TRUE(_res); \ 15 | _arg; \ 16 | }) 17 | 18 | #define CONSOLE_COMMAND_GET_STR_ARG(NAME) ({ \ 19 | const char* _arg; \ 20 | const bool _res = console_command_get_str_arg(NAME, &_arg); \ 21 | ASSERT_TRUE(_res); \ 22 | _arg; \ 23 | }) 24 | 25 | #define CONSOLE_COMMAND_GET_OPTIONAL_INT_ARG(NAME, DEFAULT_VALUE) ({ \ 26 | int _arg; \ 27 | const bool _res = console_command_get_int_arg(NAME, &_arg); \ 28 | _res ? _arg : DEFAULT_VALUE; \ 29 | }) 30 | 31 | #if CONSOLE_HELP_COMMAND 32 | CONSOLE_COMMAND_DEF(say_hi, "Says hi"); 33 | CONSOLE_COMMAND_DEF(say_bye, "Says bye"); 34 | CONSOLE_COMMAND_DEF(minimal, NULL); 35 | CONSOLE_COMMAND_DEF(add, "Add two numbers", 36 | CONSOLE_INT_ARG_DEF(num1, "First number"), 37 | CONSOLE_INT_ARG_DEF(num2, "Second number"), 38 | CONSOLE_OPTIONAL_INT_ARG_DEF(num3, "Third (optional) number") 39 | ); 40 | CONSOLE_COMMAND_DEF(stroff, "Prints a string starting from an offset", 41 | CONSOLE_STR_ARG_DEF(str, "The string"), 42 | CONSOLE_INT_ARG_DEF(offset, "Offset into the string") 43 | ); 44 | #else 45 | CONSOLE_COMMAND_DEF(say_hi); 46 | CONSOLE_COMMAND_DEF(say_bye); 47 | CONSOLE_COMMAND_DEF(minimal); 48 | CONSOLE_COMMAND_DEF(add, 49 | CONSOLE_INT_ARG_DEF(num1), 50 | CONSOLE_INT_ARG_DEF(num2), 51 | CONSOLE_OPTIONAL_INT_ARG_DEF(num3) 52 | ); 53 | CONSOLE_COMMAND_DEF(stroff, 54 | CONSOLE_STR_ARG_DEF(str), 55 | CONSOLE_INT_ARG_DEF(offset) 56 | ); 57 | #endif 58 | 59 | std::vector g_console_write_buffer; 60 | 61 | static void write_function(const char* str) { 62 | char c; 63 | while ((c = *str++) != '\0') { 64 | g_console_write_buffer.push_back(c); 65 | } 66 | } 67 | 68 | static void say_hi_command_handler(void) { 69 | write_function("hi\n"); 70 | } 71 | 72 | static void say_bye_command_handler(void) { 73 | write_function("hi\n"); 74 | } 75 | 76 | static void minimal_command_handler(void) { 77 | write_function("Hello world!\n"); 78 | } 79 | 80 | static void add_command_handler(const add_args_t* args) { 81 | char buffer[100]; 82 | if (args->num3 != CONSOLE_INT_ARG_DEFAULT) { 83 | snprintf(buffer, sizeof(buffer), "%ld + %ld + %ld = %ld\n", args->num1, args->num2, args->num3, args->num1 + args->num2 + args->num3); 84 | } else { 85 | snprintf(buffer, sizeof(buffer), "%ld + %ld = %ld\n", args->num1, args->num2, args->num1 + args->num2); 86 | } 87 | write_function(buffer); 88 | } 89 | 90 | static void stroff_command_handler(const stroff_args_t* args) { 91 | char buffer[100]; 92 | snprintf(buffer, sizeof(buffer), "%s\n", args->str + args->offset); 93 | write_function(buffer); 94 | } 95 | 96 | int main(int argc, char **argv) { 97 | const console_init_t init_console = { 98 | .write_function = write_function, 99 | }; 100 | console_init(&init_console); 101 | g_console_write_buffer.clear(); 102 | console_command_register(say_hi); 103 | console_command_register(say_bye); 104 | console_command_register(minimal); 105 | console_command_register(add); 106 | console_command_register(stroff); 107 | 108 | ::testing::InitGoogleTest(&argc, argv); 109 | return RUN_ALL_TESTS(); 110 | } 111 | -------------------------------------------------------------------------------- /console/tests/test_console_help.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | extern "C" { 4 | 5 | #include "anchor/console/console.h" 6 | 7 | }; 8 | 9 | #include 10 | 11 | extern std::vector g_console_write_buffer; 12 | 13 | #define EXPECT_WRITE_BUFFER(str) do { \ 14 | g_console_write_buffer.push_back('\0'); \ 15 | const char* write_buffer_str = g_console_write_buffer.data(); \ 16 | EXPECT_STREQ(write_buffer_str, str); \ 17 | g_console_write_buffer.clear(); \ 18 | } while (0) 19 | 20 | static void process_line(const char* str) { 21 | console_process((const uint8_t*)str, (uint32_t)strlen(str)); 22 | } 23 | 24 | #if CONSOLE_HELP_COMMAND 25 | TEST(ConsoleTest, TestHelpCommand) { 26 | process_line("help\n"); 27 | EXPECT_WRITE_BUFFER("help\n" 28 | "Available commands:\n" 29 | " help - List all commands, or give details about a specific command\n" 30 | " say_hi - Says hi\n" 31 | " say_bye - Says bye\n" 32 | " minimal\n" 33 | " add - Add two numbers\n" 34 | " stroff - Prints a string starting from an offset\n" 35 | "> "); 36 | 37 | process_line("help add\n"); 38 | EXPECT_WRITE_BUFFER( 39 | "help add\n" 40 | "Add two numbers\n" 41 | "Usage: add num1 num2 [num3]\n" 42 | " num1 - First number\n" 43 | " num2 - Second number\n" 44 | " num3 - Third (optional) number\n" 45 | "> "); 46 | 47 | process_line("help minimal\n"); 48 | EXPECT_WRITE_BUFFER( 49 | "help minimal\n" 50 | "Usage: minimal\n" 51 | "> "); 52 | } 53 | 54 | TEST(ConsoleTest, TestHelpTabCompletion) { 55 | process_line("help sa\t"); 56 | EXPECT_WRITE_BUFFER("help say_"); 57 | 58 | process_line("\t"); 59 | EXPECT_WRITE_BUFFER("\nsay_hi say_bye\n> help say_"); 60 | 61 | process_line("h\t"); 62 | EXPECT_WRITE_BUFFER("hi"); 63 | } 64 | #endif 65 | -------------------------------------------------------------------------------- /console/tests/test_console_no_help.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | extern "C" { 4 | 5 | #include "anchor/console/console.h" 6 | 7 | }; 8 | 9 | #include 10 | 11 | #define LEFT_ARROW_SEQUENCE "\x1b[D" 12 | #define RIGHT_ARROW_SEQUENCE "\x1b[C" 13 | 14 | extern std::vector g_console_write_buffer; 15 | 16 | #define EXPECT_WRITE_BUFFER(str) do { \ 17 | g_console_write_buffer.push_back('\0'); \ 18 | const char* write_buffer_str = g_console_write_buffer.data(); \ 19 | EXPECT_STREQ(write_buffer_str, str); \ 20 | g_console_write_buffer.clear(); \ 21 | } while (0) 22 | 23 | static void process_line(const char* str) { 24 | console_process((const uint8_t*)str, (uint32_t)strlen(str)); 25 | } 26 | 27 | TEST(ConsoleTest, TestInvalidCommand) { 28 | process_line("\n"); 29 | EXPECT_WRITE_BUFFER("\n> "); 30 | 31 | process_line("invalid\n"); 32 | EXPECT_WRITE_BUFFER( 33 | "invalid\n" 34 | "ERROR: Command not found (invalid)\n> "); 35 | 36 | process_line("2 add\n"); 37 | EXPECT_WRITE_BUFFER( 38 | "2 add\n" 39 | "ERROR: Command not found (2)\n> "); 40 | 41 | process_line("\a\n"); 42 | EXPECT_WRITE_BUFFER("\n> "); 43 | } 44 | 45 | TEST(ConsoleTest, TestValidHiCommand) { 46 | process_line("say_hi\n"); 47 | EXPECT_WRITE_BUFFER( 48 | "say_hi\n" 49 | "hi\n> "); 50 | } 51 | 52 | TEST(ConsoleTest, TestInvalidHiCommand) { 53 | process_line("say_hi there\n"); 54 | EXPECT_WRITE_BUFFER( 55 | "say_hi there\n" 56 | "ERROR: Too many arguments\n> "); 57 | 58 | process_line("say_hi \n"); 59 | EXPECT_WRITE_BUFFER( 60 | "say_hi \n" 61 | "ERROR: Extra whitespace between arguments\n> "); 62 | } 63 | 64 | TEST(ConsoleTest, TestValidAddCommand) { 65 | process_line("add 1 2\n"); 66 | EXPECT_WRITE_BUFFER( 67 | "add 1 2\n" 68 | "1 + 2 = 3\n> "); 69 | 70 | process_line("add 1 2 3\n"); 71 | EXPECT_WRITE_BUFFER( 72 | "add 1 2 3\n" 73 | "1 + 2 + 3 = 6\n> "); 74 | 75 | process_line("add -5 10\n"); 76 | EXPECT_WRITE_BUFFER( 77 | "add -5 10\n" 78 | "-5 + 10 = 5\n> "); 79 | 80 | process_line("add 0xff 0x100\n"); 81 | EXPECT_WRITE_BUFFER( 82 | "add 0xff 0x100\n" 83 | "255 + 256 = 511\n> "); 84 | } 85 | 86 | TEST(ConsoleTest, TestInvalidAddCommand) { 87 | process_line("add\n"); 88 | EXPECT_WRITE_BUFFER( 89 | "add\n" 90 | "ERROR: Too few arguments\n> "); 91 | 92 | process_line("add 2\n"); 93 | EXPECT_WRITE_BUFFER( 94 | "add 2\n" 95 | "ERROR: Too few arguments\n> "); 96 | 97 | process_line("add 2 3 4 5\n"); 98 | EXPECT_WRITE_BUFFER( 99 | "add 2 3 4 5\n" 100 | "ERROR: Too many arguments\n> "); 101 | 102 | process_line("add 2 3\n"); 103 | EXPECT_WRITE_BUFFER( 104 | "add 2 3\n" 105 | "ERROR: Extra whitespace between arguments\n> "); 106 | 107 | process_line("add 2 x\n"); 108 | EXPECT_WRITE_BUFFER( 109 | "add 2 x\n" 110 | "ERROR: Invalid value for 'num2' (x)\n> "); 111 | 112 | process_line("add 2 0xZZ\n"); 113 | EXPECT_WRITE_BUFFER( 114 | "add 2 0xZZ\n" 115 | "ERROR: Invalid value for 'num2' (0xZZ)\n> "); 116 | 117 | process_line("add 0xZZ 0xYY\n"); 118 | EXPECT_WRITE_BUFFER( 119 | "add 0xZZ 0xYY\n" 120 | "ERROR: Invalid value for 'num1' (0xZZ)\n> "); 121 | } 122 | 123 | TEST(ConsoleTest, TestValidStroffCommand) { 124 | process_line("stroff myteststr 0\n"); 125 | EXPECT_WRITE_BUFFER( 126 | "stroff myteststr 0\n" 127 | "myteststr\n> "); 128 | 129 | process_line("stroff myteststr 2\n"); 130 | EXPECT_WRITE_BUFFER( 131 | "stroff myteststr 2\n" 132 | "teststr\n> "); 133 | 134 | process_line("stroff myteststr 9\n"); 135 | EXPECT_WRITE_BUFFER( 136 | "stroff myteststr 9\n" 137 | "\n> "); 138 | 139 | process_line("stroff 22222 1\n"); 140 | EXPECT_WRITE_BUFFER( 141 | "stroff 22222 1\n" 142 | "2222\n> "); 143 | } 144 | 145 | TEST(ConsoleTest, TestInvalidStroffCommand) { 146 | process_line("stroff\n"); 147 | EXPECT_WRITE_BUFFER( 148 | "stroff\n" 149 | "ERROR: Too few arguments\n> "); 150 | 151 | process_line("stroff 2\n"); 152 | EXPECT_WRITE_BUFFER( 153 | "stroff 2\n" 154 | "ERROR: Too few arguments\n> "); 155 | 156 | process_line("stroff 2 2 1\n"); 157 | EXPECT_WRITE_BUFFER( 158 | "stroff 2 2 1\n" 159 | "ERROR: Too many arguments\n> "); 160 | 161 | process_line("stroff 2 test\n"); 162 | EXPECT_WRITE_BUFFER( 163 | "stroff 2 test\n" 164 | "ERROR: Invalid value for 'offset' (test)\n> "); 165 | } 166 | 167 | #if CONSOLE_HELP_COMMAND 168 | TEST(ConsoleTest, TestHelpCommand) { 169 | process_line("help\n"); 170 | EXPECT_WRITE_BUFFER("help\n" 171 | "Available commands:\n" 172 | " help - List all commands, or give details about a specific command\n" 173 | " say_hi - Says hi\n" 174 | " say_bye - Says bye\n" 175 | " minimal\n" 176 | " add - Add two numbers\n" 177 | " stroff - Prints a string starting from an offset\n" 178 | "> "); 179 | 180 | process_line("help add\n"); 181 | EXPECT_WRITE_BUFFER( 182 | "help add\n" 183 | "Add two numbers\n" 184 | "Usage: add num1 num2 [num3]\n" 185 | " num1 - First number\n" 186 | " num2 - Second number\n" 187 | " num3 - Third (optional) number\n" 188 | "> "); 189 | 190 | process_line("help minimal\n"); 191 | EXPECT_WRITE_BUFFER( 192 | "help minimal\n" 193 | "Usage: minimal\n" 194 | "> "); 195 | } 196 | #endif 197 | 198 | TEST(ConsoleTest, TestTabComplete) { 199 | process_line("sa\t"); 200 | EXPECT_WRITE_BUFFER("say_"); 201 | 202 | process_line("\t"); 203 | EXPECT_WRITE_BUFFER("\nsay_hi say_bye\n> say_"); 204 | 205 | process_line("b\t"); 206 | EXPECT_WRITE_BUFFER("bye"); 207 | 208 | process_line("\t"); 209 | EXPECT_WRITE_BUFFER(""); 210 | 211 | process_line("\n"); 212 | EXPECT_WRITE_BUFFER("\nhi\n> "); 213 | } 214 | 215 | TEST(ConsoleTest, TestLeftRightArrow) { 216 | process_line("sayh"); 217 | EXPECT_WRITE_BUFFER("sayh"); 218 | 219 | process_line(LEFT_ARROW_SEQUENCE); 220 | EXPECT_WRITE_BUFFER("\b"); 221 | 222 | process_line(RIGHT_ARROW_SEQUENCE); 223 | EXPECT_WRITE_BUFFER("h"); 224 | 225 | process_line(LEFT_ARROW_SEQUENCE); 226 | EXPECT_WRITE_BUFFER("\b"); 227 | 228 | process_line("_"); 229 | EXPECT_WRITE_BUFFER("_h\b"); 230 | 231 | process_line(RIGHT_ARROW_SEQUENCE); 232 | EXPECT_WRITE_BUFFER("h"); 233 | 234 | process_line("i"); 235 | EXPECT_WRITE_BUFFER("i"); 236 | 237 | process_line("\n"); 238 | EXPECT_WRITE_BUFFER("\nhi\n> "); 239 | } 240 | 241 | TEST(ConsoleTest, TestPrintLine) { 242 | process_line("say_"); 243 | EXPECT_WRITE_BUFFER("say_"); 244 | 245 | process_line(LEFT_ARROW_SEQUENCE); 246 | EXPECT_WRITE_BUFFER("\b"); 247 | 248 | console_print_line("Log line\n"); 249 | EXPECT_WRITE_BUFFER( 250 | "\rLog line\n" 251 | "> say_\b"); 252 | 253 | process_line(RIGHT_ARROW_SEQUENCE); 254 | EXPECT_WRITE_BUFFER("_"); 255 | 256 | process_line("hi\n"); 257 | EXPECT_WRITE_BUFFER("hi\nhi\n> "); 258 | } 259 | -------------------------------------------------------------------------------- /fsm/README.md: -------------------------------------------------------------------------------- 1 | # FSM 2 | 3 | A small library which implements an event-driven FSM, where the implementation 4 | exists entirely within state entry and exit handlers. 5 | 6 | ## Usage 7 | 8 | The first step is to define the set of states and events which are involved in 9 | the FSM. This is accomplished using the `FSM_STATE_DEF(NAME)` and 10 | `FSM_EVENT_DEF(NAME)` macros. Next, all valid transitions should be defined in 11 | an array of `fsm_transition_t` instances. The `FSM_STATE(NAME)` and 12 | `FSM_EVENT(NAME)` macros can be used to referenced the defined states and 13 | events respectively. These macros allow for convenient naming and namespacing 14 | of states and events, while allowing the transitions array to be defined at 15 | compile time, and thus live in program space (.rodata), as opposed to RAM. 16 | 17 | A state entry and exit handler should be defined, and passed along with the 18 | the transitions array to fsm_init() to initialize the FSM. 19 | 20 | Events are then passed to `fsm_process_event()` which generates appropriate 21 | state entry / exit callbacks, if there is an appropriate transition defined. 22 | 23 | ## Compile Options 24 | 25 | Additional features of the FSM library can be enabled by compiling with the 26 | following defines: 27 | * `FSM_USE_LOGGING` - Controls logging via the `logging` library. Set to 1 to 28 | enable logging of error conditions and state transitions. Set to 2 to also 29 | enable logging of ignored events. 30 | 31 | ## Example 32 | 33 | ``` 34 | // FSM States 35 | FSM_STATE_DEF(OFF); 36 | FSM_STATE_DEF(GREEN); 37 | FSM_STATE_DEF(RED); 38 | 39 | // FSM Events 40 | FSM_EVENT_DEF(REQUESTED_OFF); 41 | FSM_EVENT_DEF(REQUESTED_GREEN); 42 | FSM_EVENT_DEF(REQUESTED_RED); 43 | 44 | // FSM Transitions 45 | static const fsm_transition_t LED_FSM_TRANSITIONS[] = { 46 | {FSM_STATE(OFF), FSM_STATE(GREEN), FSM_EVENT(REQUESTED_GREEN)}, 47 | {FSM_STATE(OFF), FSM_STATE(RED), FSM_EVENT(REQUESTED_RED)}, 48 | {FSM_STATE(GREEN), FSM_STATE(OFF), FSM_EVENT(REQUESTED_OFF)}, 49 | {FSM_STATE(GREEN), FSM_STATE(RED), FSM_EVENT(REQUESTED_RED)}, 50 | {FSM_STATE(RED), FSM_STATE(OFF), FSM_EVENT(REQUESTED_OFF)}, 51 | {FSM_STATE(RED), FSM_STATE(GREEN), FSM_EVENT(REQUESTED_GREEN)}, 52 | }; 53 | 54 | static void fsm_on_state_enter_handler(const fsm_t* fsm, fsm_state_t state) { 55 | if (state == FSM_STATE(GREEN)) { 56 | gpio_set(GREEN_LED_PIN, HIGH); 57 | } else if (state == FSM_STATE(RED)) { 58 | gpio_set(RED_LED_PIN, HIGH); 59 | } 60 | } 61 | 62 | static void fsm_on_state_exit_handler(const fsm_t* fsm, fsm_state_t state) { 63 | if (state == FSM_STATE(GREEN)) { 64 | gpio_set(GREEN_LED_PIN, LOW); 65 | } else if (state == FSM_STATE(RED)) { 66 | gpio_set(RED_LED_PIN, LOW); 67 | } 68 | } 69 | 70 | void led_thread(void) { 71 | fsm_t fsm = { 72 | .transitions = FSM_TRANSITIONS, 73 | .num_transitions = ARRAY_LENGTH(FSM_TRANSITIONS), 74 | .on_state_enter_handler = fsm_on_state_enter_handler, 75 | .on_state_exit_handler = fsm_on_state_exit_handler, 76 | .initial_state = FSM_STATE(OFF), 77 | }; 78 | fsm_init(&fsm); 79 | 80 | while (true) { 81 | // get request 82 | if (/* requested off */) { 83 | fsm_process_event(&fsm, FSM_EVENT(REQUESTED_OFF)); 84 | } else if (/* requested green */) { 85 | fsm_process_event(&fsm, FSM_EVENT(REQUESTED_GREEN)); 86 | } else if (/* requested red */) { 87 | fsm_process_event(&fsm, FSM_EVENT(REQUESTED_RED)); 88 | } 89 | } 90 | } 91 | ``` 92 | -------------------------------------------------------------------------------- /fsm/include/anchor/fsm/fsm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #ifdef __SDCC_VERSION_MAJOR 6 | #define _FSM_CONTEXT_SIZE (sizeof(uintptr_t) + sizeof(uint8_t)) 7 | #else 8 | #define _FSM_CONTEXT_SIZE (sizeof(uintptr_t) * 2) 9 | #endif 10 | 11 | typedef struct { 12 | const char* name; 13 | } fsm_state_impl_t; 14 | typedef const fsm_state_impl_t* fsm_state_t; 15 | 16 | // Define a new FSM state with a given name 17 | #define FSM_STATE_DEF(NAME) \ 18 | static const fsm_state_impl_t _##NAME##_fsm_state = { \ 19 | .name = #NAME, \ 20 | } 21 | // Reference a previously-defined FSM state 22 | #define FSM_STATE(NAME) &_##NAME##_fsm_state 23 | 24 | typedef struct { 25 | const char* name; 26 | } fsm_event_impl_t; 27 | typedef const fsm_event_impl_t* fsm_event_t; 28 | 29 | // Define a new FSM event with a given name 30 | #define FSM_EVENT_DEF(NAME) \ 31 | static const fsm_event_impl_t _##NAME##_fsm_event = { \ 32 | .name = #NAME, \ 33 | } 34 | // Reference a previously-defined FSM event 35 | #define FSM_EVENT(NAME) &_##NAME##_fsm_event 36 | 37 | typedef struct { 38 | fsm_state_t from_state; 39 | fsm_state_t to_state; 40 | fsm_event_t event; 41 | } fsm_transition_t; 42 | 43 | // Structure which represents an FSM 44 | typedef struct fsm { 45 | // Allocated space for private context to be used by the FSM implementation only 46 | uint8_t _private[_FSM_CONTEXT_SIZE]; 47 | // Supported FSM transitions 48 | const fsm_transition_t* transitions; 49 | // Number of transitions 50 | uint32_t num_transitions; 51 | // Handler called when a state is entered 52 | void (*on_state_enter_handler)(const struct fsm* fsm, fsm_state_t state); 53 | // Handler called when a state is exitted 54 | void (*on_state_exit_handler)(const struct fsm* fsm, fsm_state_t state); 55 | // Initial FSM state 56 | fsm_state_t initial_state; 57 | } fsm_t; 58 | 59 | // Initialize the FSM 60 | void fsm_init(fsm_t* fsm); 61 | 62 | // Process an FSM event 63 | void fsm_process_event(fsm_t* fsm, fsm_event_t event); 64 | -------------------------------------------------------------------------------- /fsm/src/fsm.c: -------------------------------------------------------------------------------- 1 | #include "anchor/fsm/fsm.h" 2 | 3 | #ifndef FSM_USE_LOGGING 4 | #define FSM_USE_LOGGING 0 5 | #endif 6 | 7 | #if FSM_USE_LOGGING 8 | #include "anchor/logging/logging.h" 9 | #endif 10 | 11 | #include 12 | 13 | #define GET_IMPL(FSM) ((fsm_impl_t*)((FSM)->_private)) 14 | 15 | typedef struct { 16 | fsm_state_t state; 17 | bool in_transition; 18 | } fsm_impl_t; 19 | _Static_assert(sizeof(fsm_impl_t) == sizeof(((fsm_t*)0)->_private), "Invalid fsm_impl_t size"); 20 | 21 | void fsm_init(fsm_t* fsm) { 22 | fsm_impl_t* impl = GET_IMPL(fsm); 23 | impl->state = fsm->initial_state; 24 | } 25 | 26 | void fsm_process_event(fsm_t* fsm, fsm_event_t event) { 27 | fsm_impl_t* impl = (fsm_impl_t*)(fsm->_private); 28 | if (impl->in_transition) { 29 | #if FSM_USE_LOGGING >= 1 30 | LOG_ERROR("Attempting to process event from handler, dropping event"); 31 | #endif 32 | return; 33 | } 34 | for (uint32_t i = 0; i < fsm->num_transitions; i++) { 35 | const fsm_transition_t* transition = &fsm->transitions[i]; 36 | if (impl->state == transition->from_state && event == transition->event) { 37 | // execute this transition 38 | #if FSM_USE_LOGGING >= 1 39 | LOG_INFO("Executing transition from %s to %s (event=%s)", transition->from_state->name, transition->to_state->name, event->name); 40 | #endif 41 | impl->in_transition = true; 42 | fsm->on_state_exit_handler(fsm, impl->state); 43 | impl->state = transition->to_state; 44 | fsm->on_state_enter_handler(fsm, impl->state); 45 | impl->in_transition = false; 46 | return; 47 | } 48 | } 49 | #if FSM_USE_LOGGING >= 2 50 | LOG_INFO("Ignoring event with no defined transition (state=%s, event=%s)", impl->state->name, event->name); 51 | #endif 52 | } 53 | -------------------------------------------------------------------------------- /logging/README.md: -------------------------------------------------------------------------------- 1 | # Logging 2 | 3 | Simple logging library with support for multiple levels, timestamps, file name, 4 | and line numbers. 5 | 6 | ## Usage 7 | 8 | The logging library is initialized by calling `logging_init()`. 9 | 10 | To generate logs, simply include `logging.h` from your source file and use 11 | one of the `LOG_[DEBUG|INFO|WARN|ERROR]` macros as you would use `printf` to 12 | write a log line at a given level. Each file is treated as its own module, and 13 | the level of each module can be changed using the `LOG_SET_LEVEL(...)` macro. 14 | 15 | ### Shorter File Names 16 | 17 | The logging library will use the `__FILE__` define provided by gcc to get the 18 | name of the file. To reduce code size, and make the logs easier to read, it's 19 | recommended to use the `-fmacro-prefix-map={DIRECTORY}=` GCC option. 20 | An example Make rule which does this is shown below: 21 | ``` 22 | $(BUILD_DIR)/%.o: %.c 23 | @$(CC) -c -fmacro-prefix-map=$(dir $<)/= $(CFLAGS) $< -o $@ 24 | ``` 25 | 26 | ### Module Prefix 27 | 28 | In order to prefix the file name with a module name, define 29 | `LOGGING_MODULE_NAME` before including `logging.h` in your source file. 30 | 31 | ### Per-File Default Level 32 | 33 | In order to set a default logging level at compile time, define 34 | `LOGGING_FILE_DEFAULT_LEVEL` before including `logging.h`. 35 | 36 | ### Compile Options 37 | 38 | Parameters and additional features of the logging library can be configured 39 | with the following defines: 40 | * `LOGGING_CUSTOM_HANDLER` - Allows for specifying a custom log handler, which 41 | bypasses any formatting done by logging library. 42 | * `LOGGING_MAX_MSG_LENGTH` - The maximum length (128 by default) of a log 43 | message not including the extra prefixes added by the logging library. This 44 | setting is not used if LOGGING_CUSTOM_HANDLER is set. 45 | * `LOGGING_USE_DATETIME` - Prefixes log lines with a full datetime string 46 | (YYYY-MM-DD HH:MM:SS.SSS) instead of a system uptime (HHH:MM:SS.SSS) and 47 | changes `time_ms_function` to return a `uint64_t` which should be the number of 48 | milliseconds since epoch. This defaults to 0. 49 | 50 | ## Example Output 51 | ``` 52 | 0:00:00.626 WARN system.c:199: Last reset due to software reset 53 | 0:00:00.632 INFO system.c:243: Serial number is 003b001b524b501020353548 54 | 0:00:00.639 INFO system.c:257: Detected DVT hardware 55 | 0:00:00.648 INFO piezo.c:45: is_present=1 56 | 0:00:16.732 ERROR SONAR:link_layer.c:161: Invalid packet: Response sequence number does not match request 57 | ``` 58 | -------------------------------------------------------------------------------- /logging/include/anchor/logging/logging.h: -------------------------------------------------------------------------------- 1 | // We explicitly don't use include guards as this file should not be included recursively. 2 | 3 | // NOTE: LOGGING_MODULE_NAME can be defined before including this header in order to specify the module 4 | // NOTE: LOGGING_FILE_DEFAULT_LEVEL can be defined before including this header in order to change the default log level 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #ifdef __cplusplus 11 | extern "C" { 12 | #endif 13 | 14 | // The maximum length of a log message (not including extra formatting - not used if LOGGING_CUSTOM_HANDLER is set) 15 | #ifndef LOGGING_MAX_MSG_LENGTH 16 | #define LOGGING_MAX_MSG_LENGTH 128 17 | #endif 18 | 19 | // Define this to prefix logs with datetime instead of system time (also changes logging_timestamp_t to a uint64_t) 20 | #ifndef LOGGING_USE_DATETIME 21 | #define LOGGING_USE_DATETIME 0 22 | #endif 23 | 24 | // Define this in order to replace the logging library's handler - this is useful to implement custom formatters 25 | #ifndef LOGGING_CUSTOM_HANDLER 26 | #define LOGGING_CUSTOM_HANDLER 0 27 | #endif 28 | 29 | #if LOGGING_USE_DATETIME 30 | #define LOGGING_TIMESTAMP_FMT_STR "%04u-%02u-%02u %02u:%02u:%02u.%03u" 31 | #define LOGGING_TIMESTAMP_FMT_ARGS(COMPONENTS) \ 32 | (COMPONENTS).year, (COMPONENTS).month, (COMPONENTS).day, \ 33 | (COMPONENTS).hour, (COMPONENTS).minute, (COMPONENTS).second, (COMPONENTS).ms 34 | typedef uint64_t logging_timestamp_t; 35 | #else 36 | #define LOGGING_TIMESTAMP_FMT_STR "%3u:%02u:%02u.%03u" 37 | #define LOGGING_TIMESTAMP_FMT_ARGS(COMPONENTS) \ 38 | (COMPONENTS).hour, (COMPONENTS).minute, (COMPONENTS).second, (COMPONENTS).ms 39 | typedef uint32_t logging_timestamp_t; 40 | #endif 41 | 42 | typedef enum { 43 | LOGGING_LEVEL_DEFAULT = 0, // Used to represent the default level specified to logging_init() 44 | LOGGING_LEVEL_DEBUG, 45 | LOGGING_LEVEL_INFO, 46 | LOGGING_LEVEL_WARN, 47 | LOGGING_LEVEL_ERROR, 48 | } logging_level_t; 49 | 50 | typedef struct { 51 | #if LOGGING_USE_DATETIME 52 | uint16_t year; 53 | uint8_t month; 54 | uint8_t day; 55 | #endif 56 | uint8_t hour; 57 | uint8_t minute; 58 | uint8_t second; 59 | uint16_t ms; 60 | } logging_timestamp_components_t; 61 | 62 | typedef struct { 63 | logging_level_t level; 64 | const char* file; 65 | int line; 66 | const char* module_prefix; 67 | logging_timestamp_t timestamp; 68 | logging_timestamp_components_t timestamp_components; 69 | const char* fmt; 70 | va_list args; 71 | } logging_line_t; 72 | 73 | typedef struct { 74 | #if LOGGING_CUSTOM_HANDLER 75 | // Handler function 76 | void(*handler)(const logging_line_t* log_line); 77 | #else 78 | // Write function which gets passed a fully-formatted log line 79 | void(*write_function)(const char* str); 80 | // A lock function which is called to make the logging library thread-safe 81 | void(*lock_function)(bool acquire); 82 | #endif 83 | // A function which is called to get the current timestamp in milliseconds (either an epoch time or system uptime) 84 | // NOTE: This is not called while handling the lock_function() so much be implicitly thread-safe 85 | logging_timestamp_t(*time_ms_function)(void); 86 | // The default logging level 87 | logging_level_t default_level; 88 | } logging_init_t; 89 | 90 | // Initialize the logging library 91 | bool logging_init(const logging_init_t* init); 92 | 93 | // Logs a line which was manually captured through a printf-style function hook / macro (filtered by the default level) 94 | void logging_log_line(logging_level_t level, const char* file, int line, const char* module_prefix, const char* fmt, va_list args); 95 | 96 | // Formats a log line into the specified buffer 97 | void logging_format_line(const logging_line_t* log_line, char* buffer, uint32_t size); 98 | 99 | // Need to include this after our types or defined 100 | #include "logging_internal.h" 101 | 102 | // Returns whether a given level is active 103 | #define LOG_LEVEL_IS_ACTIVE(LEVEL) logging_level_is_active(&_logging_logger, LEVEL) 104 | 105 | // Change the logging threshold for the current module 106 | #define LOG_SET_LEVEL(LEVEL) _logging_logger.level = LEVEL 107 | 108 | // Macros for logging at each level 109 | #define LOG_DEBUG(...) _LOG_LEVEL_IMPL(LOGGING_LEVEL_DEBUG, __VA_ARGS__) 110 | #define LOG_INFO(...) _LOG_LEVEL_IMPL(LOGGING_LEVEL_INFO, __VA_ARGS__) 111 | #define LOG_WARN(...) _LOG_LEVEL_IMPL(LOGGING_LEVEL_WARN, __VA_ARGS__) 112 | #define LOG_ERROR(...) _LOG_LEVEL_IMPL(LOGGING_LEVEL_ERROR, __VA_ARGS__) 113 | 114 | #ifdef __cplusplus 115 | } 116 | #endif 117 | -------------------------------------------------------------------------------- /logging/include/anchor/logging/logging_internal.h: -------------------------------------------------------------------------------- 1 | // We explicitly don't use include guards as this file should not be included recursively. 2 | 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #ifdef _MSC_VER 8 | #define _LOGGING_FORMAT_ATTR 9 | #define _LOGGING_USED_ATTR 10 | #else 11 | #define _LOGGING_FORMAT_ATTR __attribute__((format(printf, 5, 6))) 12 | #define _LOGGING_USED_ATTR __attribute__((used)) 13 | #endif 14 | 15 | // Internal type used to represent a logger (should not be directly modified) 16 | typedef struct { 17 | logging_level_t level; 18 | const char* const module_prefix; 19 | } logging_logger_t; 20 | 21 | // Internal implementation macros / functions which are called via the LOG_* macros 22 | #define _LOG_LEVEL_IMPL(LEVEL, ...) if (logging_level_is_active(&_logging_logger, LEVEL)) logging_log_impl(LEVEL, __FILE__, __LINE__, _logging_logger.module_prefix, __VA_ARGS__) 23 | bool logging_level_is_active(logging_logger_t* logger, logging_level_t level); 24 | void logging_log_impl(logging_level_t level, const char* file, int line, const char* module_prefix, const char* fmt, ...) _LOGGING_FORMAT_ATTR; 25 | 26 | #ifdef LOGGING_MODULE_NAME 27 | #define _LOGGING_MODULE_PREFIX LOGGING_MODULE_NAME ":" 28 | #undef LOGGING_MODULE_NAME 29 | #else 30 | #define _LOGGING_MODULE_PREFIX 0 31 | #endif 32 | 33 | #ifndef LOGGING_FILE_DEFAULT_LEVEL 34 | #define LOGGING_FILE_DEFAULT_LEVEL LOGGING_LEVEL_DEFAULT 35 | #endif 36 | 37 | // Per-file context object which we should create 38 | static logging_logger_t _logging_logger _LOGGING_USED_ATTR = { 39 | .level = LOGGING_FILE_DEFAULT_LEVEL, 40 | .module_prefix = _LOGGING_MODULE_PREFIX, 41 | }; 42 | 43 | // Clean up any configuration / internal defines 44 | #undef LOGGING_FILE_DEFAULT_LEVEL 45 | #undef _LOGGING_MODULE_PREFIX 46 | #undef _LOGGING_FORMAT_ATTR 47 | #undef _LOGGING_USED_ATTR 48 | 49 | #ifdef __cplusplus 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /logging/src/logging.c: -------------------------------------------------------------------------------- 1 | #include "anchor/logging/logging.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define LEVEL_PREFIX_LENGTH 6 8 | #if LOGGING_USE_DATETIME 9 | #define TIME_LENGTH 24 10 | #else 11 | #define TIME_LENGTH 14 12 | #endif 13 | #define FILE_NAME_LENGTH 32 14 | #define FULL_LOG_MAX_LENGTH (LOGGING_MAX_MSG_LENGTH + LEVEL_PREFIX_LENGTH + TIME_LENGTH + FILE_NAME_LENGTH + 1) 15 | 16 | #define BUFFER_PRINTF(BUFFER, SIZE, FMT, ...) ({ \ 17 | const uint32_t _len = strlen(BUFFER); \ 18 | snprintf(&BUFFER[_len], (SIZE)-_len, FMT, __VA_ARGS__); \ 19 | }) 20 | 21 | #define BUFFER_VA_PRINTF(BUFFER, SIZE, FMT, ARGS) ({ \ 22 | const uint32_t _len = strlen(BUFFER); \ 23 | vsnprintf(&BUFFER[_len], (SIZE)-_len, FMT, ARGS); \ 24 | }) 25 | 26 | #define BUFFER_WRITE(BUFFER, SIZE, STR) BUFFER_PRINTF(BUFFER, SIZE, "%s", STR) 27 | 28 | static const char* const LEVEL_PREFIX[] = { 29 | [LOGGING_LEVEL_DEFAULT] = "????? ", // should never be printed 30 | [LOGGING_LEVEL_DEBUG] = "DEBUG ", 31 | [LOGGING_LEVEL_INFO] = "INFO ", 32 | [LOGGING_LEVEL_WARN] = "WARN ", 33 | [LOGGING_LEVEL_ERROR] = "ERROR ", 34 | }; 35 | #if LOGGING_USE_DATETIME 36 | static const uint8_t DAYS_PER_MONTH[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; 37 | #endif 38 | 39 | static logging_init_t m_init; 40 | 41 | #if LOGGING_USE_DATETIME 42 | static bool is_leap_year(uint16_t year) { 43 | // A year is a leap year if it's evenly divisible by 4 unless it's evenly divisible by 100 but not 400. 44 | return ((year & 3) == 0 && (year % 100)) || (year % 400) == 0; 45 | } 46 | #endif 47 | 48 | static void get_timestamp_components(logging_timestamp_t timestamp, logging_timestamp_components_t* components) { 49 | #if LOGGING_USE_DATETIME 50 | logging_timestamp_t seconds_timestamp = timestamp % 86400000; 51 | #else 52 | logging_timestamp_t seconds_timestamp = timestamp; 53 | #endif 54 | components->ms = seconds_timestamp % 1000; 55 | seconds_timestamp /= 1000; 56 | components->second = seconds_timestamp % 60; 57 | seconds_timestamp /= 60; 58 | components->minute = seconds_timestamp % 60; 59 | seconds_timestamp /= 60; 60 | components->hour = seconds_timestamp; 61 | 62 | #if LOGGING_USE_DATETIME 63 | // Calculate days since epoch (add 1 for the current day) 64 | uint32_t days = timestamp / 86400000 + 1; 65 | 66 | // Calculate the year 67 | uint16_t days_in_year = 365; 68 | components->year = 1970; 69 | while (days > days_in_year) { 70 | days -= days_in_year; 71 | components->year++; 72 | if (is_leap_year(components->year)) { 73 | // this is a leap year 74 | days_in_year = 366; 75 | } else { 76 | days_in_year = 365; 77 | } 78 | } 79 | 80 | // Calculate the month and day 81 | components->month = 0; 82 | for (uint8_t month = 1; month <= 12; month++) { 83 | const uint8_t days_in_month = (month == 2 && is_leap_year(components->year)) ? 29 : DAYS_PER_MONTH[month-1]; 84 | if (days <= days_in_month) { 85 | components->month = month; 86 | break; 87 | } else { 88 | days -= days_in_month; 89 | } 90 | } 91 | components->day = days; 92 | #endif 93 | } 94 | 95 | #if !LOGGING_CUSTOM_HANDLER 96 | static void handle_log_line(const logging_line_t* log_line) { 97 | if (m_init.lock_function) { 98 | m_init.lock_function(true); 99 | } 100 | static char buffer[FULL_LOG_MAX_LENGTH]; 101 | logging_format_line(log_line, buffer, sizeof(buffer)); 102 | m_init.write_function(buffer); 103 | if (m_init.lock_function) { 104 | m_init.lock_function(false); 105 | } 106 | } 107 | #endif 108 | 109 | static void log_line_helper(logging_level_t level, const char* file, int line, const char* module_prefix, const char* fmt, va_list args) { 110 | logging_line_t log_line = { 111 | .level = level, 112 | .file = file, 113 | .line = line, 114 | .module_prefix = module_prefix, 115 | .timestamp = m_init.time_ms_function ? m_init.time_ms_function() : 0, 116 | .fmt = fmt, 117 | .args = args, 118 | }; 119 | get_timestamp_components(log_line.timestamp, &log_line.timestamp_components); 120 | 121 | #if LOGGING_CUSTOM_HANDLER 122 | m_init.handler(&log_line); 123 | #else 124 | handle_log_line(&log_line); 125 | #endif 126 | } 127 | 128 | bool logging_init(const logging_init_t* init) { 129 | if (init->default_level == LOGGING_LEVEL_DEFAULT) { 130 | return false; 131 | } 132 | #if LOGGING_CUSTOM_HANDLER 133 | if (!init->handler) { 134 | return false; 135 | } 136 | #else 137 | if (!init->write_function) { 138 | return false; 139 | } 140 | #endif 141 | m_init = *init; 142 | return true; 143 | } 144 | 145 | void logging_log_line(logging_level_t level, const char* file, int line, const char* module_prefix, const char* fmt, va_list args) { 146 | if (level < m_init.default_level) { 147 | return; 148 | } 149 | log_line_helper(level, file, line, module_prefix, fmt, args); 150 | } 151 | 152 | bool logging_level_is_active(logging_logger_t* logger, logging_level_t level) { 153 | const logging_level_t min_level = logger->level == LOGGING_LEVEL_DEFAULT ? m_init.default_level : logger->level; 154 | return level >= min_level; 155 | } 156 | 157 | void logging_log_impl(logging_level_t level, const char* file, int line, const char* module_prefix, const char* fmt, ...) { 158 | va_list args; 159 | va_start(args, fmt); 160 | log_line_helper(level, file, line, module_prefix, fmt, args); 161 | va_end(args); 162 | } 163 | 164 | void logging_format_line(const logging_line_t* log_line, char* buffer, uint32_t size) { 165 | buffer[0] = '\0'; 166 | 167 | // Add the timestamp 168 | if (m_init.time_ms_function || log_line->timestamp) { 169 | logging_timestamp_components_t timestamp_components; 170 | get_timestamp_components(log_line->timestamp, ×tamp_components); 171 | BUFFER_PRINTF(buffer, size, LOGGING_TIMESTAMP_FMT_STR " ", LOGGING_TIMESTAMP_FMT_ARGS(timestamp_components)); 172 | } 173 | 174 | // Add the level 175 | BUFFER_WRITE(buffer, size, LEVEL_PREFIX[log_line->level]); 176 | 177 | // Add the module (if set) 178 | if (log_line->module_prefix) { 179 | BUFFER_WRITE(buffer, size, log_line->module_prefix); 180 | } 181 | 182 | // Add the file name 183 | BUFFER_WRITE(buffer, size, log_line->file); 184 | 185 | // Add the line number 186 | BUFFER_PRINTF(buffer, size, ":%d: ", log_line->line); 187 | 188 | // Add the formatted log message 189 | BUFFER_VA_PRINTF(buffer, size, log_line->fmt, log_line->args); 190 | 191 | // Always add a newline, even if it means truncating the message 192 | if (strlen(buffer) == size - 1) { 193 | // Need to make space for the newline 194 | buffer[size-2] = '\0'; 195 | } 196 | BUFFER_WRITE(buffer, size, "\n"); 197 | } 198 | -------------------------------------------------------------------------------- /sonar/README.md: -------------------------------------------------------------------------------- 1 | # SONAR 2 | 3 | This library contains an implementation of the SONAR communication protocol as 4 | described by the [Protocol Specification](ProtocolSpecification.md). The 5 | library is written such that it can be instantiated multiple times (as either 6 | a client, server, or both), with no internal static state. This is achieved by 7 | providing macros for the application code to define any static memory used to 8 | store client, server, or attribute state. 9 | 10 | ## Attributes 11 | 12 | Attributes represent either configuration or state data on the server and are 13 | the unit of data which is sent (as a read / write / notify request) between the 14 | client and server. 15 | 16 | It's recommended (but not strictly required) to use 17 | [protobuf](https://developers.google.com/protocol-buffers) for defining 18 | attributes. The SONAR library includes a plugin which makes it easy to use 19 | such attributes with SONAR. Note that SONAR attributes need to have a known 20 | max size, so it's suggested to also use the appropriate properties of a 21 | protobuf implementation such as [nanopb](https://github.com/nanopb/nanopb). 22 | 23 | ### Protobuf Extensions 24 | 25 | When defining a protobuf message for an attribute, the extensions defined in 26 | [sonar_extensions.proto](protobuf/sonar_extensions.proto) should be added. For 27 | example, to define a `DeviceInfo` attribute with attribute ID `0x264` which 28 | supports read and notify requests: 29 | ```protobuf 30 | message DeviceInfo { 31 | option (nanopb_msgopt).msgid = 0x264; 32 | option (sonar_msgopt).attr_ops = ATTR_OPS_R; 33 | 34 | enum HardwareRevision { 35 | UNKNOWN = 0; 36 | EVT = 1; 37 | DVT = 2; 38 | PVT = 3; 39 | } 40 | 41 | HardwareRevision hardware_revision = 1; 42 | string serial = 2 [(nanopb).max_size = 25]; 43 | } 44 | ``` 45 | 46 | ### Protoc Plugin 47 | 48 | SONAR's protoc plugin is used to extend the generated protobuf code (currently 49 | supports C, Java, and Python). The recommended way to leverage this for C code 50 | is via the [sonar_nanopb.mk](protobuf/sonar_nanopb.mk) GNU make file which 51 | contains rules for using nanopb to turn .proto files into .pb.c/.pb.h files, 52 | and will include the SONAR protoc plugin. 53 | 54 | ## Server 55 | 56 | The server is responsible for exposing a set of attributes to clients, handling 57 | read / write requests, and sending notify requests to the client. 58 | 59 | ### Initialization and Processing 60 | 61 | A SONAR server instance should first be defined using the 62 | `SONAR_SERVER_DEF()` macro. This instance is then initialized by calling 63 | `sonar_server_init()`. This function takes a few function pointers which are 64 | used by the SONAR server implementation: 65 | * `write_byte` - sends a byte of data over the physical layer to the client 66 | * `get_system_time_ms` - gets the current system time in ms to manage various 67 | retries and timeouts 68 | * `connection_changed_callback` - called when a client connects or disconnects 69 | * `attribute_notify_complete_handler` called when a notify request completes 70 | 71 | The `sonar_server_process()` function should be called regularly to allow the 72 | library to process any pending requests and handle timeouts. This function 73 | should be passed in any data which was received since the last time it was 74 | called, but should still be called even if no new data is available to handle 75 | any applicable connection and notify timeouts. The timeouts (defined in 76 | [timeouts.h](src/link_layer/timeouts.h)) create a lower bound of 100ms for the 77 | minimum interval which this should be called at. 78 | 79 | ### Attributes 80 | 81 | A server attribute whose data is defined via protobuf can be defined using the 82 | `SONAR_SERVER_PROTO_ATTR_DEF(PROTO_MSG_TYPE, NAME)` macro. This macro defines 83 | read and write handlers for the attributes (if those operations are supported) 84 | which handle the encoding/decoding of the attribute using nanopb. The macro 85 | defines the following prototypes which the application code must define (as 86 | applicable based on the supported requests) which it then calls: 87 | ```c 88 | static bool _read_handler(* msg); 89 | static bool _write_handler(const * msg); 90 | ``` 91 | 92 | Attributes are then registered with a SONAR server using the 93 | `sonar_server_register()` function. A notify can be sent to the client by 94 | calling `sonar_server_notify()`. Once the request is complete (either 95 | succeeds or fails), the `attribute_notify_complete_handler` which was 96 | previously specified will be called. 97 | 98 | ## Client 99 | 100 | The client connects to a server, issues read / write requests against its 101 | attributes, and handles notify requests from the server. 102 | 103 | ### Initialization and Processing 104 | 105 | A SONAR client instance should first be defined using the 106 | `SONAR_CLIENT_DEF()` macro. This instance is then initialized by calling 107 | `sonar_client_init()`. This function takes a few function pointers which are 108 | used by the SONAR client implementation: 109 | * `write_byte` - sends a byte of data over the physical layer to the server 110 | * `get_system_time_ms` - gets the current system time in ms to manage various 111 | retries and timeouts 112 | * `connection_changed_callback` - called when a client connects or disconnects 113 | * `attribute_read_complete_handler` - called when a read request completes 114 | * `attribute_write_complete_handler` - called when a write request completes 115 | * `attribute_notify_handler` handles a notify requests 116 | 117 | The `sonar_client_process()` function should be called regularly to allow the 118 | library to process any pending requests and handle timeouts. This function 119 | should be passed in any data which was received since the last time it was 120 | called, but should still be called even if no new data is available to handle 121 | any applicable connection and notify timeouts. The timeouts (defined in 122 | [timeouts.h](src/link_layer/timeouts.h)) create a lower bound of 100ms for the 123 | minimum interval which this should be called at. 124 | 125 | ### Attributes 126 | 127 | Attributes are registered with a SONAR client using `sonar_client_register()`. 128 | Notify requests are then handled via the `attribute_notify_handler` function 129 | which was previously specified. 130 | 131 | A read / write can be sent to the server by calling `sonar_client_read()` and 132 | `sonar_client_write()` functions respectively. If the attribute is not 133 | supported by the server, these functions will return `false`. Once the request 134 | is complete (either succeeds or fails), the appropriate 135 | `attribute_*_complete_handler` which was previously specified will be called. 136 | 137 | ## Tests 138 | 139 | The unit tests can be run by running `make` within the `tests` directory. 140 | These tests are built using 141 | [googletest](https://github.com/google/googletest) and depend on the 142 | `gtest` library being available on the system. 143 | 144 | ## Example 145 | 146 | Server: 147 | ```c 148 | #include "sonar_device_info.pb.h" 149 | 150 | SONAR_SERVER_DEF(m_server, 1024); 151 | SONAR_PROTO_ATTR_DEF(Sonar_DeviceInfo, SONAR_DEVICE_INFO); 152 | 153 | static void write_byte(uint8_t byte) { 154 | serial_write(&byte, sizeof(byte)); 155 | } 156 | 157 | static uint64_t get_system_time_ms(void) { 158 | return GET_SYS_TIME(); 159 | } 160 | 161 | static void connection_changed_callback(sonar_server_handle_t handle, bool connected) { 162 | LOG_INFO("Connection changed to %d", connected); 163 | } 164 | 165 | static bool Sonar_DeviceInfo_read_handler(Sonar_DeviceInfo* msg) { 166 | msg->hardware_revision = Sonar_DeviceInfo_HardwareRevision_PVT; 167 | strncpy(msg->serial, "123456789", sizeof(msg->serial)); 168 | return true; 169 | } 170 | 171 | static void attribute_notify_complete_handler(sonar_server_handle_t handle, bool success) { 172 | LOG_INFO("Notify complete (success=%d)", success); 173 | } 174 | 175 | void sonar_server_task(void) { 176 | const sonar_server_init_t init_server = { 177 | .write_byte = write_byte, 178 | .get_system_time_ms = get_system_time_ms, 179 | .connection_changed_callback = connection_changed_callback, 180 | .attribute_notify_complete_handler = attribute_notify_complete_handler, 181 | }; 182 | sonar_server_init(m_server, &init_server); 183 | sonar_server_register(m_server, SONAR_DEVICE_INFO); 184 | 185 | while (1) { 186 | uint8_t rx_buffer[64]; 187 | uint32_t rx_len = serial_read(rx_buffer, sizeof(rx_buffer)); 188 | sonar_server_process(m_server, rx_buffer, rx_len); 189 | // ... 190 | } 191 | } 192 | ``` 193 | Client: 194 | 195 | ```c 196 | SONAR_CLIENT_DEF(m_client, 1024); 197 | SONAR_PROTO_ATTR_DEF(Sonar_DeviceInfo); 198 | 199 | static uint32_t m_data_value = 0x42424242; 200 | 201 | static void write_byte(uint8_t byte) { 202 | serial_write(&byte, sizeof(byte)); 203 | } 204 | 205 | static uint64_t get_system_time_ms(void) { 206 | return GET_SYS_TIME(); 207 | } 208 | 209 | static void connection_changed_callback(sonar_server_handle_t handle, bool connected) { 210 | LOG_INFO("Connection changed to %d", connected); 211 | } 212 | 213 | static void attribute_read_complete_handler(bool success, const void* data, uint32_t length) { 214 | Sonar_DeviceInfo msg; 215 | if (!SONAR_PROTO_ATTR_DECODE(Sonar_DeviceInfo, data, length, &msg)) { 216 | return; 217 | } 218 | LOG_INFO("Read complete (serial=%s)", msg.serial); 219 | } 220 | 221 | static void attribute_write_complete_handler(bool success) { 222 | LOG_INFO("Write complete"); 223 | } 224 | 225 | static bool attribute_notify_handler(sonar_attribute_t attr, const void* data, uint32_t length) { 226 | // ... 227 | return false; 228 | } 229 | 230 | void sonar_client_task(void) { 231 | const sonar_client_init_t init_client = { 232 | .write_byte = write_byte, 233 | .get_system_time_ms = get_system_time_ms, 234 | .connection_changed_callback = connection_changed_callback, 235 | .attribute_read_complete_handler = attribute_read_complete_handler, 236 | .attribute_write_complete_handler = attribute_write_complete_handler, 237 | .attribute_notify_handler = attribute_notify_handler, 238 | }; 239 | sonar_client_init(m_client, &init_client); 240 | sonar_client_register(m_client, SONAR_PROTO_ATTR(Sonar_DeviceInfo)); 241 | 242 | while (1) { 243 | uint8_t rx_buffer[64]; 244 | uint32_t rx_len = serial_read(rx_buffer, sizeof(rx_buffer)); 245 | sonar_client_process(m_client, rx_buffer, rx_len); 246 | if (/* should read TEST_ATTR */) { 247 | sonar_client_read(m_client, SONAR_PROTO_ATTR(Sonar_DeviceInfo)); 248 | } 249 | // ... 250 | } 251 | } 252 | ``` 253 | -------------------------------------------------------------------------------- /sonar/include/anchor/sonar/attribute.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | struct sonar_attribute_def; 6 | typedef struct sonar_attribute_def sonar_attribute_def_t; 7 | 8 | // The type used to represent a SONAR attribute for SONAR APIs 9 | typedef sonar_attribute_def_t* sonar_attribute_t; 10 | 11 | // Enum which defines various supported attribute operations 12 | typedef enum { 13 | SONAR_ATTRIBUTE_OPS_R = 0x1000, 14 | SONAR_ATTRIBUTE_OPS_W = 0x2000, 15 | SONAR_ATTRIBUTE_OPS_N = 0x4000, 16 | SONAR_ATTRIBUTE_OPS_RW = SONAR_ATTRIBUTE_OPS_R | SONAR_ATTRIBUTE_OPS_W, 17 | SONAR_ATTRIBUTE_OPS_RN = SONAR_ATTRIBUTE_OPS_R | SONAR_ATTRIBUTE_OPS_N, 18 | SONAR_ATTRIBUTE_OPS_WN = SONAR_ATTRIBUTE_OPS_W | SONAR_ATTRIBUTE_OPS_N, 19 | SONAR_ATTRIBUTE_OPS_RWN = SONAR_ATTRIBUTE_OPS_R | SONAR_ATTRIBUTE_OPS_W | SONAR_ATTRIBUTE_OPS_N, 20 | } sonar_attribute_ops_t; 21 | 22 | struct sonar_attribute_def { 23 | // Allocated private context space - should only be accessed by the SONAR implementation 24 | uint8_t _private[sizeof(void*) * 2]; 25 | // The ID of the attribute 26 | const uint16_t attribute_id:12; 27 | // The maximum size of the attribute data 28 | const uint32_t max_size; 29 | // Which operations the attribute supports 30 | const sonar_attribute_ops_t ops; 31 | // Pointer to a statically-allocated data buffer for the attribute which is used internally by SONAR for requests 32 | uint8_t* const request_buffer; 33 | // Pointer to a statically-allocated data buffer for the attribute which is used internally by SONAR for responses 34 | uint8_t* const response_buffer; 35 | }; 36 | 37 | 38 | // SONAR_ATTR_BUFFER_ATTRIBUTES can optionally be set to add attributes to the buffers used by SONAR attributes 39 | #ifndef SONAR_ATTR_BUFFER_ATTRIBUTES 40 | #define SONAR_ATTR_BUFFER_ATTRIBUTES 41 | #endif 42 | 43 | /* 44 | * The SONAR_ATTR_DEF macro below is used to define SONAR attributes: 45 | * NAME - The name of the variable which will be created and can be passed to sonar_server_* APIs 46 | * ID - The 12-bit attribute ID 47 | * MAX_SIZE - The maximum size of the attribute 48 | * OPS - The operations supported by the attribute (R/W/N/RW/RN/WN/RWN) 49 | * 50 | * NOTE: MSVC doesn't allow for zero-sized buffers, so we make sure the size is 1 in those cases. 51 | */ 52 | #define SONAR_ATTR_DEF(NAME, ID, MAX_SIZE, OPS) \ 53 | static uint8_t _##NAME##_request_buffer[(MAX_SIZE) ? (MAX_SIZE) : 1] SONAR_ATTR_BUFFER_ATTRIBUTES; \ 54 | static uint8_t _##NAME##_response_buffer[(MAX_SIZE) ? (MAX_SIZE) : 1] SONAR_ATTR_BUFFER_ATTRIBUTES; \ 55 | static sonar_attribute_def_t _##NAME##_def = { \ 56 | ._private = {0}, \ 57 | .attribute_id = ID, \ 58 | .max_size = MAX_SIZE, \ 59 | .ops = SONAR_ATTRIBUTE_OPS_##OPS, \ 60 | .request_buffer = _##NAME##_request_buffer, \ 61 | .response_buffer = _##NAME##_response_buffer, \ 62 | }; \ 63 | static const sonar_attribute_t NAME = &_##NAME##_def; 64 | -------------------------------------------------------------------------------- /sonar/include/anchor/sonar/client.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "anchor/sonar/error_types.h" 4 | #include "anchor/sonar/attribute.h" 5 | 6 | #include 7 | #include 8 | 9 | // The context size depends on whether we're compiling for a 64-bit or 32-bit system due to struct padding 10 | #define _SONAR_CLIENT_CONTEXT_SIZE_32 340 11 | #define _SONAR_CLIENT_CONTEXT_SIZE_64 560 12 | #define _SONAR_CLIENT_CONTEXT_SIZE ( \ 13 | sizeof(sonar_client_init_t) + \ 14 | ((sizeof(uintptr_t) == 8) ? _SONAR_CLIENT_CONTEXT_SIZE_64 : _SONAR_CLIENT_CONTEXT_SIZE_32)) 15 | 16 | // Defines a SONAR client object which can support attributes of up to MAX_ATTR_SIZE 17 | #define SONAR_CLIENT_DEF(NAME, MAX_ATTR_SIZE) \ 18 | static uint8_t _##NAME##_receive_buffer[MAX_ATTR_SIZE + 6]; \ 19 | static sonar_client_context_t _##NAME##_context = { \ 20 | ._private = {0}, \ 21 | .receive_buffer = _##NAME##_receive_buffer, \ 22 | .receive_buffer_size = sizeof(_##NAME##_receive_buffer), \ 23 | }; \ 24 | static sonar_client_handle_t NAME = &_##NAME##_context 25 | 26 | typedef struct { 27 | // A function which writes a single byte over the physical layer 28 | void (*write_byte)(uint8_t byte); 29 | // A function which gets the current system time in ms 30 | uint64_t (*get_system_time_ms)(void); 31 | // Callback when the connection state changes 32 | void (*connection_changed_callback)(bool connected); 33 | // Callback when a sonar_client_read() request completes 34 | void (*attribute_read_complete_handler)(bool success, const void* data, uint32_t length); 35 | // Callback when a sonar_client_write() request completes 36 | void (*attribute_write_complete_handler)(bool success); 37 | // Callback when a notify request is received 38 | bool (*attribute_notify_handler)(sonar_attribute_t attr, const void* data, uint32_t length); 39 | } sonar_client_init_t; 40 | 41 | typedef struct { 42 | // Allocated space for private context to be used by the SONAR implementation only 43 | uint8_t _private[_SONAR_CLIENT_CONTEXT_SIZE]; 44 | // Receive buffer used by SONAR - should be large enough to store the largest supported attribute plus 6 bytes for overhead 45 | uint8_t* receive_buffer; 46 | // The size of the receive buffer in bytes 47 | uint32_t receive_buffer_size; 48 | } sonar_client_context_t; 49 | 50 | typedef sonar_client_context_t* sonar_client_handle_t; 51 | 52 | // Initialize the SONAR client 53 | void sonar_client_init(sonar_client_handle_t handle, const sonar_client_init_t* init); 54 | 55 | // The main process function for the SONAR client which gets passed data received since the last call 56 | // This should be called regularly even if there's no received data 57 | void sonar_client_process(sonar_client_handle_t handle, const uint8_t* received_data, uint32_t received_data_length); 58 | 59 | // Returns whether or not a client is connected to the SONAR client 60 | bool sonar_client_is_connected(sonar_client_handle_t handle); 61 | 62 | // Register a SONAR client attribute which was defined with `SONAR_CLIENT_ATTR_DEF()` 63 | void sonar_client_register(sonar_client_handle_t handle, sonar_attribute_t attr); 64 | 65 | // Sends a read request for the specified attribute 66 | bool sonar_client_read(sonar_client_handle_t handle, sonar_attribute_t attr); 67 | 68 | // Sends a write request for the specified attribute 69 | bool sonar_client_write(sonar_client_handle_t handle, sonar_attribute_t attr, const void* data, uint32_t length); 70 | 71 | // Gets the error counters and then clears them 72 | void sonar_client_get_and_clear_errors(sonar_client_handle_t handle, sonar_errors_t* errors); 73 | -------------------------------------------------------------------------------- /sonar/include/anchor/sonar/error_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | typedef struct { 6 | struct { 7 | // Invalid packets (bad header) 8 | uint32_t invalid_header; 9 | // Invalid packets (bad CRC) 10 | uint32_t invalid_crc; 11 | // Receive buffer overflows 12 | uint32_t buffer_overflow; 13 | // Invalid HDLC escape sequences 14 | uint32_t invalid_escape_sequence; 15 | } link_layer_receive; 16 | struct { 17 | // Invalid packets received 18 | uint32_t invalid_packet; 19 | // Packets received when none were expected 20 | uint32_t unexpected_packet; 21 | // Packets with invalid sequence numbers 22 | uint32_t invalid_sequence_number; 23 | // Transmission retries 24 | uint32_t retries; 25 | } link_layer; 26 | } sonar_errors_t; 27 | -------------------------------------------------------------------------------- /sonar/include/anchor/sonar/proto_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Gets the corresponding attribute symbol for a protobuf message type 4 | #define SONAR_PROTO_ATTR(PROTO_MSG_TYPE) PROTO_MSG_TYPE##_ATTR 5 | 6 | // Calls SONAR_ATTR_DEF() to define an attribute with the specified protobuf message type 7 | // The attribute symbol can be accessed via `SONAR_PROTO_ATTR(PROTO_MSG_TYPE)` 8 | #define SONAR_PROTO_ATTR_DEF(PROTO_MSG_TYPE) \ 9 | SONAR_PROTO_ATTR_DEF_WITH_NAME(PROTO_MSG_TYPE, SONAR_PROTO_ATTR(PROTO_MSG_TYPE)) 10 | #define SONAR_PROTO_ATTR_DEF_WITH_NAME(PROTO_MSG_TYPE, NAME) \ 11 | _SONAR_PROTO_ATTR_DEF_IMPL(NAME, PROTO_MSG_TYPE, PROTO_MSG_TYPE##_ops) 12 | 13 | // Helper macros for SONAR_PROTO_ATTR_*() 14 | #define _SONAR_PROTO_ATTR_DEF_IMPL(NAME, PROTO_MSG_TYPE, OPS) \ 15 | SONAR_ATTR_DEF(NAME, PROTO_MSG_TYPE##_msgid, PROTO_MSG_TYPE##_size, OPS) 16 | 17 | // Calls SONAR_SERVER_ATTR_DEF() to define a server attribute with the specified protobuf 18 | // message type. This macro also creates a wrapper around the read/write handler to take 19 | // care of the encoding / decoding and change the prototypes to be: 20 | // static bool _read_handler(PROTO_MSG_TYPE* msg); 21 | // static bool _write_handler(const PROTO_MSG_TYPE* msg); 22 | #define SONAR_SERVER_PROTO_ATTR_DEF(PROTO_MSG_TYPE, NAME) \ 23 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL(NAME, PROTO_MSG_TYPE, PROTO_MSG_TYPE##_ops) 24 | #define SONAR_SERVER_PROTO_ATTR_DEF_COPY(PROTO_MSG_TYPE, NAME) \ 25 | _SONAR_SERVER_PROTO_ATTR_DEF_COPY_IMPL(NAME, PROTO_MSG_TYPE, PROTO_MSG_TYPE##_ops) 26 | #define _SONAR_SERVER_PROTO_ATTR_DEF_COPY_IMPL(NAME, PROTO_MSG_TYPE, OPS) \ 27 | _SONAR_SERVER_PROTO_ATTR_DEF_COPY_IMPL2(NAME, PROTO_MSG_TYPE, OPS) 28 | #define _SONAR_SERVER_PROTO_ATTR_DEF_COPY_IMPL2(NAME, PROTO_MSG_TYPE, OPS) \ 29 | SONAR_SERVER_ATTR_DEF_NO_PROTOTYPES(PROTO_MSG_TYPE##_ATTR, NAME, PROTO_MSG_TYPE##_msgid, PROTO_MSG_TYPE##_size, OPS) 30 | 31 | // Helper macros for SONAR_SERVER_ATTR_*() 32 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL(NAME, PROTO_MSG_TYPE, OPS) \ 33 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL2(NAME, PROTO_MSG_TYPE, OPS) 34 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL2(NAME, PROTO_MSG_TYPE, OPS) \ 35 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_##OPS(PROTO_MSG_TYPE) \ 36 | SONAR_SERVER_ATTR_DEF(PROTO_MSG_TYPE##_ATTR, NAME, PROTO_MSG_TYPE##_msgid, PROTO_MSG_TYPE##_size, OPS) 37 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_R(PROTO_MSG_TYPE) \ 38 | _SONAR_SERVER_PROTO_READ_HANDLER_DEF(PROTO_MSG_TYPE) 39 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_W(PROTO_MSG_TYPE) \ 40 | _SONAR_SERVER_PROTO_WRITE_HANDLER_DEF(PROTO_MSG_TYPE) 41 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_N(PROTO_MSG_TYPE) 42 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_RW(PROTO_MSG_TYPE) \ 43 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_R(PROTO_MSG_TYPE) \ 44 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_W(PROTO_MSG_TYPE) 45 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_RN(PROTO_MSG_TYPE) \ 46 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_R(PROTO_MSG_TYPE) 47 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_WN(PROTO_MSG_TYPE) \ 48 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_W(PROTO_MSG_TYPE) 49 | #define _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_RWN(PROTO_MSG_TYPE) \ 50 | _SONAR_SERVER_PROTO_ATTR_DEF_IMPL_RW(PROTO_MSG_TYPE) 51 | 52 | #define _SONAR_SERVER_PROTO_READ_HANDLER_DEF(PROTO_MSG_TYPE) \ 53 | static bool PROTO_MSG_TYPE##_read_handler(PROTO_MSG_TYPE* msg); \ 54 | static uint32_t PROTO_MSG_TYPE##_ATTR_read_handler(void* response_data, uint32_t response_max_size) { \ 55 | PROTO_MSG_TYPE msg = {}; \ 56 | if (!PROTO_MSG_TYPE##_read_handler(&msg)) { \ 57 | return 0; \ 58 | }; \ 59 | return SONAR_PROTO_ATTR_ENCODE(PROTO_MSG_TYPE, &msg, response_data, response_max_size); \ 60 | } 61 | #define _SONAR_SERVER_PROTO_WRITE_HANDLER_DEF(PROTO_MSG_TYPE) \ 62 | static bool PROTO_MSG_TYPE##_write_handler(const PROTO_MSG_TYPE* msg); \ 63 | static bool PROTO_MSG_TYPE##_ATTR_write_handler(const void* data, uint32_t length) { \ 64 | PROTO_MSG_TYPE msg = {}; \ 65 | if (!SONAR_PROTO_ATTR_DECODE(PROTO_MSG_TYPE, data, length, &msg)) { \ 66 | return false; \ 67 | } \ 68 | return PROTO_MSG_TYPE##_write_handler(&msg); \ 69 | } 70 | 71 | #define SONAR_PROTO_ATTR_ENCODE(PROTO_MSG_TYPE, MSG_PTR, BUFFER, BUFFER_SIZE) ({ \ 72 | uint32_t _result; \ 73 | pb_ostream_t _stream = pb_ostream_from_buffer(BUFFER, BUFFER_SIZE); \ 74 | if (pb_encode(&_stream, PROTO_MSG_TYPE##_fields, MSG_PTR)) { \ 75 | _result = _stream.bytes_written; \ 76 | } else { \ 77 | LOG_ERROR("Failed to encode %s", #PROTO_MSG_TYPE); \ 78 | _result = 0; \ 79 | } \ 80 | _result; \ 81 | }) 82 | 83 | #define SONAR_PROTO_ATTR_DECODE(PROTO_MSG_TYPE, BUFFER, BUFFER_LEN, MSG_PTR) ({ \ 84 | PROTO_MSG_TYPE _temp_msg; \ 85 | pb_istream_t _stream = pb_istream_from_buffer(BUFFER, BUFFER_LEN); \ 86 | bool _result = pb_decode(&_stream, PROTO_MSG_TYPE##_fields, &_temp_msg); \ 87 | if (_result) { \ 88 | *(MSG_PTR) = _temp_msg; \ 89 | } else { \ 90 | LOG_ERROR("Failed to decode %s", #PROTO_MSG_TYPE); \ 91 | } \ 92 | _result; \ 93 | }) 94 | -------------------------------------------------------------------------------- /sonar/include/anchor/sonar/server.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "anchor/sonar/error_types.h" 4 | #include "anchor/sonar/attribute.h" 5 | 6 | #include 7 | #include 8 | 9 | // The context size depends on whether we're compiling for a 64-bit or 32-bit system due to struct padding 10 | // TODO: haven't figured out the correct 32-bit value yet 11 | #define _SONAR_SERVER_CONTEXT_SIZE_32 352 12 | #define _SONAR_SERVER_CONTEXT_SIZE_64 576 13 | #define _SONAR_SERVER_CONTEXT_SIZE ( \ 14 | sizeof(sonar_server_init_t) + \ 15 | ((sizeof(uintptr_t) == 8) ? _SONAR_SERVER_CONTEXT_SIZE_64 : _SONAR_SERVER_CONTEXT_SIZE_32)) 16 | 17 | // Defines a SONAR server object which can support attributes of up to MAX_ATTR_SIZE 18 | #define SONAR_SERVER_DEF(NAME, MAX_ATTR_SIZE) \ 19 | static uint8_t _##NAME##_receive_buffer[MAX_ATTR_SIZE + 6 /* protocol overhead */]; \ 20 | static struct sonar_server_context _##NAME##_context = { \ 21 | ._private = {0}, \ 22 | .receive_buffer = _##NAME##_receive_buffer, \ 23 | .receive_buffer_size = sizeof(_##NAME##_receive_buffer), \ 24 | }; \ 25 | static sonar_server_handle_t NAME = &_##NAME##_context; 26 | 27 | // Defines a SONAR server attribute object 28 | #define SONAR_SERVER_ATTR_DEF(ATTR_NAME, VAR_NAME, ID, MAX_SIZE, OPS) \ 29 | _SONAR_SERVER_ATTR_HANDLERS_##OPS(ATTR_NAME) \ 30 | SONAR_SERVER_ATTR_DEF_NO_PROTOTYPES(ATTR_NAME, VAR_NAME, ID, MAX_SIZE, OPS) 31 | #define SONAR_SERVER_ATTR_DEF_NO_PROTOTYPES(ATTR_NAME, VAR_NAME, ID, MAX_SIZE, OPS) \ 32 | SONAR_ATTR_DEF(_##VAR_NAME##_attr, ID, MAX_SIZE, OPS); \ 33 | static struct sonar_server_attribute _##VAR_NAME##_server_attr = { \ 34 | ._private = {0}, \ 35 | .attr = _##VAR_NAME##_attr, \ 36 | .read_handler = ATTR_NAME##_read_handler, \ 37 | .write_handler = ATTR_NAME##_write_handler, \ 38 | }; \ 39 | static sonar_server_attribute_t VAR_NAME = &_##VAR_NAME##_server_attr 40 | 41 | // Helper macros for SONAR_SERVER_ATTR_DEF() 42 | #define _SONAR_SERVER_ATTR_HANDLERS_R(NAME) \ 43 | static uint32_t NAME##_read_handler(void* response_data, uint32_t response_max_size); \ 44 | static const void* const NAME##_write_handler = NULL; 45 | #define _SONAR_SERVER_ATTR_HANDLERS_W(NAME) \ 46 | static const void* const NAME##_read_handler = NULL; \ 47 | static bool NAME##_write_handler(const void* data, uint32_t length); 48 | #define _SONAR_SERVER_ATTR_HANDLERS_N(NAME) \ 49 | static const void* const NAME##_read_handler = NULL; \ 50 | static const void* const NAME##_write_handler = NULL; 51 | #define _SONAR_SERVER_ATTR_HANDLERS_RW(NAME) \ 52 | static uint32_t NAME##_read_handler(void* response_data, uint32_t response_max_size); \ 53 | static bool NAME##_write_handler(const void* data, uint32_t length); 54 | #define _SONAR_SERVER_ATTR_HANDLERS_RN(NAME) _SONAR_SERVER_ATTR_HANDLERS_R(NAME) 55 | #define _SONAR_SERVER_ATTR_HANDLERS_WN(NAME) _SONAR_SERVER_ATTR_HANDLERS_W(NAME) 56 | #define _SONAR_SERVER_ATTR_HANDLERS_RWN(NAME) _SONAR_SERVER_ATTR_HANDLERS_RW(NAME) 57 | 58 | // forward-declare some types 59 | struct sonar_server_context; 60 | typedef struct sonar_server_context* sonar_server_handle_t; 61 | struct sonar_server_attribute; 62 | typedef struct sonar_server_attribute* sonar_server_attribute_t; 63 | 64 | typedef struct { 65 | // A function which writes a single byte over the physical layer 66 | void (*write_byte)(uint8_t byte); 67 | // A function which gets the current system time in ms 68 | uint64_t (*get_system_time_ms)(void); 69 | // Callback when the connection state changes 70 | void (*connection_changed_callback)(sonar_server_handle_t handle, bool connected); 71 | // Attribute notify complete handler 72 | void (*attribute_notify_complete_handler)(sonar_server_handle_t handle, bool success); 73 | } sonar_server_init_t; 74 | 75 | // Function prototype for attribute read handlers 76 | typedef uint32_t (*sonar_server_attribute_read_handler_t)(void* response_data, uint32_t response_max_size); 77 | 78 | // Function prototype for attribute write handlers 79 | typedef bool (*sonar_server_attribute_write_handler_t)(const void* data, uint32_t length); 80 | 81 | // A wrapper around an attribute for use by a server 82 | struct sonar_server_attribute { 83 | // Allocated space for private context to be used by the SONAR server implementation only 84 | uint8_t _private[sizeof(void*)]; 85 | // The SONAR attribute 86 | sonar_attribute_t attr; 87 | // Read handler for the attribute 88 | sonar_server_attribute_read_handler_t read_handler; 89 | // Write handler for the attribute 90 | sonar_server_attribute_write_handler_t write_handler; 91 | }; 92 | 93 | struct sonar_server_context { 94 | // Allocated space for private context to be used by the SONAR server implementation only 95 | uint8_t _private[_SONAR_SERVER_CONTEXT_SIZE]; 96 | // Receive buffer used by SONAR - should be large enough to store the largest supported attribute plus 6 bytes for overhead 97 | uint8_t* receive_buffer; 98 | // The size of the receive buffer in bytes 99 | uint32_t receive_buffer_size; 100 | }; 101 | 102 | // Initialize the SONAR server 103 | void sonar_server_init(sonar_server_handle_t handle, const sonar_server_init_t* init); 104 | 105 | // The main process function for the SONAR server which gets passed data received since the last call 106 | // This should be called regularly even if there's no received data 107 | void sonar_server_process(sonar_server_handle_t handle, const uint8_t* received_data, uint32_t received_data_length); 108 | 109 | // Returns whether or not a client is connected to the SONAR server 110 | bool sonar_server_is_connected(sonar_server_handle_t handle); 111 | 112 | // Function to register a SONAR server attribute which was defined with `SONAR_SERVER_ATTR_DEF()` 113 | void sonar_server_register(sonar_server_handle_t handle, sonar_server_attribute_t attr); 114 | 115 | // Sends a notify request for the specified attribute 116 | // NOTE: the data passed to this function must remain valid until attribute_notify_complete_handler() is called 117 | bool sonar_server_notify(sonar_server_handle_t handle, sonar_server_attribute_t attr, const void* data, uint32_t length); 118 | 119 | // Sends a notify request for the specified attribute based on the data returned by the attribute_read_handler() 120 | bool sonar_server_notify_read_data(sonar_server_handle_t handle, sonar_server_attribute_t attr); 121 | 122 | // Gets the error counters and then clears them 123 | void sonar_server_get_and_clear_errors(sonar_server_handle_t handle, sonar_errors_t* errors); 124 | -------------------------------------------------------------------------------- /sonar/protobuf/sonar_extensions.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | import "google/protobuf/descriptor.proto"; 4 | 5 | enum AttrOps { 6 | ATTR_OPS_INVALID = 0; 7 | ATTR_OPS_R = 0x1000; 8 | ATTR_OPS_W = 0x2000; 9 | ATTR_OPS_N = 0x4000; 10 | ATTR_OPS_RW = 0x3000; 11 | ATTR_OPS_RN = 0x5000; 12 | ATTR_OPS_WN = 0x6000; 13 | ATTR_OPS_RWN = 0x7000; 14 | } 15 | 16 | message SonarOptions { 17 | // Operations which a SONAR attribute supports 18 | optional AttrOps attr_ops = 1 [default = ATTR_OPS_INVALID]; 19 | } 20 | 21 | extend google.protobuf.MessageOptions { 22 | optional SonarOptions sonar_msgopt = 9910; 23 | } 24 | -------------------------------------------------------------------------------- /sonar/protobuf/sonar_nanopb.mk: -------------------------------------------------------------------------------- 1 | SONAR_NANOPB_BUILD_DIR ?= build 2 | PROTOC ?= protoc 3 | 4 | ifeq (SONAR_PROTO_SOURCES,) 5 | $(error "Must set SONAR_PROTO_SOURCES before including sonar_nanopb.mk") 6 | endif 7 | ifeq (NANOPB_ROOT_DIR,) 8 | $(error "Must set NANOPB_ROOT_DIR before including sonar_nanopb.mk") 9 | endif 10 | 11 | SONAR_PROTO_NANOPB_MAKEFILE_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) 12 | SONAR_NANOPB_PROTO_FLAGS := \ 13 | --plugin=protoc-gen-nanopb=$(NANOPB_ROOT_DIR)/generator/protoc-gen-nanopb \ 14 | --plugin=protoc-gen-sonar=$(abspath $(SONAR_PROTO_NANOPB_MAKEFILE_DIR)/sonar_proto_gen.py) \ 15 | $(addprefix -I,$(abspath $(dir $(SONAR_PROTO_SOURCES)))) 16 | 17 | vpath %.proto $(sort $(dir $(SONAR_PROTO_SOURCES))) 18 | vpath %.pb.c $(SONAR_NANOPB_BUILD_DIR) 19 | 20 | # Rule for building the generated file which nanopb depends on 21 | NANOPB_DEP := $(NANOPB_ROOT_DIR)/generator/proto/nanopb_pb2.py 22 | $(NANOPB_DEP): $(NANOPB_ROOT_DIR)/generator/proto/nanopb.proto $(NANOPB_ROOT_DIR)/generator/proto/plugin.proto 23 | @$(MAKE) -C $(NANOPB_ROOT_DIR)/generator/proto 24 | 25 | # Rule for building the .pb.c file from .proto files 26 | $(SONAR_NANOPB_BUILD_DIR)/%.pb.c: %.proto $(SONAR_NANOPB_PROTO_SOURCES) $(NANOPB_DEP) Makefile | $(SONAR_NANOPB_BUILD_DIR) 27 | @echo "Compiling $(notdir $@)" 28 | @NANOPB_ROOT_DIR=$(NANOPB_ROOT_DIR) $(PROTOC) --nanopb_out=$(dir $@) --sonar_out=c:$(dir $@) $(SONAR_NANOPB_PROTO_FLAGS) $(abspath $<) 29 | 30 | # Create a no-op rule for the generated nanopb headers since they are generated along with the C files 31 | $(SONAR_NANOPB_BUILD_DIR)/%.pb.h: $(SONAR_NANOPB_BUILD_DIR)/%.pb.c 32 | -------------------------------------------------------------------------------- /sonar/protobuf/sonar_proto_gen.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import subprocess 5 | import sys 6 | 7 | if 'NANOPB_ROOT_DIR' not in os.environ: 8 | sys.stderr.write("Must define NANOPB_ROOT_DIR before calling sonar_proto_gen.py\n") 9 | sys.exit(1) 10 | NANOPB_ROOT_DIR = os.path.abspath(os.environ['NANOPB_ROOT_DIR']) 11 | 12 | # Handle msys path weirdness (FIXME: this is hacky) 13 | file_path = __file__.replace("C:", "/c").replace("\\", "/") 14 | 15 | file_dir = os.path.dirname(os.path.realpath(file_path)) 16 | 17 | # Need to import the plugin_pb2 object from nanopb to be able to use its message options 18 | sys.path += [os.path.join(NANOPB_ROOT_DIR, "generator")] 19 | import proto.nanopb_pb2 as nanopb_pb2 20 | import proto.plugin_pb2 as plugin_pb2 21 | if not os.path.exists(os.path.join(file_dir, "build", "sonar_extensions_pb2.py")): 22 | # Automatically build the sonar_extensions_pb2.py file if it doesn't exist 23 | include_paths = [ 24 | os.path.abspath(os.getcwd()), 25 | os.path.join(NANOPB_ROOT_DIR, "generator"), 26 | os.path.join(NANOPB_ROOT_DIR, "generator", "proto"), 27 | file_dir, 28 | ] 29 | os.makedirs(os.path.join(file_dir, 'build'), exist_ok=True) 30 | subprocess.check_call(['protoc', '--python_out=build'] + ['-I' + p for p in include_paths] + ['sonar_extensions.proto'], cwd=file_dir) 31 | sys.path += [os.path.join(file_dir, "build")] 32 | import sonar_extensions_pb2 33 | 34 | def process_request(request): 35 | response = plugin_pb2.CodeGeneratorResponse() 36 | for proto_file in request.proto_file: 37 | if proto_file.name not in request.file_to_generate: 38 | # this is a dependency file, so we don't need to process it 39 | continue 40 | 41 | if request.parameter == 'python': 42 | f = response.file.add() 43 | f.name = proto_file.name.replace('.proto', '_pb2.py') 44 | f.insertion_point = 'module_scope' 45 | f.content = "MESSAGE_ID_TO_CLASS = {}" 46 | 47 | # iterate through each message in this proto file 48 | for message in proto_file.message_type: 49 | if not message.options.HasExtension(nanopb_pb2.nanopb_msgopt): 50 | # ignore messages without the nanopb_msgopt option 51 | continue 52 | elif not message.options.HasExtension(sonar_extensions_pb2.sonar_msgopt): 53 | # ignore messages without the sonar_msgopt option 54 | continue 55 | # grab the msgid option from nanopb 56 | nanopb_options = nanopb_pb2.NanoPBOptions() 57 | nanopb_options.MergeFrom(message.options.Extensions[nanopb_pb2.nanopb_msgopt]) 58 | msg_id = nanopb_options.msgid 59 | # grab our attr_ops option 60 | sonar_options = sonar_extensions_pb2.SonarOptions() 61 | sonar_options.MergeFrom(message.options.Extensions[sonar_extensions_pb2.sonar_msgopt]) 62 | attr_ops = sonar_options.attr_ops 63 | attr_ops_name = sonar_extensions_pb2.AttrOps.Name(attr_ops).replace("ATTR_OPS_", "") 64 | f = response.file.add() 65 | if request.parameter == "java": 66 | if 'SONAR_PROTO_GEN_REQUEST_INFO_JAVA_CLASS' not in os.environ: 67 | sys.stderr.write("Must define SONAR_PROTO_GEN_REQUEST_INFO_JAVA_CLASS before calling sonar_proto_gen.py\n") 68 | sys.exit(1) 69 | request_info_class = os.environ['SONAR_PROTO_GEN_REQUEST_INFO_JAVA_CLASS'] 70 | # generate the path to the target java file path which we'll be modifying 71 | f.name = os.path.join(os.path.join(*proto_file.options.java_package.split('.')), proto_file.package + ".java") 72 | f.insertion_point = "class_scope:" + proto_file.package + "." + message.name 73 | # generate a REQUEST_INFO field 74 | f.content = "" 75 | f.content += "public static %s<%s> REQUEST_INFO;\n"%(request_info_class, message.name) 76 | f.content += "static {\n" 77 | f.content += " REQUEST_INFO = new %s<%s>() {\n"%(request_info_class, message.name) 78 | f.content += " @Override\n" 79 | f.content += " public short getAttributeId() {\n" 80 | f.content += " return 0x%03x;\n"%(msg_id) 81 | f.content += " }\n" 82 | f.content += " @Override\n" 83 | f.content += " public boolean supportsNotify() {\n" 84 | f.content += " return %s;\n"%("true" if "N" in attr_ops_name else "false") 85 | f.content += " }\n" 86 | f.content += " @Override\n" 87 | f.content += " public com.google.protobuf.Parser<%s> getParser() {\n"%(message.name) 88 | f.content += " return parser();\n" 89 | f.content += " }\n" 90 | f.content += " };\n" 91 | f.content += "}\n" 92 | elif request.parameter == "python": 93 | # # generate the path to the target python file path which we'll be modifying 94 | f.name = proto_file.name.replace('.proto', '_pb2.py') 95 | f.insertion_point = "module_scope" 96 | f.content = "MESSAGE_ID_TO_CLASS[%d] = %s" % (msg_id, message.name) 97 | elif request.parameter == "c": 98 | # generate the path to the target C header which we'll be modifying 99 | f.name = proto_file.name.replace(".proto", ".pb.h") 100 | struct_name = proto_file.package + "_" + message.name 101 | f.insertion_point = "struct:" + struct_name 102 | # generate a define for the operations which will get passed to SONAR_ATTR_DEF() 103 | f.content += "#define %s_ops %s\n"%(struct_name, attr_ops_name) 104 | else: 105 | raise Exception("Unknown target language (parameter=%s)"%(request.parameter)) 106 | return response 107 | 108 | def run(): 109 | # Read the request from stdin 110 | if hasattr(sys.stdin, 'buffer'): 111 | data = sys.stdin.buffer.read() 112 | else: 113 | data = sys.stdin.read() 114 | request = plugin_pb2.CodeGeneratorRequest() 115 | request.ParseFromString(data) 116 | 117 | # Get the response and write it to stdout 118 | response = process_request(request) 119 | if hasattr(sys.stdout, 'buffer'): 120 | sys.stdout.buffer.write(response.SerializeToString()) 121 | else: 122 | sys.stdout.write(response.SerializeToString()) 123 | 124 | if __name__ == '__main__': 125 | run() 126 | -------------------------------------------------------------------------------- /sonar/sonar.mk: -------------------------------------------------------------------------------- 1 | SONAR_BASE_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) 2 | 3 | SONAR_C_SOURCES := \ 4 | $(SONAR_BASE_DIR)/src/client.c \ 5 | $(SONAR_BASE_DIR)/src/server.c \ 6 | $(SONAR_BASE_DIR)/src/common/buffer_chain.c \ 7 | $(SONAR_BASE_DIR)/src/common/crc16.c \ 8 | $(SONAR_BASE_DIR)/src/link_layer/link_layer.c \ 9 | $(SONAR_BASE_DIR)/src/link_layer/receive.c \ 10 | $(SONAR_BASE_DIR)/src/link_layer/transmit.c \ 11 | $(SONAR_BASE_DIR)/src/application_layer/application_layer.c \ 12 | $(SONAR_BASE_DIR)/src/attribute/attribute_server.c \ 13 | $(SONAR_BASE_DIR)/src/attribute/attribute_client.c 14 | -------------------------------------------------------------------------------- /sonar/src/application_layer/application_layer.c: -------------------------------------------------------------------------------- 1 | #include "application_layer.h" 2 | 3 | #include "types.h" 4 | 5 | #define LOGGING_MODULE_NAME "SONAR" 6 | #include "anchor/logging/logging.h" 7 | 8 | #include 9 | 10 | typedef struct { 11 | bool is_active; 12 | bool pending_read_response; 13 | sonar_application_layer_header_t header; 14 | buffer_chain_entry_t header_buffer_chain; 15 | buffer_chain_entry_t data_buffer_chain; 16 | } pending_request_info_t; 17 | 18 | typedef struct { 19 | sonar_application_layer_init_t init; 20 | pending_request_info_t request; 21 | } instance_impl_t; 22 | _Static_assert(sizeof(sonar_application_layer_context_t) == sizeof(instance_impl_t), "Invalid context size"); 23 | 24 | static bool issue_request(instance_impl_t* inst, uint16_t attribute_id, uint16_t op, const uint8_t* data, uint32_t length) { 25 | if (inst->request.is_active) { 26 | LOG_ERROR("Application layer request already pending"); 27 | return false; 28 | } else if (attribute_id & SONAR_APPLICATION_ATTRIBUTE_ID_OP_MASK) { 29 | LOG_ERROR("Invalid attribute ID: 0x%x", attribute_id); 30 | return false; 31 | } 32 | 33 | bool is_invalid_op; 34 | switch (op) { 35 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_READ: 36 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_WRITE: 37 | is_invalid_op = inst->init.is_server; 38 | break; 39 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_NOTIFY: 40 | is_invalid_op = !inst->init.is_server; 41 | break; 42 | default: 43 | // should never happen 44 | LOG_ERROR("Invalid application layer operation (%u)", op); 45 | return false; 46 | } 47 | if (is_invalid_op) { 48 | LOG_ERROR("Invalid application layer operation (is_server=%d, op=%u)", inst->init.is_server, op); 49 | return false; 50 | } 51 | 52 | inst->request.is_active = true; 53 | inst->request.header = (sonar_application_layer_header_t) { 54 | .attribute_id = attribute_id | op, 55 | }; 56 | buffer_chain_set_data(&inst->request.data_buffer_chain, data, length); 57 | if (!inst->init.send_data_function(inst->init.send_data_handle, &inst->request.header_buffer_chain)) { 58 | inst->request.is_active = false; 59 | return false; 60 | } 61 | return true; 62 | } 63 | 64 | void sonar_application_layer_init(sonar_application_layer_handle_t handle, const sonar_application_layer_init_t* init) { 65 | instance_impl_t* inst = (instance_impl_t*)handle; 66 | *inst = (instance_impl_t){ 67 | .init = *init, 68 | }; 69 | buffer_chain_set_data(&inst->request.header_buffer_chain, (const uint8_t*)&inst->request.header, sizeof(inst->request.header)); 70 | buffer_chain_push_back(&inst->request.header_buffer_chain, &inst->request.data_buffer_chain); 71 | } 72 | 73 | bool sonar_application_layer_read_request(sonar_application_layer_handle_t handle, uint16_t attribute_id) { 74 | instance_impl_t* inst = (instance_impl_t*)handle; 75 | return issue_request(inst, attribute_id, SONAR_APPLICATION_ATTRIBUTE_ID_OP_READ, NULL, 0); 76 | } 77 | 78 | bool sonar_application_layer_write_request(sonar_application_layer_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 79 | instance_impl_t* inst = (instance_impl_t*)handle; 80 | return issue_request(inst, attribute_id, SONAR_APPLICATION_ATTRIBUTE_ID_OP_WRITE, data, length); 81 | } 82 | 83 | bool sonar_application_layer_notify_request(sonar_application_layer_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 84 | instance_impl_t* inst = (instance_impl_t*)handle; 85 | return issue_request(inst, attribute_id, SONAR_APPLICATION_ATTRIBUTE_ID_OP_NOTIFY, data, length); 86 | } 87 | 88 | bool sonar_application_layer_handle_request(sonar_application_layer_handle_t handle, const uint8_t* data, uint32_t length) { 89 | instance_impl_t* inst = (instance_impl_t*)handle; 90 | if (length < sizeof(sonar_application_layer_header_t)) { 91 | LOG_ERROR("Invalid application layer packet: too short"); 92 | return false; 93 | } 94 | 95 | // grab the header off the front of the data 96 | const sonar_application_layer_header_t* header = (const sonar_application_layer_header_t*)data; 97 | data += sizeof(*header); 98 | length -= sizeof(*header); 99 | 100 | // decode the header and call the corresponding operation handler 101 | const uint16_t op = header->attribute_id & SONAR_APPLICATION_ATTRIBUTE_ID_OP_MASK; 102 | const uint16_t attribute_id = header->attribute_id & SONAR_APPLICATION_ATTRIBUTE_ID_ATTRIBUTE_ID_MASK; 103 | switch (op) { 104 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_READ: { 105 | if (!inst->init.is_server) { 106 | LOG_ERROR("Invalid application layer packet: read request from server"); 107 | return false; 108 | } 109 | if (length) { 110 | LOG_ERROR("Invalid application layer packet: read request with data (%"PRIu32")", length); 111 | return false; 112 | } 113 | inst->request.pending_read_response = true; 114 | const bool success = inst->init.attribute_read_handler(inst->init.attr_handler_handle, attribute_id); 115 | const bool set_response = !inst->request.pending_read_response; 116 | inst->request.pending_read_response = false; 117 | if (!success) { 118 | return false; 119 | } else if (!set_response) { 120 | // should never happen 121 | LOG_ERROR("No read response was set"); 122 | return false; 123 | } 124 | return true; 125 | } 126 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_WRITE: 127 | if (!inst->init.is_server) { 128 | LOG_ERROR("Invalid application layer packet: write request from server"); 129 | return false; 130 | } 131 | if (!inst->init.attribute_write_handler(inst->init.attr_handler_handle, attribute_id, data, length)) { 132 | return false; 133 | } 134 | inst->init.set_response_function(inst->init.send_data_handle, NULL, 0); 135 | return true; 136 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_NOTIFY: 137 | if (inst->init.is_server) { 138 | LOG_ERROR("Invalid application layer packet: notify request from client"); 139 | return false; 140 | } 141 | if (!inst->init.attribute_notify_handler(inst->init.attr_handler_handle, attribute_id, data, length)) { 142 | return false; 143 | } 144 | inst->init.set_response_function(inst->init.send_data_handle, NULL, 0); 145 | return true; 146 | default: 147 | LOG_ERROR("Invalid application layer packet: invalid op (%u)", op); 148 | return false; 149 | } 150 | } 151 | 152 | void sonar_application_layer_handle_response(sonar_application_layer_handle_t handle, bool success, const uint8_t* data, uint32_t length) { 153 | instance_impl_t* inst = (instance_impl_t*)handle; 154 | if (!inst->request.is_active) { 155 | // should never happen 156 | LOG_ERROR("Unexpected response"); 157 | return; 158 | } 159 | inst->request.is_active = false; 160 | const uint16_t attribute_id = inst->request.header.attribute_id & SONAR_APPLICATION_ATTRIBUTE_ID_ATTRIBUTE_ID_MASK; 161 | switch (inst->request.header.attribute_id & SONAR_APPLICATION_ATTRIBUTE_ID_OP_MASK) { 162 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_READ: 163 | inst->init.read_request_complete_handler(inst->init.request_complete_handle, attribute_id, success, data, length); 164 | break; 165 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_WRITE: 166 | inst->init.write_request_complete_handler(inst->init.request_complete_handle, attribute_id, success); 167 | break; 168 | case SONAR_APPLICATION_ATTRIBUTE_ID_OP_NOTIFY: 169 | inst->init.notify_request_complete_handler(inst->init.request_complete_handle, attribute_id, success); 170 | break; 171 | default: 172 | // should never happen 173 | LOG_ERROR("Invalid operation (0x%x)", inst->request.header.attribute_id); 174 | return; 175 | } 176 | } 177 | 178 | void sonar_application_layer_read_response(sonar_application_layer_handle_t handle, const uint8_t* data, uint32_t length) { 179 | instance_impl_t* inst = (instance_impl_t*)handle; 180 | if (!inst->request.pending_read_response) { 181 | LOG_ERROR("Unexpected read response"); 182 | return; 183 | } 184 | inst->request.pending_read_response = false; 185 | inst->init.set_response_function(inst->init.send_data_handle, data, length); 186 | } 187 | -------------------------------------------------------------------------------- /sonar/src/application_layer/application_layer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../common/buffer_chain.h" 4 | 5 | #include 6 | #include 7 | 8 | #define _SONAR_APPLICATION_LAYER_CONTEXT_SIZE ( \ 9 | sizeof(uintptr_t) + /* pending_request_info_t.{is_active,header} */ \ 10 | sizeof(buffer_chain_entry_t) * 2 + /* pending_request_info_t.{header_buffer_chain,data_buffer_chain} */ \ 11 | sizeof(sonar_application_layer_init_t)) 12 | 13 | // Handle type passed to send_data_function() 14 | typedef void* sonar_application_layer_send_data_handle_t; 15 | 16 | // Handle type passed to attribute_*_handler() 17 | typedef void* sonar_application_layer_attribute_handler_handle_t; 18 | 19 | // Handle type passed to *_request_complete_handler() 20 | typedef void* sonar_application_layer_request_complete_handler_handle_t; 21 | 22 | typedef struct { 23 | // Whether or not this is the server (vs. client) 24 | bool is_server; 25 | // Function which is called to write a buffer chain over the physical link 26 | bool (*send_data_function)(sonar_application_layer_send_data_handle_t handle, const buffer_chain_entry_t* data); 27 | // Function which is called to set the response while handling a request 28 | void (*set_response_function)(sonar_application_layer_send_data_handle_t handle, const uint8_t* data, uint32_t length); 29 | // Handle passed to send_data_function() 30 | sonar_application_layer_send_data_handle_t send_data_handle; 31 | // Handler for attribute read requests 32 | // NOTE: This must call sonar_application_layer_read_response() with the response data 33 | bool (*attribute_read_handler)(sonar_application_layer_attribute_handler_handle_t handle, uint16_t attribute_id); 34 | // Handler for attribute write requests 35 | bool (*attribute_write_handler)(sonar_application_layer_attribute_handler_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 36 | // Handler for attribute notify requests 37 | bool (*attribute_notify_handler)(sonar_application_layer_attribute_handler_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 38 | // Handle passed to attribute_*_handler() 39 | sonar_application_layer_attribute_handler_handle_t attr_handler_handle; 40 | // Handler for read request completion 41 | void(*read_request_complete_handler)(sonar_application_layer_request_complete_handler_handle_t handle, uint16_t attribute_id, bool success, const uint8_t* data, uint32_t length); 42 | // Handler for write request completion 43 | void(*write_request_complete_handler)(sonar_application_layer_request_complete_handler_handle_t handle, uint16_t attribute_id, bool success); 44 | // Handler for notify request completion 45 | void(*notify_request_complete_handler)(sonar_application_layer_request_complete_handler_handle_t handle, uint16_t attribute_id, bool success); 46 | // Handle passed to *_request_complete_handler() 47 | sonar_application_layer_request_complete_handler_handle_t request_complete_handle; 48 | } sonar_application_layer_init_t; 49 | 50 | // The handle is a pointer to a pre-allocated context type (to be accessed by the SONAR implementation only) 51 | typedef uint8_t sonar_application_layer_context_t[_SONAR_APPLICATION_LAYER_CONTEXT_SIZE]; 52 | typedef sonar_application_layer_context_t* sonar_application_layer_handle_t; 53 | 54 | // Initializes the sonar application layer code 55 | void sonar_application_layer_init(sonar_application_layer_handle_t handle, const sonar_application_layer_init_t* init); 56 | 57 | // Sends a SONAR application layer read request for a given attribute, with the handler specified in sonar_application_layer_init_t being being called on completion 58 | bool sonar_application_layer_read_request(sonar_application_layer_handle_t handle, uint16_t attribute_id); 59 | 60 | // Sends a SONAR application layer write request for a given attribute, with the handler specified in sonar_application_layer_init_t being called on completion 61 | // NOTE: the data pointer must remain valid until the handler is called 62 | bool sonar_application_layer_write_request(sonar_application_layer_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 63 | 64 | // Sends a SONAR application layer notify request for a given attribute, with the handler specified in sonar_application_layer_init_t being being called on completion 65 | // NOTE: the data pointer must remain valid until the handler is called 66 | bool sonar_application_layer_notify_request(sonar_application_layer_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 67 | 68 | // Handles a received SONAR application layer request, populating the response as applicable 69 | bool sonar_application_layer_handle_request(sonar_application_layer_handle_t handle, const uint8_t* data, uint32_t length); 70 | 71 | // Handles a received SONAR application layer response 72 | void sonar_application_layer_handle_response(sonar_application_layer_handle_t handle, bool success, const uint8_t* data, uint32_t length); 73 | 74 | // Sends a SONAR application layer read response - should only (and must) be called from attribute_read_handler() 75 | void sonar_application_layer_read_response(sonar_application_layer_handle_t handle, const uint8_t* data, uint32_t length); 76 | -------------------------------------------------------------------------------- /sonar/src/application_layer/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define SONAR_APPLICATION_ATTRIBUTE_ID_ATTRIBUTE_ID_MASK 0x0fff 6 | #define SONAR_APPLICATION_ATTRIBUTE_ID_OP_MASK 0xf000 7 | #define SONAR_APPLICATION_ATTRIBUTE_ID_OP_OFFSET 12 8 | #define SONAR_APPLICATION_ATTRIBUTE_ID_OP_READ (1 << SONAR_APPLICATION_ATTRIBUTE_ID_OP_OFFSET) 9 | #define SONAR_APPLICATION_ATTRIBUTE_ID_OP_WRITE (2 << SONAR_APPLICATION_ATTRIBUTE_ID_OP_OFFSET) 10 | #define SONAR_APPLICATION_ATTRIBUTE_ID_OP_NOTIFY (3 << SONAR_APPLICATION_ATTRIBUTE_ID_OP_OFFSET) 11 | 12 | 13 | typedef struct { 14 | uint16_t attribute_id; 15 | } sonar_application_layer_header_t; 16 | -------------------------------------------------------------------------------- /sonar/src/attribute/attribute_server.c: -------------------------------------------------------------------------------- 1 | #include "server.h" 2 | 3 | #include "../application_layer/types.h" 4 | #include "control_helpers.h" 5 | 6 | #define LOGGING_MODULE_NAME "SONAR" 7 | #include "anchor/logging/logging.h" 8 | 9 | #include 10 | 11 | #define GET_CONTEXT(IMPL_PTR) ((attribute_context_t*)(IMPL_PTR)->_private) 12 | 13 | typedef struct { 14 | sonar_attribute_t next; 15 | bool is_registered; 16 | } attribute_context_t; 17 | _Static_assert(sizeof(attribute_context_t) == sizeof(((sonar_attribute_t)0)->_private), "Invalid size"); 18 | 19 | typedef struct { 20 | sonar_attribute_server_init_t init; 21 | sonar_attribute_t attr_list; 22 | CTRL_NUM_ATTRS_TYPE ctrl_num_attrs; 23 | CTRL_ATTR_OFFSET_TYPE ctrl_attr_offset; 24 | CTRL_ATTR_LIST_TYPE ctrl_attr_list; 25 | } instance_impl_t; 26 | _Static_assert(sizeof(instance_impl_t) == sizeof(sonar_attribute_server_context_t), "Invalid context size"); 27 | 28 | static sonar_attribute_t get_attr_by_id(instance_impl_t* inst, uint16_t attribute_id) { 29 | for (sonar_attribute_t attr = inst->attr_list; attr; attr = GET_CONTEXT(attr)->next) { 30 | if (attr->attribute_id == attribute_id) { 31 | return attr; 32 | } 33 | } 34 | return NULL; 35 | } 36 | 37 | static bool validate_attr_for_notify(instance_impl_t* inst, sonar_attribute_t attr) { 38 | if (!attr) { 39 | LOG_ERROR("Unknown attribute"); 40 | return false; 41 | } else if (!(attr->ops & SONAR_ATTRIBUTE_OPS_N)) { 42 | LOG_ERROR("Notify not allowed for attribute (0x%x)", attr->attribute_id); 43 | return false; 44 | } else if (!GET_CONTEXT(attr)->is_registered) { 45 | LOG_ERROR("Attribute not registered"); 46 | return false; 47 | } 48 | return true; 49 | } 50 | 51 | void sonar_attribute_server_init(sonar_attribute_server_handle_t handle, const sonar_attribute_server_init_t* init) { 52 | instance_impl_t* inst = (instance_impl_t*)handle; 53 | *inst = (instance_impl_t){ 54 | .init = *init, 55 | }; 56 | } 57 | 58 | void sonar_attribute_server_register(sonar_attribute_server_handle_t handle, sonar_attribute_t attr) { 59 | instance_impl_t* inst = (instance_impl_t*)handle; 60 | if (!attr) { 61 | // should never happen as these are all setup by SONAR macros 62 | LOG_ERROR("Invalid parameters"); 63 | return; 64 | } else if (attr->attribute_id & SONAR_APPLICATION_ATTRIBUTE_ID_OP_MASK) { 65 | LOG_ERROR("Invalid attribute ID (0x%x)", attr->attribute_id); 66 | return; 67 | } else if (get_attr_by_id(inst, attr->attribute_id)) { 68 | LOG_ERROR("Attribute with this ID (0x%x) already registered", attr->attribute_id); 69 | return; 70 | } 71 | GET_CONTEXT(attr)->is_registered = true; 72 | if (inst->attr_list) { 73 | // add to the front of the list 74 | GET_CONTEXT(attr)->next = inst->attr_list; 75 | inst->attr_list = attr; 76 | } else { 77 | inst->attr_list = attr; 78 | } 79 | inst->ctrl_num_attrs++; 80 | } 81 | 82 | bool sonar_attribute_server_notify(sonar_attribute_server_handle_t handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length) { 83 | instance_impl_t* inst = (instance_impl_t*)handle; 84 | if (!validate_attr_for_notify(inst, attr)) { 85 | return false; 86 | } else if (length > attr->max_size) { 87 | LOG_ERROR("Notify data is too big"); 88 | return false; 89 | } 90 | memcpy(attr->request_buffer, data, length); 91 | return inst->init.send_notify_request_function(inst->init.handle, attr->attribute_id, attr->request_buffer, length); 92 | } 93 | 94 | bool sonar_attribute_server_notify_read_data(sonar_attribute_server_handle_t handle, sonar_attribute_t attr) { 95 | instance_impl_t* inst = (instance_impl_t*)handle; 96 | if (!validate_attr_for_notify(inst, attr)) { 97 | return false; 98 | } else if (!(attr->ops & SONAR_ATTRIBUTE_OPS_R)) { 99 | LOG_ERROR("Read request not supported"); 100 | return false; 101 | } 102 | const uint32_t length = inst->init.read_handler(inst->init.handle, attr, attr->request_buffer, attr->max_size); 103 | if (length > attr->max_size) { 104 | LOG_ERROR("Notify data is too big"); 105 | return false; 106 | } 107 | return inst->init.send_notify_request_function(inst->init.handle, attr->attribute_id, attr->request_buffer, length); 108 | } 109 | 110 | bool sonar_attribute_server_handle_read_request(sonar_attribute_server_handle_t handle, uint16_t attribute_id) { 111 | instance_impl_t* inst = (instance_impl_t*)handle; 112 | // handle control attributes explicitly inline here since they aren't registered 113 | if (attribute_id == CTRL_NUM_ATTRS_ID) { 114 | inst->init.read_response_handler(inst->init.handle, (const uint8_t*)&inst->ctrl_num_attrs, sizeof(inst->ctrl_num_attrs)); 115 | return true; 116 | } else if (attribute_id == CTRL_ATTR_OFFSET_ID) { 117 | inst->init.read_response_handler(inst->init.handle, (const uint8_t*)&inst->ctrl_attr_offset, sizeof(inst->ctrl_attr_offset)); 118 | return true; 119 | } else if (attribute_id == CTRL_ATTR_LIST_ID) { 120 | memset(inst->ctrl_attr_list, 0, sizeof(inst->ctrl_attr_list)); 121 | uint16_t index = 0; 122 | uint16_t offset = inst->ctrl_attr_offset; 123 | for (sonar_attribute_t attr = inst->attr_list; attr && index < CTRL_ATTR_LIST_LENGTH; attr = GET_CONTEXT(attr)->next) { 124 | if (offset > 0) { 125 | // offset past this one 126 | offset--; 127 | continue; 128 | } 129 | inst->ctrl_attr_list[index++] = attr->attribute_id | attr->ops; 130 | } 131 | inst->init.read_response_handler(inst->init.handle, (const uint8_t*)inst->ctrl_attr_list, sizeof(inst->ctrl_attr_list)); 132 | return true; 133 | } 134 | sonar_attribute_t attr = get_attr_by_id(inst, attribute_id); 135 | if (!attr) { 136 | LOG_ERROR("Got read request for unknown attribute (0x%x)", attribute_id); 137 | return false; 138 | } else if (!(attr->ops & SONAR_ATTRIBUTE_OPS_R)) { 139 | LOG_ERROR("Read request not supported for attribute (0x%x)", attribute_id); 140 | return false; 141 | } 142 | const uint32_t response_size = inst->init.read_handler(inst->init.handle, attr, attr->response_buffer, attr->max_size); 143 | inst->init.read_response_handler(inst->init.handle, attr->response_buffer, response_size); 144 | return true; 145 | } 146 | 147 | bool sonar_attribute_server_handle_write_request(sonar_attribute_server_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 148 | instance_impl_t* inst = (instance_impl_t*)handle; 149 | // handle control attributes explicitly inline here 150 | if (attribute_id == CTRL_ATTR_OFFSET_ID) { 151 | // CTRL_ATTR_OFFSET 152 | if (length != sizeof(CTRL_ATTR_OFFSET_TYPE)) { 153 | LOG_ERROR("Invalid request length (%"PRIu32") for CTRL_ATTR_OFFSET", length); 154 | return false; 155 | } 156 | memcpy(&inst->ctrl_attr_offset, data, length); 157 | return true; 158 | } 159 | sonar_attribute_t attr = get_attr_by_id(inst, attribute_id); 160 | if (!attr) { 161 | LOG_ERROR("Got write request for unknown attribute (0x%x)", attribute_id); 162 | return false; 163 | } else if (!(attr->ops & SONAR_ATTRIBUTE_OPS_W)) { 164 | LOG_ERROR("Write request not supported for attribute (0x%x)", attribute_id); 165 | return false; 166 | } else if (length > attr->max_size) { 167 | LOG_ERROR("Write request is too big (%"PRIu32") for attribute (0x%x)", length, attribute_id); 168 | return false; 169 | } 170 | return inst->init.write_handler(inst->init.handle, attr, data, length); 171 | } 172 | 173 | void sonar_attribute_server_handle_notify_response(sonar_attribute_server_handle_t handle, uint16_t attribute_id, bool success) { 174 | instance_impl_t* inst = (instance_impl_t*)handle; 175 | sonar_attribute_t attr = get_attr_by_id(inst, attribute_id); 176 | if (!attr || !(attr->ops & SONAR_ATTRIBUTE_OPS_N)) { 177 | // should never happen 178 | LOG_ERROR("Unexpected notify response"); 179 | return; 180 | } 181 | inst->init.notify_complete_handler(inst->init.handle, success); 182 | } 183 | -------------------------------------------------------------------------------- /sonar/src/attribute/client.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "anchor/sonar/attribute.h" 4 | 5 | #include 6 | #include 7 | 8 | #define _SONAR_ATTRIBUTE_CLIENT_CONTEXT_SIZE \ 9 | (sizeof(sonar_attribute_client_init_t) + sizeof(void*) + sizeof(uint32_t) * 2) 10 | 11 | typedef struct { 12 | bool(*send_read_request_function)(void* handle, uint16_t attribute_id); 13 | bool(*send_write_request_function)(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 14 | void(*connection_changed_callback)(void* handle, bool connected); 15 | void(*read_complete_handler)(void* handle, bool success, const uint8_t* data, uint32_t length); 16 | void(*write_complete_handler)(void* handle, bool success); 17 | bool(*notify_handler)(void* handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length); 18 | void* handle; 19 | } sonar_attribute_client_init_t; 20 | 21 | // The handle is a pointer to a pre-allocated context type (to be accessed by the SONAR implementation only) 22 | typedef uint8_t sonar_attribute_client_context_t[_SONAR_ATTRIBUTE_CLIENT_CONTEXT_SIZE]; 23 | typedef sonar_attribute_client_context_t* sonar_attribute_client_handle_t; 24 | 25 | // Initialize the SONAR attribute client code 26 | void sonar_attribute_client_init(sonar_attribute_client_handle_t, const sonar_attribute_client_init_t* init); 27 | 28 | // Register an implementation for an attribute supported by the client 29 | void sonar_attribute_client_register(sonar_attribute_client_handle_t handle, sonar_attribute_t def); 30 | 31 | // Called when the low-level connection status changes 32 | void sonar_attribute_client_low_level_connection_changed(sonar_attribute_client_handle_t handle, bool is_connected); 33 | 34 | // Returns whether or not the attribute client is currently connected 35 | bool sonar_attribute_client_is_connected(sonar_attribute_client_handle_t handle); 36 | 37 | // Issue a read request for an attribute 38 | bool sonar_attribute_client_read(sonar_attribute_client_handle_t handle, sonar_attribute_t attr); 39 | 40 | // Issue a write request for an attribute 41 | bool sonar_attribute_client_write(sonar_attribute_client_handle_t handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length); 42 | 43 | // Handles a received attribute read response 44 | void sonar_attribute_client_handle_read_response(sonar_attribute_client_handle_t handle, uint16_t attribute_id, bool success, const uint8_t* data, uint32_t length); 45 | 46 | // Handles a received attribute write response 47 | void sonar_attribute_client_handle_write_response(sonar_attribute_client_handle_t handle, uint16_t attribute_id, bool success); 48 | 49 | // Handles a received attribute notify request 50 | bool sonar_attribute_client_handle_notify_request(sonar_attribute_client_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 51 | -------------------------------------------------------------------------------- /sonar/src/attribute/control_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define CTRL_ATTR_LIST_OP_BIT_READ (1 << 12) 6 | #define CTRL_ATTR_LIST_OP_BIT_WRITE (1 << 13) 7 | #define CTRL_ATTR_LIST_OP_BIT_NOTIFY (1 << 14) 8 | #define CTRL_ATTR_LIST_OP_BIT_RESERVED (1 << 15) 9 | 10 | #define CTRL_ATTR_LIST_LENGTH 8 11 | typedef uint16_t ctrl_attr_list_t[CTRL_ATTR_LIST_LENGTH]; 12 | 13 | #define CTRL_NUM_ATTRS_ID 0x101 14 | #define CTRL_ATTR_OFFSET_ID 0x102 15 | #define CTRL_ATTR_LIST_ID 0x103 16 | 17 | #define CTRL_NUM_ATTRS_TYPE uint16_t 18 | #define CTRL_ATTR_OFFSET_TYPE uint16_t 19 | #define CTRL_ATTR_LIST_TYPE ctrl_attr_list_t 20 | -------------------------------------------------------------------------------- /sonar/src/attribute/server.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "anchor/sonar/attribute.h" 4 | 5 | #include 6 | #include 7 | 8 | #define _SONAR_ATTRIBUTE_SERVER_CONTEXT_SIZE \ 9 | (sizeof(sonar_attribute_server_init_t) + sizeof(void*) + sizeof(uintptr_t) + sizeof(uint16_t) * 8) 10 | 11 | typedef struct { 12 | bool (*send_notify_request_function)(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 13 | void (*read_response_handler)(void* handle, const uint8_t* data, uint32_t length); 14 | uint32_t (*read_handler)(void* handle, sonar_attribute_t attr, void* response_data, uint32_t response_max_size); 15 | bool (*write_handler)(void* handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length); 16 | void (*notify_complete_handler)(void* handle, bool success); 17 | void* handle; 18 | } sonar_attribute_server_init_t; 19 | 20 | // The handle is a pointer to a pre-allocated context type (to be accessed by the SONAR implementation only) 21 | typedef uint8_t sonar_attribute_server_context_t[_SONAR_ATTRIBUTE_SERVER_CONTEXT_SIZE]; 22 | typedef sonar_attribute_server_context_t* sonar_attribute_server_handle_t; 23 | 24 | // Initialize the SONAR attribute server code 25 | void sonar_attribute_server_init(sonar_attribute_server_handle_t handle, const sonar_attribute_server_init_t* init); 26 | 27 | // Register an implementation for an attribute supported by the server 28 | void sonar_attribute_server_register(sonar_attribute_server_handle_t handle, sonar_attribute_t attribute); 29 | 30 | // Issue a notify request for an attribute 31 | bool sonar_attribute_server_notify(sonar_attribute_server_handle_t handle, sonar_attribute_t attribute, const uint8_t* data, uint32_t length); 32 | 33 | // Issue a notify request for an attribute, using the data returned by calling the read handlers 34 | bool sonar_attribute_server_notify_read_data(sonar_attribute_server_handle_t handle, sonar_attribute_t attribute); 35 | 36 | // Handles a received attribute read reqyest 37 | bool sonar_attribute_server_handle_read_request(sonar_attribute_server_handle_t handle, uint16_t attribute_id); 38 | 39 | // Handles a received attribute write request 40 | bool sonar_attribute_server_handle_write_request(sonar_attribute_server_handle_t handle, uint16_t attribute_id, const uint8_t* data, uint32_t length); 41 | 42 | // Handles a received attribute notify response 43 | void sonar_attribute_server_handle_notify_response(sonar_attribute_server_handle_t handle, uint16_t attribute_id, bool success); 44 | -------------------------------------------------------------------------------- /sonar/src/client.c: -------------------------------------------------------------------------------- 1 | #include "anchor/sonar/client.h" 2 | 3 | #include "link_layer/link_layer.h" 4 | #include "application_layer/application_layer.h" 5 | #include "attribute/client.h" 6 | 7 | #define LOGGING_MODULE_NAME "SONAR" 8 | #include "anchor/logging/logging.h" 9 | 10 | typedef struct { 11 | sonar_client_init_t init; 12 | sonar_link_layer_context_t link_layer_context; 13 | sonar_application_layer_context_t application_layer_context; 14 | sonar_attribute_client_context_t attr_client_context; 15 | sonar_link_layer_handle_t link_layer_handle; 16 | sonar_application_layer_handle_t application_layer_handle; 17 | sonar_attribute_client_handle_t attr_client_handle; 18 | } instance_impl_t; 19 | _Static_assert(sizeof(instance_impl_t) == sizeof(((sonar_client_context_t*)0)->_private), "Invalid context size"); 20 | 21 | static void link_layer_connection_changed_handler(void* handle, bool connected) { 22 | instance_impl_t* inst = handle; 23 | return sonar_attribute_client_low_level_connection_changed(inst->attr_client_handle, connected); 24 | } 25 | 26 | static bool link_layer_request_handler(void* handle, const uint8_t* data, uint32_t length) { 27 | instance_impl_t* inst = handle; 28 | return sonar_application_layer_handle_request(inst->application_layer_handle, data, length); 29 | } 30 | 31 | static void link_layer_response_handler(void* handle, bool success, const uint8_t* data, uint32_t length) { 32 | instance_impl_t* inst = handle; 33 | sonar_application_layer_handle_response(inst->application_layer_handle, success, data, length); 34 | } 35 | 36 | static bool application_layer_send_data_function(void* handle, const buffer_chain_entry_t* data) { 37 | return sonar_link_layer_send_request(handle, data); 38 | } 39 | 40 | static void application_layer_set_response_function(void* handle, const uint8_t* data, uint32_t length) { 41 | return sonar_link_layer_set_response(handle, data, length); 42 | } 43 | 44 | static bool application_layer_attribute_read_handler(void* handle, uint16_t attribute_id) { 45 | // the client should never got read requests 46 | LOG_ERROR("Got unexpected attribute read request (0x%x)", attribute_id); 47 | return false; 48 | } 49 | 50 | static bool application_layer_attribute_write_handler(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 51 | // the client should never got write requests 52 | LOG_ERROR("Got unexpected attribute write request (0x%x)", attribute_id); 53 | return false; 54 | } 55 | 56 | static bool application_layer_attribute_notify_handler(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 57 | return sonar_attribute_client_handle_notify_request(handle, attribute_id, data, length); 58 | } 59 | 60 | static void attribute_client_handle_read_response(void* handle, uint16_t attribute_id, bool success, const uint8_t* data, uint32_t length) { 61 | sonar_attribute_client_handle_read_response(handle, attribute_id, success, data, length); 62 | } 63 | 64 | static bool attribute_client_send_read_request_function(void* handle, uint16_t attribute_id) { 65 | instance_impl_t* inst = handle; 66 | return sonar_application_layer_read_request(inst->application_layer_handle, attribute_id); 67 | } 68 | 69 | static void attribute_client_handle_write_response(void* handle, uint16_t attribute_id, bool success) { 70 | sonar_attribute_client_handle_write_response(handle, attribute_id, success); 71 | } 72 | 73 | static bool attribute_client_send_write_request_function(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 74 | instance_impl_t* inst = handle; 75 | return sonar_application_layer_write_request(inst->application_layer_handle, attribute_id, data, length); 76 | } 77 | 78 | static void attribute_client_connection_changed_callback(void* handle, bool connected) { 79 | instance_impl_t* inst = handle; 80 | inst->init.connection_changed_callback(connected); 81 | } 82 | 83 | static void attribute_client_read_complete_handler(void* handle, bool success, const uint8_t* data, uint32_t length) { 84 | instance_impl_t* inst = handle; 85 | inst->init.attribute_read_complete_handler(success, data, length); 86 | } 87 | 88 | static void attribute_client_write_complete_handler(void* handle, bool success) { 89 | instance_impl_t* inst = handle; 90 | inst->init.attribute_write_complete_handler(success); 91 | } 92 | 93 | static bool attribute_client_notify_handler(void* handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length) { 94 | instance_impl_t* inst = handle; 95 | return inst->init.attribute_notify_handler(attr, data, length); 96 | } 97 | 98 | void sonar_client_init(sonar_client_handle_t handle, const sonar_client_init_t* init) { 99 | instance_impl_t* inst = (instance_impl_t*)handle; 100 | *inst = (instance_impl_t){ 101 | .init = *init, 102 | .link_layer_handle = &inst->link_layer_context, 103 | .application_layer_handle = &inst->application_layer_context, 104 | .attr_client_handle = &inst->attr_client_context, 105 | }; 106 | const sonar_link_layer_init_t init_link_layer = { 107 | .config = { 108 | .is_server = false, 109 | }, 110 | .buffers = { 111 | .receive = handle->receive_buffer, 112 | .receive_size = handle->receive_buffer_size, 113 | }, 114 | .functions = { 115 | .get_system_time_ms = init->get_system_time_ms, 116 | .write_byte = init->write_byte, 117 | }, 118 | .handlers = { 119 | .connection_changed = link_layer_connection_changed_handler, 120 | .request = link_layer_request_handler, 121 | .request_complete = link_layer_response_handler, 122 | .handler_handle = inst, 123 | }, 124 | }; 125 | sonar_link_layer_init(inst->link_layer_handle, &init_link_layer); 126 | 127 | const sonar_attribute_client_init_t init_attr_client = { 128 | .send_read_request_function = attribute_client_send_read_request_function, 129 | .send_write_request_function = attribute_client_send_write_request_function, 130 | .connection_changed_callback = attribute_client_connection_changed_callback, 131 | .read_complete_handler = attribute_client_read_complete_handler, 132 | .write_complete_handler = attribute_client_write_complete_handler, 133 | .notify_handler = attribute_client_notify_handler, 134 | .handle = inst, 135 | }; 136 | sonar_attribute_client_init(inst->attr_client_handle, &init_attr_client); 137 | 138 | const sonar_application_layer_init_t init_application_layer = { 139 | .is_server = false, 140 | .send_data_function = application_layer_send_data_function, 141 | .set_response_function = application_layer_set_response_function, 142 | .send_data_handle = inst->link_layer_handle, 143 | .attribute_read_handler = application_layer_attribute_read_handler, 144 | .attribute_write_handler = application_layer_attribute_write_handler, 145 | .attribute_notify_handler = application_layer_attribute_notify_handler, 146 | .attr_handler_handle = inst->attr_client_handle, 147 | 148 | .read_request_complete_handler = attribute_client_handle_read_response, 149 | .write_request_complete_handler = attribute_client_handle_write_response, 150 | .request_complete_handle = inst->attr_client_handle, 151 | }; 152 | sonar_application_layer_init(inst->application_layer_handle, &init_application_layer); 153 | } 154 | 155 | bool sonar_client_is_connected(sonar_client_handle_t handle) { 156 | instance_impl_t* inst = (instance_impl_t*)handle; 157 | return sonar_attribute_client_is_connected(inst->attr_client_handle); 158 | } 159 | 160 | void sonar_client_process(sonar_client_handle_t handle, const uint8_t* received_data, uint32_t received_data_length) { 161 | instance_impl_t* inst = (instance_impl_t*)handle; 162 | sonar_link_layer_handle_receive_data(inst->link_layer_handle, received_data, received_data_length); 163 | sonar_link_layer_process(inst->link_layer_handle); 164 | } 165 | 166 | void sonar_client_register(sonar_client_handle_t handle, sonar_attribute_t attr) { 167 | instance_impl_t* inst = ((instance_impl_t*)handle); 168 | sonar_attribute_client_register(inst->attr_client_handle, attr); 169 | } 170 | 171 | bool sonar_client_read(sonar_client_handle_t handle, sonar_attribute_t attr) { 172 | instance_impl_t* inst = (instance_impl_t*)handle; 173 | return sonar_attribute_client_read(inst->attr_client_handle, attr); 174 | } 175 | 176 | bool sonar_client_write(sonar_client_handle_t handle, sonar_attribute_t attr, const void* data, uint32_t length) { 177 | instance_impl_t* inst = (instance_impl_t*)handle; 178 | return sonar_attribute_client_write(inst->attr_client_handle, attr, data, length); 179 | } 180 | 181 | void sonar_client_get_and_clear_errors(sonar_client_handle_t handle, sonar_errors_t* errors) { 182 | instance_impl_t* inst = (instance_impl_t*)handle; 183 | sonar_link_layer_errors_t link_layer_errors; 184 | sonar_link_layer_receive_errors_t link_layer_receive_errors; 185 | sonar_link_layer_get_and_clear_errors(inst->link_layer_handle, &link_layer_errors, &link_layer_receive_errors); 186 | *errors = (sonar_errors_t){ 187 | .link_layer_receive = { 188 | .invalid_header = link_layer_receive_errors.invalid_header, 189 | .invalid_crc = link_layer_receive_errors.invalid_crc, 190 | .buffer_overflow = link_layer_receive_errors.buffer_overflow, 191 | .invalid_escape_sequence = link_layer_receive_errors.invalid_escape_sequence, 192 | }, 193 | .link_layer = { 194 | .invalid_packet = link_layer_errors.invalid_packet, 195 | .unexpected_packet = link_layer_errors.unexpected_packet, 196 | .invalid_sequence_number = link_layer_errors.invalid_sequence_number, 197 | .retries = link_layer_errors.retries, 198 | }, 199 | }; 200 | } 201 | -------------------------------------------------------------------------------- /sonar/src/common/buffer_chain.c: -------------------------------------------------------------------------------- 1 | #include "buffer_chain.h" 2 | 3 | void buffer_chain_set_data(buffer_chain_entry_t* entry, const uint8_t* data, uint32_t length) { 4 | entry->data = data; 5 | entry->length = length; 6 | } 7 | 8 | void buffer_chain_push_back(buffer_chain_entry_t* chain, buffer_chain_entry_t* to_insert) { 9 | while (chain->next) { 10 | chain = chain->next; 11 | } 12 | chain->next = to_insert; 13 | } 14 | -------------------------------------------------------------------------------- /sonar/src/common/buffer_chain.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define FOREACH_BUFFER_CHAIN_ENTRY(START_PTR, ENTRY_PTR_NAME) \ 6 | for (const buffer_chain_entry_t* ENTRY_PTR_NAME = START_PTR; ENTRY_PTR_NAME; ENTRY_PTR_NAME = (ENTRY_PTR_NAME)->next) 7 | 8 | typedef struct buffer_chain_entry { 9 | // Next entry in the chain 10 | struct buffer_chain_entry* next; 11 | // The data buffer represented by this entry 12 | const uint8_t* data; 13 | // The length of the data buffer represented by this entry 14 | uint32_t length; 15 | } buffer_chain_entry_t; 16 | 17 | // Sets the data represented by a buffer chain entry 18 | void buffer_chain_set_data(buffer_chain_entry_t* entry, const uint8_t* data, uint32_t length); 19 | 20 | // Inserts a new entry at the end of the chain 21 | void buffer_chain_push_back(buffer_chain_entry_t* chain, buffer_chain_entry_t* to_insert); 22 | -------------------------------------------------------------------------------- /sonar/src/common/crc16.c: -------------------------------------------------------------------------------- 1 | #include "crc16.h" 2 | 3 | uint16_t crc16(const uint8_t* data, uint32_t length, uint16_t crc) { 4 | for (uint32_t i = 0; i < length; i++) { 5 | uint8_t x = (crc >> 8) ^ data[i]; 6 | x ^= x >> 4; 7 | crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x; 8 | } 9 | return crc; 10 | } 11 | -------------------------------------------------------------------------------- /sonar/src/common/crc16.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define CRC16_INITIAL_VALUE UINT16_C(0xffff) 6 | 7 | uint16_t crc16(const uint8_t* data, uint32_t length, uint16_t crc); 8 | -------------------------------------------------------------------------------- /sonar/src/link_layer/link_layer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "receive.h" 4 | #include "transmit.h" 5 | #include "../common/buffer_chain.h" 6 | 7 | #include 8 | #include 9 | 10 | #define _SONAR_LINK_LAYER_CONTEXT_SIZE ( \ 11 | sizeof(sonar_link_layer_init_t) + \ 12 | sizeof(sonar_link_layer_errors_t) + \ 13 | sizeof(sonar_link_layer_receive_context_t) + \ 14 | sizeof(sonar_link_layer_transmit_context_t) + \ 15 | sizeof(sonar_link_layer_receive_handle_t) + \ 16 | sizeof(sonar_link_layer_transmit_handle_t) + \ 17 | sizeof(buffer_chain_entry_t) + \ 18 | sizeof(uint64_t) * 2 + \ 19 | sizeof(uint64_t) * 4 + \ 20 | sizeof(uintptr_t) + sizeof(uint64_t) * 2 + sizeof(void*) + \ 21 | sizeof(uint32_t) * 2 + sizeof(void*) + \ 22 | sizeof(uintptr_t)) 23 | 24 | typedef struct { 25 | struct { 26 | // Whether or not this is the server (vs. client) 27 | bool is_server; 28 | } config; 29 | struct { 30 | // Buffer used to receive data into by the link layer receive code 31 | // Should be as big as the largest supported data segment received by the link layer (after decoding) plus 4 bytes (for the link layer header / footer) 32 | uint8_t* receive; 33 | // Size of the `receive` buffer in bytes 34 | uint32_t receive_size; 35 | } buffers; 36 | struct { 37 | // Function which returns the current system time in ms 38 | uint64_t (*get_system_time_ms)(void); 39 | // Function which is called to write data over the physical link 40 | void (*write_byte)(uint8_t byte); 41 | } functions; 42 | struct { 43 | // Function which is called when the connection state changes 44 | void (*connection_changed)(void* handle, bool connected); 45 | // Function which is called when a request is received 46 | // This function should return false on error or return true and populate the response as appropriate 47 | // NOTE: On success, this callback MUST call sonar_link_layer_set_response() with the response data (or NULL/0) 48 | bool (*request)(void* handle, const uint8_t* data, uint32_t length); 49 | // Function which is called when a request (issued via sonar_link_layer_send_request()) completes 50 | void (*request_complete)(void* handle, bool success, const uint8_t* data, uint32_t length); 51 | // Handle which is passed to the handlers 52 | void* handler_handle; 53 | } handlers; 54 | } sonar_link_layer_init_t; 55 | 56 | typedef struct { 57 | // Invalid packets received 58 | uint32_t invalid_packet; 59 | // Packets received when none were expected 60 | uint32_t unexpected_packet; 61 | // Packets with invalid sequence numbers 62 | uint32_t invalid_sequence_number; 63 | // Transmission retries 64 | uint32_t retries; 65 | } sonar_link_layer_errors_t; 66 | 67 | // The handle is a pointer to a pre-allocated context type (to be accessed by the SONAR implementation only) 68 | typedef uint8_t sonar_link_layer_context_t[_SONAR_LINK_LAYER_CONTEXT_SIZE]; 69 | typedef sonar_link_layer_context_t* sonar_link_layer_handle_t; 70 | 71 | // Initializes the link layer 72 | void sonar_link_layer_init(sonar_link_layer_handle_t handle, const sonar_link_layer_init_t* init); 73 | 74 | // Returns whether or not we're currently connected 75 | bool sonar_link_layer_is_connected(sonar_link_layer_handle_t handle); 76 | 77 | // Processes received data 78 | void sonar_link_layer_handle_receive_data(sonar_link_layer_handle_t handle, const uint8_t* data, uint32_t length); 79 | 80 | // NOTE: If this returns true, `data` must remain valid and stable until the request_complete() callback is called 81 | bool sonar_link_layer_send_request(sonar_link_layer_handle_t handle, const buffer_chain_entry_t* data); 82 | 83 | // Run link layer processing (should be called regularly - ideally at least every 1ms) 84 | void sonar_link_layer_process(sonar_link_layer_handle_t handle); 85 | 86 | // Sets the SONAR link layer response - should only (and must) be called from handlers.request() 87 | void sonar_link_layer_set_response(sonar_link_layer_handle_t handle, const uint8_t* data, uint32_t length); 88 | 89 | // Get and then clear the current error counters 90 | void sonar_link_layer_get_and_clear_errors(sonar_link_layer_handle_t handle, sonar_link_layer_errors_t* errors, sonar_link_layer_receive_errors_t* receive_errors); 91 | -------------------------------------------------------------------------------- /sonar/src/link_layer/receive.c: -------------------------------------------------------------------------------- 1 | #include "receive.h" 2 | 3 | #include "../common/crc16.h" 4 | #include "types.h" 5 | 6 | #define LOGGING_MODULE_NAME "SONAR" 7 | #include "anchor/logging/logging.h" 8 | 9 | #include 10 | #include 11 | 12 | typedef struct { 13 | sonar_link_layer_receive_init_t init; 14 | sonar_link_layer_receive_errors_t errors; 15 | uint32_t received_len; 16 | bool packet_started; 17 | bool escaping; 18 | } instance_impl_t; 19 | _Static_assert(sizeof(sonar_link_layer_receive_context_t) == sizeof(instance_impl_t), "Invalid context size"); 20 | 21 | static void process_packet(instance_impl_t* inst) { 22 | if (inst->received_len < (sizeof(sonar_link_layer_header_t) + sizeof(sonar_link_layer_footer_t))) { 23 | return; 24 | } 25 | 26 | const uint32_t data_length = inst->received_len - sizeof(sonar_link_layer_header_t) - sizeof(sonar_link_layer_footer_t); 27 | const sonar_link_layer_header_t* header = (const sonar_link_layer_header_t*)inst->init.buffer; 28 | const sonar_link_layer_footer_t* footer = (const sonar_link_layer_footer_t*)&inst->init.buffer[sizeof(*header) + data_length]; 29 | 30 | const uint8_t version = (header->flags & SONAR_LINK_LAYER_FLAGS_VERSION_MASK) >> SONAR_LINK_LAYER_FLAGS_VERSION_OFFSET; 31 | const uint16_t calculated_crc = crc16(inst->init.buffer, sizeof(sonar_link_layer_header_t) + data_length, CRC16_INITIAL_VALUE); 32 | const bool is_server_to_client = header->flags & SONAR_LINK_LAYER_FLAGS_DIRECTION_MASK; 33 | 34 | if (header->flags & SONAR_LINK_LAYER_FLAGS_RESERVED_MASK) { 35 | LOG_ERROR("Invalid packet: bad reserved bits"); 36 | inst->errors.invalid_header++; 37 | return; 38 | } else if (version != SONAR_VERSION) { 39 | LOG_ERROR("Invalid packet: bad version"); 40 | inst->errors.invalid_header++; 41 | return; 42 | } else if (footer->crc != calculated_crc) { 43 | LOG_ERROR("Invalid packet: bad CRC"); 44 | inst->errors.invalid_crc++; 45 | return; 46 | } else if (is_server_to_client == inst->init.is_server) { 47 | LOG_ERROR("Invalid packet: wrong direction"); 48 | inst->errors.invalid_header++; 49 | return; 50 | } 51 | 52 | const bool is_response = header->flags & SONAR_LINK_LAYER_FLAGS_RESPONSE_MASK; 53 | const bool is_link_control = header->flags & SONAR_LINK_LAYER_FLAGS_LINK_CONTROL_MASK; 54 | inst->init.packet_handler(inst->init.handler_handle, is_response, is_link_control, header->sequence_num, &inst->init.buffer[sizeof(*header)], data_length); 55 | } 56 | 57 | static void store_byte(instance_impl_t* inst, uint8_t byte) { 58 | if (inst->received_len < inst->init.buffer_size) { 59 | inst->init.buffer[inst->received_len++] = byte; 60 | } else { 61 | // buffer overflowed, so drop this packet and wait for the next flag byte 62 | LOG_ERROR("Invalid packet: overflowed buffer"); 63 | inst->errors.buffer_overflow++; 64 | inst->packet_started = false; 65 | inst->received_len = 0; 66 | } 67 | } 68 | 69 | static void receive_byte(instance_impl_t* inst, uint8_t byte) { 70 | if (inst->packet_started) { 71 | if (inst->escaping) { 72 | inst->escaping = false; 73 | if (byte == SONAR_ENCODING_ESCAPE_BYTE || byte == SONAR_ENCODING_FLAG_BYTE) { 74 | // illegal escape sequence, so drop the current packet 75 | LOG_ERROR("Illegal escape sequence in data"); 76 | inst->errors.invalid_escape_sequence++; 77 | inst->packet_started = false; 78 | inst->received_len = 0; 79 | } else { 80 | store_byte(inst, byte ^ SONAR_ENCODING_ESCAPE_XOR); 81 | } 82 | } else { 83 | if (byte == SONAR_ENCODING_ESCAPE_BYTE) { 84 | // escape the next byte and ignore this one 85 | inst->escaping = true; 86 | } else if (byte != SONAR_ENCODING_FLAG_BYTE) { 87 | store_byte(inst, byte); 88 | } 89 | } 90 | } else { 91 | inst->escaping = false; 92 | } 93 | 94 | if (byte == SONAR_ENCODING_FLAG_BYTE) { 95 | // a flag byte is always the end of the current packet and the start of a new packet 96 | process_packet(inst); 97 | inst->packet_started = true; 98 | inst->received_len = 0; 99 | } 100 | } 101 | 102 | void sonar_link_layer_receive_init(sonar_link_layer_receive_handle_t handle, const sonar_link_layer_receive_init_t* init) { 103 | instance_impl_t* inst = (instance_impl_t*)handle; 104 | *inst = (instance_impl_t){ 105 | .init = *init, 106 | .packet_started = false, 107 | .escaping = false, 108 | .received_len = 0, 109 | }; 110 | } 111 | 112 | void sonar_link_layer_receive_process_data(sonar_link_layer_receive_handle_t handle, const uint8_t* data, uint32_t length) { 113 | instance_impl_t* inst = (instance_impl_t*)handle; 114 | while (length--) { 115 | receive_byte(inst, *data++); 116 | } 117 | } 118 | 119 | void sonar_link_layer_receive_get_and_clear_errors(sonar_link_layer_receive_handle_t handle, sonar_link_layer_receive_errors_t* errors) { 120 | instance_impl_t* inst = (instance_impl_t*)handle; 121 | *errors = inst->errors; 122 | inst->errors = (sonar_link_layer_receive_errors_t){0}; 123 | } 124 | -------------------------------------------------------------------------------- /sonar/src/link_layer/receive.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #define _SONAR_LINK_LAYER_RECEIVE_CONTEXT_SIZE \ 7 | (sizeof(uint32_t) * 2 + sizeof(sonar_link_layer_receive_init_t) + sizeof(sonar_link_layer_receive_errors_t)) 8 | 9 | typedef struct { 10 | // Whether or not this is the server (vs. client) 11 | bool is_server; 12 | // Buffer which is used internally to store incoming packets 13 | // Should be as big as the largest supported data segment received by the link layer (after decoding) plus 4 bytes (for the link layer header / footer) 14 | uint8_t* buffer; 15 | // Size of `buffer` in bytes 16 | uint32_t buffer_size; 17 | // Function which is called with complete SONAR link layer packets upon receipt 18 | void (*packet_handler)(void* handle, bool is_response, bool is_link_control, uint8_t sequence_num, const uint8_t* data, uint32_t length); 19 | // Handle which is passed to packet_handler() 20 | void* handler_handle; 21 | } sonar_link_layer_receive_init_t; 22 | 23 | typedef struct { 24 | // Invalid packets (bad header) 25 | uint32_t invalid_header; 26 | // Invalid packets (bad CRC) 27 | uint32_t invalid_crc; 28 | // Receive buffer overflows 29 | uint32_t buffer_overflow; 30 | // Invalid HDLC escape sequences 31 | uint32_t invalid_escape_sequence; 32 | } sonar_link_layer_receive_errors_t; 33 | 34 | // The handle is a pointer to a pre-allocated context type (to be accessed by the SONAR implementation only) 35 | typedef uint8_t sonar_link_layer_receive_context_t[_SONAR_LINK_LAYER_RECEIVE_CONTEXT_SIZE]; 36 | typedef sonar_link_layer_receive_context_t* sonar_link_layer_receive_handle_t; 37 | 38 | // Initializes the SONAR link layer receive code 39 | void sonar_link_layer_receive_init(sonar_link_layer_receive_handle_t handle, const sonar_link_layer_receive_init_t* init); 40 | 41 | // Processes received SONAR data 42 | void sonar_link_layer_receive_process_data(sonar_link_layer_receive_handle_t handle, const uint8_t* data, uint32_t length); 43 | 44 | // Get and then clear the current error counters 45 | void sonar_link_layer_receive_get_and_clear_errors(sonar_link_layer_receive_handle_t handle, sonar_link_layer_receive_errors_t* errors); 46 | -------------------------------------------------------------------------------- /sonar/src/link_layer/timeouts.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // How long before we disconnect if we haven't received a response. A connection maintenance 4 | // message is sent by the client at half this interval. 5 | #define CONNECTION_TIMEOUT_MS 1000 6 | #define CONNECTION_MAINTENANCE_INTERVAL_MS 500 7 | #define REQUEST_RETRY_INTERVAL_MS 100 8 | #define REQUEST_TIMEOUT_MS 300 9 | 10 | // Make sure that we have enough time to send a connection maintenance request and get the 11 | // response before disconnecting, using the retry interval as an extra buffer. 12 | #if CONNECTION_TIMEOUT_MS < CONNECTION_MAINTENANCE_INTERVAL_MS + REQUEST_TIMEOUT_MS + REQUEST_RETRY_INTERVAL_MS 13 | #error "The connection timeout must be at least double the request timeout" 14 | #endif 15 | -------------------------------------------------------------------------------- /sonar/src/link_layer/transmit.c: -------------------------------------------------------------------------------- 1 | #include "transmit.h" 2 | 3 | #include "../common/crc16.h" 4 | #include "types.h" 5 | 6 | typedef struct { 7 | sonar_link_layer_transmit_init_t init; 8 | } instance_impl_t; 9 | _Static_assert(sizeof(instance_impl_t) == sizeof(sonar_link_layer_transmit_context_t), "Invalid context size"); 10 | 11 | static void write_encoded_bytes(instance_impl_t* inst, const uint8_t* data, uint32_t length) { 12 | for (uint32_t i = 0; i < length; i++) { 13 | uint8_t byte = data[i]; 14 | if (byte == SONAR_ENCODING_FLAG_BYTE || byte == SONAR_ENCODING_ESCAPE_BYTE) { 15 | inst->init.write_byte_function(SONAR_ENCODING_ESCAPE_BYTE); 16 | byte ^= SONAR_ENCODING_ESCAPE_XOR; 17 | } 18 | inst->init.write_byte_function(byte); 19 | } 20 | } 21 | 22 | void sonar_link_layer_transmit_init(sonar_link_layer_transmit_handle_t handle, const sonar_link_layer_transmit_init_t* init) { 23 | instance_impl_t* inst = (instance_impl_t*)handle; 24 | *inst = (instance_impl_t){ 25 | .init = *init, 26 | }; 27 | } 28 | 29 | void sonar_link_layer_transmit_send_packet(sonar_link_layer_transmit_handle_t handle, bool is_response, bool is_link_control, uint8_t sequence_num, const buffer_chain_entry_t* data) { 30 | instance_impl_t* inst = (instance_impl_t*)handle; 31 | uint16_t crc = CRC16_INITIAL_VALUE; 32 | 33 | // write the starting flag byte 34 | inst->init.write_byte_function(SONAR_ENCODING_FLAG_BYTE); 35 | 36 | // write the header 37 | const sonar_link_layer_header_t header = { 38 | .flags = (SONAR_VERSION << SONAR_LINK_LAYER_FLAGS_VERSION_OFFSET) | 39 | (is_link_control ? SONAR_LINK_LAYER_FLAGS_LINK_CONTROL_MASK : 0) | 40 | (inst->init.is_server ? SONAR_LINK_LAYER_FLAGS_DIRECTION_MASK : 0) | 41 | (is_response ? SONAR_LINK_LAYER_FLAGS_RESPONSE_MASK : 0), 42 | .sequence_num = sequence_num, 43 | }; 44 | write_encoded_bytes(inst, (const uint8_t*)&header, sizeof(header)); 45 | crc = crc16((const uint8_t*)&header, sizeof(header), crc); 46 | 47 | // write the data 48 | FOREACH_BUFFER_CHAIN_ENTRY(data, entry) { 49 | write_encoded_bytes(inst, entry->data, entry->length); 50 | crc = crc16(entry->data, entry->length, crc); 51 | } 52 | 53 | // write the footer 54 | const sonar_link_layer_footer_t footer = { 55 | .crc = crc, 56 | }; 57 | write_encoded_bytes(inst, (const uint8_t*)&footer, sizeof(footer)); 58 | 59 | // write the ending flag byte 60 | inst->init.write_byte_function(SONAR_ENCODING_FLAG_BYTE); 61 | } 62 | -------------------------------------------------------------------------------- /sonar/src/link_layer/transmit.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../common/buffer_chain.h" 4 | 5 | #include 6 | #include 7 | 8 | #define _SONAR_LINK_LAYER_TRANSMIT_CONTEXT_SIZE \ 9 | (sizeof(sonar_link_layer_transmit_init_t)) 10 | 11 | typedef struct { 12 | // Whether or not this is the server (vs. client) 13 | bool is_server; 14 | // Function which is called to write a byte of data over the physical link 15 | void (*write_byte_function)(uint8_t byte); 16 | } sonar_link_layer_transmit_init_t; 17 | 18 | 19 | // The handle is a pointer to a pre-allocated context type (to be accessed by the SONAR implementation only) 20 | typedef uint8_t sonar_link_layer_transmit_context_t[_SONAR_LINK_LAYER_TRANSMIT_CONTEXT_SIZE]; 21 | typedef sonar_link_layer_transmit_context_t* sonar_link_layer_transmit_handle_t; 22 | 23 | // Initializes the SONAR link layer transmit code 24 | void sonar_link_layer_transmit_init(sonar_link_layer_transmit_handle_t handle, const sonar_link_layer_transmit_init_t* init); 25 | 26 | // Transmits a SONAR link layer packet 27 | void sonar_link_layer_transmit_send_packet(sonar_link_layer_transmit_handle_t handle, bool is_response, bool is_link_control, uint8_t sequence_num, const buffer_chain_entry_t* data); 28 | -------------------------------------------------------------------------------- /sonar/src/link_layer/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #define SONAR_VERSION 1 7 | 8 | #define SONAR_ENCODING_FLAG_BYTE 0x7E 9 | #define SONAR_ENCODING_ESCAPE_BYTE 0x7D 10 | #define SONAR_ENCODING_ESCAPE_XOR 0x20 11 | 12 | #define SONAR_LINK_LAYER_FLAGS_RESPONSE_MASK (1 << 0) 13 | #define SONAR_LINK_LAYER_FLAGS_DIRECTION_MASK (1 << 1) 14 | #define SONAR_LINK_LAYER_FLAGS_LINK_CONTROL_MASK (1 << 2) 15 | #define SONAR_LINK_LAYER_FLAGS_RESERVED_MASK (1 << 3) 16 | #define SONAR_LINK_LAYER_FLAGS_VERSION_MASK 0xf0 17 | #define SONAR_LINK_LAYER_FLAGS_VERSION_OFFSET 4 18 | 19 | #pragma pack(push, 1) 20 | 21 | typedef struct { 22 | uint8_t flags; 23 | uint8_t sequence_num; 24 | } sonar_link_layer_header_t; 25 | 26 | typedef struct { 27 | uint16_t crc; 28 | } sonar_link_layer_footer_t; 29 | 30 | #pragma pack(pop) 31 | -------------------------------------------------------------------------------- /sonar/src/server.c: -------------------------------------------------------------------------------- 1 | #include "anchor/sonar/server.h" 2 | 3 | #include "link_layer/link_layer.h" 4 | #include "application_layer/application_layer.h" 5 | #include "attribute/server.h" 6 | 7 | #define LOGGING_MODULE_NAME "SONAR" 8 | #include "anchor/logging/logging.h" 9 | 10 | #include 11 | 12 | #define GET_SERVER_IMPL(SERVER) ((instance_impl_t*)((SERVER)->_private)) 13 | #define GET_SERVER_ATTR_IMPL(SERVER) ((attr_instance_impl_t*)((SERVER)->_private)) 14 | 15 | typedef struct { 16 | sonar_server_init_t init; 17 | sonar_server_attribute_t attr_list; 18 | sonar_link_layer_context_t link_layer_context; 19 | sonar_application_layer_context_t application_layer_context; 20 | sonar_attribute_server_context_t attr_server_context; 21 | sonar_link_layer_handle_t link_layer_handle; 22 | sonar_application_layer_handle_t application_layer_handle; 23 | sonar_attribute_server_handle_t attr_server_handle; 24 | } instance_impl_t; 25 | _Static_assert(sizeof(instance_impl_t) == sizeof(((sonar_server_handle_t)0)->_private), "Invalid context size"); 26 | 27 | typedef struct { 28 | sonar_server_attribute_t next; 29 | } attr_instance_impl_t; 30 | _Static_assert(sizeof(attr_instance_impl_t) == sizeof(((sonar_server_attribute_t)0)->_private), "Invalid attribute context size"); 31 | 32 | static sonar_server_attribute_t get_server_attr(sonar_server_handle_t handle, sonar_attribute_t attr) { 33 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 34 | sonar_server_attribute_t server_attr = inst->attr_list; 35 | while (server_attr) { 36 | if (server_attr->attr == attr) { 37 | return server_attr; 38 | } 39 | server_attr = GET_SERVER_ATTR_IMPL(server_attr)->next; 40 | } 41 | return NULL; 42 | } 43 | 44 | static void link_layer_connection_changed_callback(void* handle, bool connected) { 45 | instance_impl_t* inst = handle; 46 | inst->init.connection_changed_callback(handle, connected); 47 | } 48 | 49 | static bool link_layer_request_handler(void* handle, const uint8_t* data, uint32_t length) { 50 | instance_impl_t* inst = handle; 51 | return sonar_application_layer_handle_request(inst->application_layer_handle, data, length); 52 | } 53 | 54 | static void link_layer_response_handler(void* handle, bool success, const uint8_t* data, uint32_t length) { 55 | instance_impl_t* inst = handle; 56 | sonar_application_layer_handle_response(inst->application_layer_handle, success, data, length); 57 | } 58 | 59 | static bool application_layer_send_data_function(void* handle, const buffer_chain_entry_t* data) { 60 | return sonar_link_layer_send_request(handle, data); 61 | } 62 | 63 | static void application_layer_set_response_function(void* handle, const uint8_t* data, uint32_t length) { 64 | return sonar_link_layer_set_response(handle, data, length); 65 | } 66 | 67 | static bool application_layer_attribute_read_handler(void* handle, uint16_t attribute_id) { 68 | return sonar_attribute_server_handle_read_request(handle, attribute_id); 69 | } 70 | 71 | static bool application_layer_attribute_write_handler(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 72 | return sonar_attribute_server_handle_write_request(handle, attribute_id, data, length); 73 | } 74 | 75 | static bool application_layer_attribute_notify_handler(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 76 | // the server should never got notify requests 77 | LOG_ERROR("Got unexpected attribute notify request (0x%x)", attribute_id); 78 | return false; 79 | } 80 | 81 | static void attribute_server_handle_notify_response(void* handle, uint16_t attribute_id, bool success) { 82 | sonar_attribute_server_handle_notify_response(handle, attribute_id, success); 83 | } 84 | 85 | static bool attribute_server_send_notify_request_function(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 86 | instance_impl_t* inst = handle; 87 | return sonar_application_layer_notify_request(inst->application_layer_handle, attribute_id, data, length); 88 | } 89 | 90 | static void attribute_server_read_response_handler(void* handle, const uint8_t* data, uint32_t length) { 91 | instance_impl_t* inst = handle; 92 | sonar_application_layer_read_response(inst->application_layer_handle, data, length); 93 | } 94 | 95 | static uint32_t attribute_server_read_handler(void* handle, sonar_attribute_t attr, void* response_data, uint32_t response_max_size) { 96 | sonar_server_attribute_t server_attr = get_server_attr(handle, attr); 97 | if (!server_attr) { 98 | LOG_ERROR("Unknown attribute for read request"); 99 | return 0; 100 | } 101 | return server_attr->read_handler(response_data, response_max_size); 102 | } 103 | 104 | static bool attribute_server_write_handler(void* handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length) { 105 | sonar_server_attribute_t server_attr = get_server_attr(handle, attr); 106 | if (!server_attr) { 107 | LOG_ERROR("Unknown attribute for write request"); 108 | return false; 109 | } 110 | return server_attr->write_handler(data, length); 111 | } 112 | 113 | static void attribute_server_notify_complete_handler(void* handle, bool success) { 114 | instance_impl_t* inst = handle; 115 | inst->init.attribute_notify_complete_handler(handle, success); 116 | } 117 | 118 | void sonar_server_init(sonar_server_handle_t handle, const sonar_server_init_t* init) { 119 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 120 | *inst = (instance_impl_t){ 121 | .init = *init, 122 | .link_layer_handle = &inst->link_layer_context, 123 | .application_layer_handle = &inst->application_layer_context, 124 | .attr_server_handle = &inst->attr_server_context, 125 | }; 126 | const sonar_link_layer_init_t init_link_layer = { 127 | .config = { 128 | .is_server = true, 129 | }, 130 | .buffers = { 131 | .receive = handle->receive_buffer, 132 | .receive_size = handle->receive_buffer_size, 133 | }, 134 | .functions = { 135 | .get_system_time_ms = init->get_system_time_ms, 136 | .write_byte = init->write_byte, 137 | }, 138 | .handlers = { 139 | .connection_changed = link_layer_connection_changed_callback, 140 | .request = link_layer_request_handler, 141 | .request_complete = link_layer_response_handler, 142 | .handler_handle = inst, 143 | }, 144 | }; 145 | sonar_link_layer_init(inst->link_layer_handle, &init_link_layer); 146 | 147 | const sonar_application_layer_init_t init_application_layer = { 148 | .is_server = true, 149 | .send_data_function = application_layer_send_data_function, 150 | .set_response_function = application_layer_set_response_function, 151 | .send_data_handle = inst->link_layer_handle, 152 | .attribute_read_handler = application_layer_attribute_read_handler, 153 | .attribute_write_handler = application_layer_attribute_write_handler, 154 | .attribute_notify_handler = application_layer_attribute_notify_handler, 155 | .attr_handler_handle = inst->attr_server_handle, 156 | 157 | .notify_request_complete_handler = attribute_server_handle_notify_response, 158 | .request_complete_handle = inst->attr_server_handle, 159 | }; 160 | sonar_application_layer_init(inst->application_layer_handle, &init_application_layer); 161 | 162 | const sonar_attribute_server_init_t init_attr_server = { 163 | .send_notify_request_function = attribute_server_send_notify_request_function, 164 | .read_response_handler = attribute_server_read_response_handler, 165 | .read_handler = attribute_server_read_handler, 166 | .write_handler = attribute_server_write_handler, 167 | .notify_complete_handler = attribute_server_notify_complete_handler, 168 | .handle = inst, 169 | }; 170 | sonar_attribute_server_init(inst->attr_server_handle, &init_attr_server); 171 | } 172 | 173 | bool sonar_server_is_connected(sonar_server_handle_t handle) { 174 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 175 | return sonar_link_layer_is_connected(inst->link_layer_handle); 176 | } 177 | 178 | void sonar_server_process(sonar_server_handle_t handle, const uint8_t* received_data, uint32_t received_data_length) { 179 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 180 | sonar_link_layer_handle_receive_data(inst->link_layer_handle, received_data, received_data_length); 181 | sonar_link_layer_process(inst->link_layer_handle); 182 | } 183 | 184 | void sonar_server_register(sonar_server_handle_t handle, sonar_server_attribute_t attr) { 185 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 186 | if (inst->attr_list) { 187 | GET_SERVER_ATTR_IMPL(attr)->next = inst->attr_list; 188 | } 189 | inst->attr_list = attr; 190 | sonar_attribute_server_register(inst->attr_server_handle, attr->attr); 191 | } 192 | 193 | bool sonar_server_notify(sonar_server_handle_t handle, sonar_server_attribute_t attr, const void* data, uint32_t length) { 194 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 195 | return sonar_attribute_server_notify(inst->attr_server_handle, attr->attr, data, length); 196 | } 197 | 198 | bool sonar_server_notify_read_data(sonar_server_handle_t handle, sonar_server_attribute_t attr) { 199 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 200 | return sonar_attribute_server_notify_read_data(inst->attr_server_handle, attr->attr); 201 | } 202 | 203 | void sonar_server_get_and_clear_errors(sonar_server_handle_t handle, sonar_errors_t* errors) { 204 | instance_impl_t* inst = GET_SERVER_IMPL(handle); 205 | sonar_link_layer_errors_t link_layer_errors; 206 | sonar_link_layer_receive_errors_t link_layer_receive_errors; 207 | sonar_link_layer_get_and_clear_errors(inst->link_layer_handle, &link_layer_errors, &link_layer_receive_errors); 208 | *errors = (sonar_errors_t){ 209 | .link_layer_receive = { 210 | .invalid_header = link_layer_receive_errors.invalid_header, 211 | .invalid_crc = link_layer_receive_errors.invalid_crc, 212 | .buffer_overflow = link_layer_receive_errors.buffer_overflow, 213 | .invalid_escape_sequence = link_layer_receive_errors.invalid_escape_sequence, 214 | }, 215 | .link_layer = { 216 | .invalid_packet = link_layer_errors.invalid_packet, 217 | .unexpected_packet = link_layer_errors.unexpected_packet, 218 | .invalid_sequence_number = link_layer_errors.invalid_sequence_number, 219 | .retries = link_layer_errors.retries, 220 | }, 221 | }; 222 | } 223 | -------------------------------------------------------------------------------- /sonar/tests/Makefile: -------------------------------------------------------------------------------- 1 | TARGET := unit_test 2 | BUILD_DIR := build/ 3 | 4 | include ../sonar.mk 5 | 6 | C_SOURCES := \ 7 | $(SONAR_C_SOURCES) \ 8 | ../../logging/src/logging.c 9 | 10 | CXX_SOURCES := \ 11 | main.cpp \ 12 | test_buffer_chain.cpp \ 13 | test_crc16.cpp \ 14 | test_link_layer_receive.cpp \ 15 | test_link_layer_transmit.cpp \ 16 | test_link_layer.cpp \ 17 | test_application_layer.cpp \ 18 | test_attribute_server.cpp \ 19 | test_attribute_client.cpp \ 20 | test_client.cpp \ 21 | test_server.cpp 22 | 23 | CXX_INCLUDES := \ 24 | -I.. \ 25 | -I../include \ 26 | -I../../logging/include 27 | 28 | OPT := 29 | 30 | CC := gcc 31 | CXX := g++ 32 | 33 | OBJECTS := $(addprefix $(BUILD_DIR)/,$(notdir $(CXX_SOURCES:.cpp=.o))) $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o))) 34 | vpath %.c $(sort $(dir $(C_SOURCES))) 35 | vpath %.cpp $(sort $(dir $(CXX_SOURCES))) 36 | 37 | CFLAGS := $(CXX_INCLUDES) -g3 -Wno-extern-c-compat -Werror 38 | LDFLAGS := -lgtest -lpthread 39 | 40 | $(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR) 41 | @echo "Compiling $(notdir $@)" 42 | @$(CC) -c -DFILENAME=\"$(notdir $<)\" $(CFLAGS) -MD -MF"$(@:%.o=%.d)" $< -o $@ 43 | 44 | $(BUILD_DIR)/%.o: %.cpp Makefile | $(BUILD_DIR) 45 | @echo "Compiling $(notdir $@)" 46 | @$(CXX) -c -DFILENAME=\"$(notdir $<)\" $(CFLAGS) -std=c++14 -MD -MF"$(@:%.o=%.d)" $< -o $@ 47 | 48 | $(BUILD_DIR)/$(TARGET): $(OBJECTS) Makefile | $(BUILD_DIR) 49 | @echo "Linking $(notdir $@)" 50 | @$(CXX) $(OBJECTS) -o $@ $(LDFLAGS) 51 | 52 | $(BUILD_DIR): 53 | @mkdir -p $@ 54 | 55 | build: $(BUILD_DIR)/$(TARGET) 56 | 57 | test: $(BUILD_DIR)/$(TARGET) 58 | @$< 59 | 60 | clean: 61 | @echo "Deleting build folder" 62 | @rm -fR $(BUILD_DIR) 63 | 64 | 65 | -include $(wildcard $(BUILD_DIR)/*.d) 66 | .PHONY: test clean build 67 | .DEFAULT_GOAL := test 68 | -------------------------------------------------------------------------------- /sonar/tests/main.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | extern "C" { 4 | #include "anchor/logging/logging.h" 5 | } 6 | 7 | #include 8 | #include 9 | 10 | static void logging_write_function(const char* str) { 11 | fputs(str, stdout); 12 | } 13 | 14 | static uint32_t logging_time_ms_function(void) { 15 | return time(NULL); 16 | } 17 | 18 | int main(int argc, char **argv) { 19 | const logging_init_t init_logging = { 20 | .write_function = logging_write_function, 21 | .lock_function = nullptr, 22 | .time_ms_function = logging_time_ms_function, 23 | .default_level = LOGGING_LEVEL_DEBUG, 24 | }; 25 | logging_init(&init_logging); 26 | 27 | ::testing::InitGoogleTest(&argc, argv); 28 | return RUN_ALL_TESTS(); 29 | } 30 | -------------------------------------------------------------------------------- /sonar/tests/test_attribute_client.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "test_common.h" 4 | 5 | extern "C" { 6 | 7 | #include "src/attribute/client.h" 8 | 9 | }; 10 | 11 | SONAR_ATTR_DEF(TEST_ATTR, 0xff1, sizeof(uint32_t), RW); 12 | SONAR_ATTR_DEF(TEST_ATTR2, 0xff2, sizeof(uint32_t), N); 13 | 14 | static uint32_t m_test_attr_num_read_complete; 15 | static bool m_test_attr_read_complete_success; 16 | static uint32_t m_test_attr_read_complete_data; 17 | static uint32_t m_test_attr_num_write_complete; 18 | static bool m_test_attr_write_complete_success; 19 | static uint32_t m_test_attr_num_notifies; 20 | static uint32_t m_test_notify_data; 21 | static uint32_t m_read_request_num; 22 | static uint32_t m_read_request_attribute_id; 23 | static uint32_t m_write_request_num; 24 | static uint16_t m_write_request_attribute_id; 25 | static std::vector m_write_request_data; 26 | static int m_num_connections; 27 | static int m_num_disconnections; 28 | 29 | static bool send_read_request_function(void* handle, uint16_t attribute_id) { 30 | m_read_request_num++; 31 | m_read_request_attribute_id = attribute_id; 32 | return true; 33 | } 34 | 35 | static bool send_write_request_function(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 36 | m_write_request_num++; 37 | m_write_request_attribute_id = attribute_id; 38 | m_write_request_data.insert(m_write_request_data.end(), data, data + length); 39 | return true; 40 | } 41 | 42 | static void read_complete_handler(void* handle, bool success, const uint8_t* data, uint32_t length) { 43 | m_test_attr_num_read_complete++; 44 | m_test_attr_read_complete_success = success; 45 | if (success) { 46 | m_test_attr_read_complete_data = *(const uint32_t*)data; 47 | } 48 | } 49 | 50 | static void write_complete_handler(void* handle, bool success) { 51 | m_test_attr_num_write_complete++; 52 | m_test_attr_write_complete_success = success; 53 | } 54 | 55 | static bool notify_handler(void* handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length) { 56 | m_test_attr_num_notifies++; 57 | if (length == sizeof(uint32_t)) { 58 | m_test_notify_data = *(uint32_t*)data; 59 | } 60 | return true; 61 | } 62 | 63 | static void connection_changed_callback(void* handle, bool connected) { 64 | if (connected) { 65 | m_num_connections++; 66 | } else { 67 | m_num_disconnections++; 68 | } 69 | } 70 | 71 | class AttributeClientTest : public ::testing::Test { 72 | protected: 73 | void SetUp() override { 74 | m_test_attr_num_read_complete = 0; 75 | m_test_attr_read_complete_success = 0; 76 | m_test_attr_read_complete_data = 0; 77 | m_test_attr_num_write_complete = 0; 78 | m_test_attr_write_complete_success = false; 79 | m_test_attr_num_notifies = 0; 80 | m_test_notify_data = 0; 81 | m_read_request_num = 0; 82 | m_read_request_attribute_id = 0; 83 | m_write_request_num = 0; 84 | m_write_request_attribute_id = 0; 85 | m_write_request_data.clear(); 86 | m_num_connections = 0; 87 | m_num_disconnections = 0; 88 | 89 | static sonar_attribute_client_context_t context; 90 | const sonar_attribute_client_init_t init_attribute_client = { 91 | .send_read_request_function = send_read_request_function, 92 | .send_write_request_function = send_write_request_function, 93 | .connection_changed_callback = connection_changed_callback, 94 | .read_complete_handler = read_complete_handler, 95 | .write_complete_handler = write_complete_handler, 96 | .notify_handler = notify_handler, 97 | .handle = NULL, 98 | }; 99 | handle_ = &context; 100 | sonar_attribute_client_init(handle_, &init_attribute_client); 101 | sonar_attribute_client_register(handle_, TEST_ATTR); 102 | sonar_attribute_client_register(handle_, TEST_ATTR2); 103 | 104 | // Run (and test) the connection process as it's required before any of the other tests can run 105 | 106 | // Set the state to connected and expect a CTRL_NUM_ATTRS read request 107 | sonar_attribute_client_low_level_connection_changed(handle_, true); 108 | EXPECT_EQ(m_read_request_num, 1); 109 | m_read_request_num = 0; 110 | EXPECT_EQ(m_read_request_attribute_id, 0x101); 111 | 112 | // Respond to the CTRL_NUM_ATTRS read request and expect a CTRL_ATTR_OFFSET write request 113 | const uint16_t num = 5; 114 | sonar_attribute_client_handle_read_response(handle_, 0x101, true, (const uint8_t*)&num, sizeof(num)); 115 | EXPECT_EQ(m_write_request_num, 1); 116 | m_write_request_num = 0; 117 | EXPECT_EQ(m_write_request_attribute_id, 0x102); 118 | EXPECT_EQ(m_write_request_data.size(), sizeof(uint16_t)); 119 | EXPECT_EQ(*(uint16_t*)m_write_request_data.data(), 0); 120 | m_write_request_data.clear(); 121 | 122 | // Respond to the CTRL_ATTR_OFFSET write request and expect a CTRL_ATTR_LIST read request 123 | sonar_attribute_client_handle_write_response(handle_, 0x102, true); 124 | EXPECT_EQ(m_read_request_num, 1); 125 | m_read_request_num = 0; 126 | EXPECT_EQ(m_read_request_attribute_id, 0x103); 127 | 128 | // Respond to the CTRL_ATTR_LIST read request 129 | const uint16_t attr_list[8] = { 0x4ff2, 0x3ff1, 0x1103, 0x3102, 0x1101 }; 130 | sonar_attribute_client_handle_read_response(handle_, 0x103, true, (const uint8_t*)&attr_list, sizeof(attr_list)); 131 | EXPECT_EQ(m_num_connections, 1); 132 | m_num_connections = 0; 133 | } 134 | 135 | void TearDown() override { 136 | sonar_attribute_client_low_level_connection_changed(handle_, false); 137 | EXPECT_EQ(m_num_disconnections, 1); 138 | 139 | EXPECT_EQ(m_test_attr_num_read_complete, 0); 140 | EXPECT_EQ(m_test_attr_num_write_complete, 0); 141 | EXPECT_EQ(m_test_attr_num_notifies, 0); 142 | EXPECT_EQ(m_read_request_num, 0); 143 | EXPECT_EQ(m_write_request_num, 0); 144 | EXPECT_TRUE(m_write_request_data.empty()); 145 | EXPECT_EQ(m_num_connections, 0); 146 | } 147 | 148 | sonar_attribute_client_handle_t handle_; 149 | }; 150 | 151 | TEST_F(AttributeClientTest, ValidReadRequest) { 152 | EXPECT_TRUE(sonar_attribute_client_read(handle_, TEST_ATTR)); 153 | EXPECT_EQ(m_read_request_num, 1); 154 | m_read_request_num = 0; 155 | EXPECT_EQ(m_read_request_attribute_id, 0xff1); 156 | } 157 | 158 | TEST_F(AttributeClientTest, InvalidReadRequest) { 159 | // attribute which doesn't support read requests 160 | EXPECT_FALSE(sonar_attribute_client_read(handle_, TEST_ATTR2)); 161 | } 162 | 163 | TEST_F(AttributeClientTest, ReadResponse) { 164 | const uint32_t data = 0x44556677; 165 | 166 | // success response 167 | sonar_attribute_client_handle_read_response(handle_, 0xff1, true, (const uint8_t*)&data, sizeof(data)); 168 | EXPECT_EQ(m_test_attr_num_read_complete, 1); 169 | m_test_attr_num_read_complete = 0; 170 | EXPECT_EQ(m_test_attr_read_complete_success, true); 171 | EXPECT_EQ(m_test_attr_read_complete_data, data); 172 | 173 | // failed response 174 | sonar_attribute_client_handle_read_response(handle_, 0xff1, false, NULL, 0); 175 | EXPECT_EQ(m_test_attr_num_read_complete, 1); 176 | m_test_attr_num_read_complete = 0; 177 | EXPECT_EQ(m_test_attr_read_complete_success, false); 178 | } 179 | 180 | TEST_F(AttributeClientTest, ValidWriteRequest) { 181 | const uint32_t value = 0xabcdabcd; 182 | EXPECT_TRUE(sonar_attribute_client_write(handle_, TEST_ATTR, (const uint8_t*)&value, sizeof(value))); 183 | EXPECT_EQ(m_write_request_num, 1); 184 | m_write_request_num = 0; 185 | EXPECT_EQ(m_write_request_attribute_id, 0xff1); 186 | EXPECT_EQ(m_write_request_data.size(), sizeof(uint32_t)); 187 | EXPECT_EQ(*(uint32_t*)m_write_request_data.data(), 0xabcdabcd); 188 | m_write_request_data.clear(); 189 | } 190 | 191 | TEST_F(AttributeClientTest, InvalidWriteRequest) { 192 | // attribute which doesn't support write requests 193 | const uint32_t value = 0; 194 | EXPECT_FALSE(sonar_attribute_client_write(handle_, TEST_ATTR2, (const uint8_t*)&value, sizeof(value))); 195 | } 196 | 197 | TEST_F(AttributeClientTest, WriteResponse) { 198 | // success response 199 | sonar_attribute_client_handle_write_response(handle_, 0xff1, true); 200 | EXPECT_EQ(m_test_attr_num_write_complete, 1); 201 | m_test_attr_num_write_complete = 0; 202 | EXPECT_EQ(m_test_attr_write_complete_success, true); 203 | 204 | // failed response 205 | sonar_attribute_client_handle_write_response(handle_, 0xff1, false); 206 | EXPECT_EQ(m_test_attr_num_write_complete, 1); 207 | m_test_attr_num_write_complete = 0; 208 | EXPECT_EQ(m_test_attr_write_complete_success, false); 209 | } 210 | 211 | TEST_F(AttributeClientTest, HandleValidNotify) { 212 | const uint32_t data = 0x12345678; 213 | EXPECT_TRUE(sonar_attribute_client_handle_notify_request(handle_, 0xff2, (const uint8_t*)&data, sizeof(data))); 214 | EXPECT_EQ(m_test_attr_num_notifies, 1); 215 | m_test_attr_num_notifies = 0; 216 | EXPECT_EQ(m_test_notify_data, data); 217 | } 218 | 219 | TEST_F(AttributeClientTest, HandleInvalidNotify) { 220 | const uint32_t data = 0xaabbccdd; 221 | 222 | // attribute which doesn't exist 223 | EXPECT_FALSE(sonar_attribute_client_handle_notify_request(handle_, 0xfff, (const uint8_t*)&data, sizeof(data))); 224 | 225 | // attribute which doesn't support notify 226 | EXPECT_FALSE(sonar_attribute_client_handle_notify_request(handle_, 0xff1, (const uint8_t*)&data, sizeof(data))); 227 | } 228 | 229 | TEST_F(AttributeClientTest, TestDisconnect) { 230 | sonar_attribute_client_low_level_connection_changed(handle_, false); 231 | EXPECT_EQ(m_num_disconnections, 1); 232 | m_num_disconnections = 0; 233 | 234 | // responses should fail / be ignored 235 | const uint32_t data = 0x44556677; 236 | sonar_attribute_client_handle_read_response(handle_, 0xff1, true, (const uint8_t*)&data, sizeof(data)); 237 | sonar_attribute_client_handle_write_response(handle_, 0xff1, true); 238 | 239 | // requests should fail 240 | EXPECT_FALSE(sonar_attribute_client_handle_notify_request(handle_, 0xff2, (const uint8_t*)&data, sizeof(data))); 241 | } 242 | -------------------------------------------------------------------------------- /sonar/tests/test_attribute_server.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "test_common.h" 4 | 5 | extern "C" { 6 | 7 | #include "src/attribute/server.h" 8 | 9 | }; 10 | 11 | #define READ_EXPECT_RESPONSE(ATTR_ID, ...) do { \ 12 | const uint8_t _response[] = { __VA_ARGS__ }; \ 13 | EXPECT_TRUE(sonar_attribute_server_handle_read_request(handle_, ATTR_ID)); \ 14 | EXPECT_TRUE(DataMatches(m_response_data, _response, sizeof(_response))); \ 15 | m_response_data.clear(); \ 16 | } while (0) 17 | 18 | SONAR_ATTR_DEF(TEST_ATTR, 0xff1, sizeof(uint32_t), RW); 19 | SONAR_ATTR_DEF(TEST_ATTR2, 0xff2, sizeof(uint32_t), N); 20 | 21 | static uint32_t m_test_attr_num_reads; 22 | static uint32_t m_test_attr_num_writes; 23 | static uint32_t m_test_attr_write_data; 24 | static uint32_t m_test_attr_num_notify_complete; 25 | static bool m_test_attr_notify_complete_success; 26 | static uint32_t m_notify_request_num; 27 | static uint16_t m_notify_request_attribute_id; 28 | static std::vector m_notify_request_data; 29 | static std::vector m_response_data; 30 | 31 | static bool send_notify_request_function(void* handle, uint16_t attribute_id, const uint8_t* data, uint32_t length) { 32 | m_notify_request_num++; 33 | m_notify_request_attribute_id = attribute_id; 34 | m_notify_request_data.insert(m_notify_request_data.end(), data, data + length); 35 | return true; 36 | } 37 | 38 | static void read_response_handler(void* handle, const uint8_t* data, uint32_t length) { 39 | m_response_data.insert(m_response_data.end(), data, data + length); 40 | } 41 | 42 | static uint32_t read_handler(void* handle, sonar_attribute_t attr, void* response_data, uint32_t response_max_size) { 43 | m_test_attr_num_reads++; 44 | if (attr == TEST_ATTR && response_max_size == sizeof(uint32_t)) { 45 | *(uint32_t*)response_data = 0x11223344; 46 | return sizeof(uint32_t); 47 | } else { 48 | return 0; 49 | } 50 | } 51 | 52 | static bool write_handler(void* handle, sonar_attribute_t attr, const uint8_t* data, uint32_t length) { 53 | m_test_attr_num_writes++; 54 | if (attr == TEST_ATTR) { 55 | if (length != sizeof(m_test_attr_write_data)) { 56 | return false; 57 | } 58 | memcpy(&m_test_attr_write_data, data, length); 59 | return true; 60 | } else { 61 | return false; 62 | } 63 | } 64 | 65 | static void notify_complete_handler(void* handle, bool success) { 66 | m_test_attr_num_notify_complete++; 67 | m_test_attr_notify_complete_success = success; 68 | } 69 | 70 | class AttributeServerTest : public ::testing::Test { 71 | protected: 72 | void SetUp() override { 73 | m_test_attr_num_reads = 0; 74 | m_test_attr_num_writes = 0; 75 | m_test_attr_write_data = 0; 76 | m_test_attr_num_notify_complete = 0; 77 | m_test_attr_notify_complete_success = false; 78 | m_notify_request_num = 0; 79 | m_notify_request_attribute_id = 0; 80 | m_notify_request_data.clear(); 81 | static sonar_attribute_server_context_t context; 82 | handle_ = &context; 83 | const sonar_attribute_server_init_t init_attribute_server = { 84 | .send_notify_request_function = send_notify_request_function, 85 | .read_response_handler = read_response_handler, 86 | .read_handler = read_handler, 87 | .write_handler = write_handler, 88 | .notify_complete_handler = notify_complete_handler, 89 | .handle = handle_, 90 | }; 91 | sonar_attribute_server_init(handle_, &init_attribute_server); 92 | sonar_attribute_server_register(handle_, TEST_ATTR); 93 | sonar_attribute_server_register(handle_, TEST_ATTR2); 94 | } 95 | 96 | void TearDown() override { 97 | EXPECT_EQ(m_test_attr_num_reads, 0); 98 | EXPECT_EQ(m_test_attr_num_writes, 0); 99 | EXPECT_EQ(m_test_attr_num_notify_complete, 0); 100 | EXPECT_EQ(m_notify_request_num, 0); 101 | } 102 | 103 | sonar_attribute_server_handle_t handle_; 104 | }; 105 | 106 | TEST_F(AttributeServerTest, HandleValidRead) { 107 | const uint32_t* data; 108 | uint32_t data_len = UINT32_MAX; 109 | READ_EXPECT_RESPONSE(0xff1, 0x44, 0x33, 0x22, 0x11); 110 | EXPECT_EQ(m_test_attr_num_reads, 1); 111 | m_test_attr_num_reads = 0; 112 | } 113 | 114 | TEST_F(AttributeServerTest, HandleInvalidRead) { 115 | const uint32_t* data; 116 | uint32_t data_len; 117 | 118 | // attribute which doesn't exist 119 | EXPECT_FALSE(sonar_attribute_server_handle_read_request(handle_, 0xfff)); 120 | EXPECT_EQ(m_test_attr_num_reads, 0); 121 | 122 | // attribute which doesn't support reads 123 | EXPECT_FALSE(sonar_attribute_server_handle_read_request(handle_, 0xff2)); 124 | EXPECT_EQ(m_test_attr_num_reads, 0); 125 | } 126 | 127 | TEST_F(AttributeServerTest, HandleValidWrite) { 128 | const uint32_t data = 0xaabbccdd; 129 | EXPECT_TRUE(sonar_attribute_server_handle_write_request(handle_, 0xff1, (const uint8_t*)&data, sizeof(data))); 130 | EXPECT_EQ(m_test_attr_write_data, data); 131 | EXPECT_EQ(m_test_attr_num_writes, 1); 132 | m_test_attr_num_writes = 0; 133 | } 134 | 135 | TEST_F(AttributeServerTest, HandleInvalidWrite) { 136 | const uint32_t data = 0xaabbccdd; 137 | 138 | // attribute which doesn't exist 139 | EXPECT_FALSE(sonar_attribute_server_handle_write_request(handle_, 0xfff, (const uint8_t*)&data, sizeof(data))); 140 | 141 | // attribute which doesn't support writes 142 | EXPECT_FALSE(sonar_attribute_server_handle_write_request(handle_, 0xff2, (const uint8_t*)&data, sizeof(data))); 143 | } 144 | 145 | TEST_F(AttributeServerTest, ValidNotifyRequest) { 146 | const uint32_t data = 0xabcdabcd; 147 | EXPECT_TRUE(sonar_attribute_server_notify(handle_, TEST_ATTR2, (const uint8_t*)&data, sizeof(data))); 148 | EXPECT_EQ(m_notify_request_num, 1); 149 | m_notify_request_num = 0; 150 | EXPECT_EQ(m_notify_request_attribute_id, 0xff2); 151 | EXPECT_EQ(m_notify_request_data.size(), sizeof(data)); 152 | EXPECT_EQ(*(uint32_t*)m_notify_request_data.data(), data); 153 | m_notify_request_data.clear(); 154 | } 155 | 156 | TEST_F(AttributeServerTest, InvalidNotifyRequest) { 157 | // attribute which doesn't support notify requests 158 | const uint32_t data = 0xabcdabcd; 159 | EXPECT_FALSE(sonar_attribute_server_notify(handle_, TEST_ATTR, (const uint8_t*)&data, sizeof(data))); 160 | } 161 | 162 | TEST_F(AttributeServerTest, NotifyResponse) { 163 | // success response 164 | sonar_attribute_server_handle_notify_response(handle_, 0xff2, true); 165 | EXPECT_EQ(m_test_attr_num_notify_complete, 1); 166 | m_test_attr_num_notify_complete = 0; 167 | EXPECT_EQ(m_test_attr_notify_complete_success, true); 168 | 169 | // failed response 170 | sonar_attribute_server_handle_notify_response(handle_, 0xff2, false); 171 | EXPECT_EQ(m_test_attr_num_notify_complete, 1); 172 | m_test_attr_num_notify_complete = 0; 173 | EXPECT_EQ(m_test_attr_notify_complete_success, false); 174 | } 175 | 176 | TEST_F(AttributeServerTest, ControlAttrs) { 177 | uint32_t data_len; 178 | 179 | // Read CTRL_NUM_ATTRS (should be 2) 180 | READ_EXPECT_RESPONSE(0x101, 0x02, 0x00); 181 | 182 | // Write to CTRL_ATTR_OFFSET to 0 183 | const uint16_t initial_attr_offset = 0; 184 | EXPECT_TRUE(sonar_attribute_server_handle_write_request(handle_, 0x102, (const uint8_t*)&initial_attr_offset, sizeof(initial_attr_offset))); 185 | 186 | // Read CTRL_ATTR_LIST 187 | READ_EXPECT_RESPONSE(0x103, 0xf2, 0x4f, 0xf1, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 188 | 189 | // Read back CTRL_ATTR_OFFSET (should still be 0) 190 | READ_EXPECT_RESPONSE(0x102, 0x00, 0x00); 191 | 192 | // Write CTRL_ATTR_OFFSET to 1 193 | const uint16_t attr_offset2 = 1; 194 | EXPECT_TRUE(sonar_attribute_server_handle_write_request(handle_, 0x102, (const uint8_t*)&attr_offset2, sizeof(attr_offset2))); 195 | 196 | // Read CTRL_ATTR_LIST again (with an offset of 1) 197 | READ_EXPECT_RESPONSE(0x103, 0xf1, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 198 | } 199 | -------------------------------------------------------------------------------- /sonar/tests/test_buffer_chain.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "test_common.h" 4 | 5 | extern "C" { 6 | 7 | #include "src/common/buffer_chain.h" 8 | 9 | }; 10 | 11 | #define EXPECT_CHAIN_DATA(START_ENTRY_PTR, ...) do { \ 12 | const uint8_t _expected_data[] = {__VA_ARGS__}; \ 13 | EXPECT_TRUE(DataMatches(buffer_chain_to_vector(START_ENTRY_PTR), _expected_data, sizeof(_expected_data))); \ 14 | } while (0) 15 | 16 | TEST(BufferChain, TestCreate) { 17 | const uint8_t data[] = {0x11, 0x12}; 18 | buffer_chain_entry_t entry = {}; 19 | buffer_chain_set_data(&entry, data, sizeof(data)); 20 | EXPECT_EQ(entry.next, nullptr); 21 | EXPECT_EQ(entry.data, data); 22 | EXPECT_EQ(entry.length, sizeof(data)); 23 | EXPECT_CHAIN_DATA(&entry, 0x11, 0x12); 24 | } 25 | 26 | TEST(BufferChain, TestInsertBack) { 27 | // create the initial entry 28 | const uint8_t data1[] = {0x11, 0x12}; 29 | buffer_chain_entry_t entry1 = {}; 30 | buffer_chain_set_data(&entry1, data1, sizeof(data1)); 31 | EXPECT_CHAIN_DATA(&entry1, 0x11, 0x12); 32 | 33 | // insert a second entry at the back 34 | const uint8_t data2[] = {0x21, 0x22}; 35 | buffer_chain_entry_t entry2 = {}; 36 | buffer_chain_set_data(&entry2, data2, sizeof(data2)); 37 | buffer_chain_push_back(&entry1, &entry2); 38 | EXPECT_EQ(entry1.next, &entry2); 39 | EXPECT_EQ(entry2.next, nullptr); 40 | EXPECT_CHAIN_DATA(&entry1, 0x11, 0x12, 0x21, 0x22); 41 | 42 | // insert a third entry at the back 43 | const uint8_t data3[] = {0x31, 0x32}; 44 | buffer_chain_entry_t entry3 = {}; 45 | buffer_chain_set_data(&entry3, data3, sizeof(data3)); 46 | buffer_chain_push_back(&entry1, &entry3); 47 | EXPECT_EQ(entry1.next, &entry2); 48 | EXPECT_EQ(entry2.next, &entry3); 49 | EXPECT_EQ(entry3.next, nullptr); 50 | EXPECT_CHAIN_DATA(&entry1, 0x11, 0x12, 0x21, 0x22, 0x31, 0x32); 51 | } 52 | -------------------------------------------------------------------------------- /sonar/tests/test_client.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "test_common.h" 4 | 5 | extern "C" { 6 | 7 | #include "anchor/sonar/client.h" 8 | #include "src/link_layer/timeouts.h" 9 | 10 | }; 11 | 12 | #define PROCESS_RECEIVE_PACKET(...) do { \ 13 | BUILD_PACKET_BUFFER(_buffer, __VA_ARGS__); \ 14 | sonar_client_process(handle_, _buffer, sizeof(_buffer)); \ 15 | } while (0) 16 | 17 | #define EXPECT_WRITE_PACKET(...) do { \ 18 | BUILD_PACKET_BUFFER(_buffer, __VA_ARGS__); \ 19 | EXPECT_TRUE(DataMatches(m_write_data, _buffer, sizeof(_buffer))); \ 20 | m_write_data.clear(); \ 21 | } while (0) 22 | 23 | SONAR_ATTR_DEF(TEST_ATTR, 0xfff, sizeof(uint32_t), RWN); 24 | 25 | static std::vector m_write_data; 26 | static int m_num_connections; 27 | static int m_num_disconnections; 28 | static int m_attr_num_read_complete; 29 | static int m_attr_num_write_complete; 30 | static int m_attr_num_notify; 31 | 32 | static void write_byte(uint8_t byte) { 33 | m_write_data.push_back(byte); 34 | } 35 | 36 | static uint64_t get_system_time_ms(void) { 37 | return 0; 38 | } 39 | 40 | static void connection_changed_callback(bool connected) { 41 | if (connected) { 42 | m_num_connections++; 43 | } else { 44 | m_num_disconnections++; 45 | } 46 | } 47 | 48 | static void attribute_read_complete_handler(bool success, const void* data, uint32_t length) { 49 | m_attr_num_read_complete++; 50 | } 51 | 52 | static void attribute_write_complete_handler(bool success) { 53 | m_attr_num_write_complete++; 54 | } 55 | 56 | static bool attribute_notify_handler(sonar_attribute_t attr, const void* data, uint32_t length) { 57 | m_attr_num_notify++; 58 | return true; 59 | } 60 | 61 | class ClientTest : public ::testing::Test { 62 | protected: 63 | void SetUp() override { 64 | m_write_data.clear(); 65 | m_num_connections = 0; 66 | m_num_disconnections = 0; 67 | m_attr_num_read_complete = 0; 68 | m_attr_num_write_complete = 0; 69 | m_attr_num_notify = 0; 70 | 71 | SONAR_CLIENT_DEF(handle, 1024); 72 | handle_ = handle; 73 | const sonar_client_init_t init_client = { 74 | .write_byte = write_byte, 75 | .get_system_time_ms = get_system_time_ms, 76 | .connection_changed_callback = connection_changed_callback, 77 | .attribute_read_complete_handler = attribute_read_complete_handler, 78 | .attribute_write_complete_handler = attribute_write_complete_handler, 79 | .attribute_notify_handler = attribute_notify_handler, 80 | }; 81 | sonar_client_init(handle, &init_client); 82 | } 83 | 84 | void TearDown() override { 85 | EXPECT_TRUE(m_write_data.empty()); 86 | EXPECT_EQ(m_num_connections, 0); 87 | EXPECT_EQ(m_num_disconnections, 0); 88 | EXPECT_EQ(m_attr_num_read_complete, 0); 89 | EXPECT_EQ(m_attr_num_write_complete, 0); 90 | EXPECT_EQ(m_attr_num_notify, 0); 91 | } 92 | 93 | sonar_client_handle_t handle_; 94 | }; 95 | 96 | TEST_F(ClientTest, Full) { 97 | sonar_client_register(handle_, TEST_ATTR); 98 | 99 | // run the process function 100 | sonar_client_process(handle_, NULL, 0); 101 | // should send a connection request 102 | EXPECT_WRITE_PACKET(0x14, 0x01, 0x00); 103 | 104 | // process the connection response 105 | PROCESS_RECEIVE_PACKET(0x17, 0x01); 106 | // should read CTRL_NUM_ATTRS 107 | EXPECT_WRITE_PACKET(0x10, 0x02, 0x01, 0x11); 108 | 109 | // process the read response 110 | PROCESS_RECEIVE_PACKET(0x13, 0x02, 0x04, 0x00); 111 | // should write CTRL_ATTR_OFFSET to 0 112 | EXPECT_WRITE_PACKET(0x10, 0x03, 0x02, 0x21, 0x00, 0x00); 113 | 114 | // process the write response 115 | PROCESS_RECEIVE_PACKET(0x13, 0x03); 116 | // should read CTRL_ATTR_LIST 117 | EXPECT_WRITE_PACKET(0x10, 0x04, 0x03, 0x11); 118 | 119 | // process the read response 120 | EXPECT_FALSE(sonar_client_is_connected(handle_)); 121 | PROCESS_RECEIVE_PACKET(0x13, 0x04, 0x01, 0x11, 0x02, 0x31, 0x03, 0x11, 0xff, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 122 | // should now be connected 123 | EXPECT_TRUE(sonar_client_is_connected(handle_)); 124 | EXPECT_EQ(m_num_connections, 1); 125 | m_num_connections = 0; 126 | 127 | // read TEST_ATTR 128 | EXPECT_TRUE(sonar_client_read(handle_, TEST_ATTR)); 129 | EXPECT_WRITE_PACKET(0x10, 0x05, 0xff, 0x1f); 130 | // process the response 131 | PROCESS_RECEIVE_PACKET(0x13, 0x05, 0x44, 0x33, 0x22, 0x11); 132 | EXPECT_EQ(m_attr_num_read_complete, 1); 133 | m_attr_num_read_complete = 0; 134 | 135 | // write TEST_ATTR 136 | const uint32_t test_attr_data = 0xaa11bb22; 137 | EXPECT_TRUE(sonar_client_write(handle_, TEST_ATTR, (const uint8_t*)&test_attr_data, sizeof(test_attr_data))); 138 | EXPECT_WRITE_PACKET(0x10, 0x06, 0xff, 0x2f, 0x22, 0xbb, 0x11, 0xaa); 139 | // process the response 140 | PROCESS_RECEIVE_PACKET(0x13, 0x06); 141 | EXPECT_EQ(m_attr_num_write_complete, 1); 142 | m_attr_num_write_complete = 0; 143 | 144 | // process a notify request for TEST_ATTR 145 | PROCESS_RECEIVE_PACKET(0x12, 0x00, 0xff, 0x3f, 0x88, 0xaa, 0x99, 0xbb); 146 | EXPECT_EQ(m_attr_num_notify, 1); 147 | m_attr_num_notify = 0; 148 | // expect a response 149 | EXPECT_WRITE_PACKET(0x11, 0x00); 150 | } 151 | -------------------------------------------------------------------------------- /sonar/tests/test_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "gtest/gtest.h" 4 | 5 | extern "C" { 6 | 7 | #include "src/common/buffer_chain.h" 8 | #include "src/common/crc16.h" 9 | 10 | }; 11 | 12 | #define BUILD_PACKET_BUFFER(NAME, ...) \ 13 | const uint8_t NAME##_data[] = {__VA_ARGS__}; \ 14 | uint8_t NAME[sizeof(NAME##_data) + 4]; \ 15 | const uint16_t _crc = crc16(NAME##_data, sizeof(NAME##_data), CRC16_INITIAL_VALUE); \ 16 | NAME[0] = 0x7e; \ 17 | memcpy(&NAME[1], NAME##_data, sizeof(NAME##_data)); \ 18 | NAME[sizeof(NAME) - 3] = _crc & 0xff; \ 19 | NAME[sizeof(NAME) - 2] = _crc >> 8; \ 20 | NAME[sizeof(NAME) - 1] = 0x7e; 21 | 22 | static ::testing::AssertionResult DataMatches(const std::vector& actual, const uint8_t* expected, size_t expected_length) { 23 | if (actual.size() != expected_length) { 24 | return ::testing::AssertionFailure() 25 | << "actual length (" << actual.size() 26 | << ") != expected length (" << expected_length << ")"; 27 | } 28 | for (size_t i = 0; i < expected_length; i++){ 29 | if (expected[i] != actual.at(i)) { 30 | std::stringstream stream; 31 | stream << "actual[" << i 32 | << "] (0x" << std::hex << std::setw(2) << std::setfill('0') << int(actual.at(i)) << ") != expected[" << i 33 | << "] (0x" << std::hex << std::setw(2) << std::setfill('0') << int(expected[i]) << ")"; 34 | return ::testing::AssertionFailure() << stream.str(); 35 | } 36 | } 37 | 38 | return ::testing::AssertionSuccess(); 39 | } 40 | 41 | static std::vector buffer_chain_to_vector(const buffer_chain_entry_t* data) { 42 | std::vector result; 43 | FOREACH_BUFFER_CHAIN_ENTRY(data, entry) { 44 | result.insert(result.end(), entry->data, entry->data + entry->length); 45 | } 46 | return result; 47 | } 48 | -------------------------------------------------------------------------------- /sonar/tests/test_crc16.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | extern "C" { 4 | 5 | #include "src/common/crc16.h" 6 | 7 | }; 8 | 9 | TEST(CRC16, TestEmpty) { 10 | EXPECT_EQ(crc16(nullptr, 0, 0), 0); 11 | EXPECT_EQ(crc16(nullptr, 0, CRC16_INITIAL_VALUE), CRC16_INITIAL_VALUE); 12 | } 13 | 14 | TEST(CRC16, TestData) { 15 | // compared against CRC16/CCITT-FALSE output from https://crccalc.com/ 16 | const char* test_data1 = "123"; 17 | EXPECT_EQ(crc16((const uint8_t*)test_data1, strlen(test_data1), CRC16_INITIAL_VALUE), 0x5BCE); 18 | const char* test_data2 = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"; 19 | EXPECT_EQ(crc16((const uint8_t*)test_data2, strlen(test_data2), CRC16_INITIAL_VALUE), 0xD8E1); 20 | } 21 | -------------------------------------------------------------------------------- /sonar/tests/test_link_layer_transmit.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "test_common.h" 4 | 5 | extern "C" { 6 | 7 | #include "src/link_layer/transmit.h" 8 | 9 | }; 10 | 11 | #define TRANSMIT_PACKET(IS_RESPONSE, IS_LINK_CONTROL, SEQUENCE_NUM, ...) do { \ 12 | const uint8_t _buffer[] = {__VA_ARGS__}; \ 13 | buffer_chain_entry_t _data = {}; \ 14 | buffer_chain_set_data(&_data, _buffer, sizeof(_buffer)); \ 15 | sonar_link_layer_transmit_send_packet(handle_, IS_RESPONSE, IS_LINK_CONTROL, SEQUENCE_NUM, &_data); \ 16 | } while (0) 17 | 18 | #define EXPECT_AND_CLEAR_SENT_DATA(...) do { \ 19 | const uint8_t _expected_data[] = {__VA_ARGS__}; \ 20 | EXPECT_TRUE(DataMatches(m_transmit_sent_data, _expected_data, sizeof(_expected_data))); \ 21 | m_transmit_sent_data.clear(); \ 22 | } while (0) 23 | 24 | static std::vector m_transmit_sent_data; 25 | 26 | static void link_layer_transmit_write_byte_function(uint8_t byte) { 27 | m_transmit_sent_data.push_back(byte); 28 | } 29 | 30 | class LinkLayerTransmitTest : public ::testing::Test { 31 | protected: 32 | void DoLinkLayerTransmitInit(bool is_server) { 33 | static sonar_link_layer_transmit_context_t context; 34 | const sonar_link_layer_transmit_init_t init_link_layer = { 35 | .is_server = is_server, 36 | .write_byte_function = link_layer_transmit_write_byte_function, 37 | }; 38 | handle_ = &context; 39 | sonar_link_layer_transmit_init(handle_, &init_link_layer); 40 | } 41 | 42 | void SetUp() override { 43 | m_transmit_sent_data.clear(); 44 | } 45 | 46 | void TearDown() override { 47 | EXPECT_TRUE(m_transmit_sent_data.empty()); 48 | } 49 | 50 | sonar_link_layer_transmit_handle_t handle_; 51 | }; 52 | 53 | class LinkLayerTransmitServerTest : public LinkLayerTransmitTest { 54 | protected: 55 | void SetUp() override { 56 | LinkLayerTransmitTest::SetUp(); 57 | DoLinkLayerTransmitInit(true); 58 | } 59 | }; 60 | 61 | class LinkLayerTransmitClientTest : public LinkLayerTransmitTest { 62 | protected: 63 | void SetUp() override { 64 | LinkLayerTransmitTest::SetUp(); 65 | DoLinkLayerTransmitInit(false); 66 | } 67 | }; 68 | 69 | TEST_F(LinkLayerTransmitServerTest, Valid) { 70 | // request, server->client, normal, no data 71 | TRANSMIT_PACKET(false, false, 11); 72 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x12, 0x0b, 0x75, 0xc9, 0x7e); 73 | 74 | // request, server->client, normal, 1 byte of data 75 | TRANSMIT_PACKET(false, false, 11, 0x42); 76 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x12, 0x0b, 0x42, 0xe3, 0x55, 0x7e); 77 | 78 | // request, server->client, link control, no data 79 | TRANSMIT_PACKET(false, true, 11); 80 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x16, 0x0b, 0xb1, 0x05, 0x7e); 81 | 82 | // request, server->client, link control, 1 byte of data 83 | TRANSMIT_PACKET(false, true, 11, 0x42); 84 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x16, 0x0b, 0x42, 0x23, 0x89, 0x7e); 85 | 86 | // response, server->client, normal, no data 87 | TRANSMIT_PACKET(true, false, 11); 88 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x13, 0x0b, 0x44, 0xfa, 0x7e); 89 | 90 | // response, server->client, normal, 1 byte of data 91 | TRANSMIT_PACKET(true, false, 11, 0x42); 92 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x13, 0x0b, 0x42, 0xd3, 0x62, 0x7e); 93 | 94 | // response, server->client, link control, no data 95 | TRANSMIT_PACKET(true, true, 11); 96 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x17, 0x0b, 0x80, 0x36, 0x7e); 97 | 98 | // response, server->client, link control, 1 byte of data 99 | TRANSMIT_PACKET(true, true, 11, 0x42); 100 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x17, 0x0b, 0x42, 0x13, 0xbe, 0x7e); 101 | } 102 | 103 | TEST_F(LinkLayerTransmitClientTest, Valid) { 104 | // request, client->server, normal, no data 105 | TRANSMIT_PACKET(false, false, 11); 106 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x10, 0x0b, 0x17, 0xaf, 0x7e); 107 | 108 | // request, client->server, normal, 1 byte of data 109 | TRANSMIT_PACKET(false, false, 11, 0x42); 110 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x10, 0x0b, 0x42, 0x83, 0x3b, 0x7e); 111 | 112 | // request, client->server, link control, no data 113 | TRANSMIT_PACKET(false, true, 11); 114 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x14, 0x0b, 0xd3, 0x63, 0x7e); 115 | 116 | // request, client->server, link control, 1 byte of data 117 | TRANSMIT_PACKET(false, true, 11, 0x42); 118 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x14, 0x0b, 0x42, 0x43, 0xe7, 0x7e); 119 | 120 | // response, client->server, normal, no data 121 | TRANSMIT_PACKET(true, false, 11); 122 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x11, 0x0b, 0x26, 0x9c, 0x7e); 123 | 124 | // response, client->server, normal, 1 byte of data 125 | TRANSMIT_PACKET(true, false, 11, 0x42); 126 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x11, 0x0b, 0x42, 0xb3, 0x0c, 0x7e); 127 | 128 | // response, client->server, link control, no data 129 | TRANSMIT_PACKET(true, true, 11); 130 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x15, 0x0b, 0xe2, 0x50, 0x7e); 131 | 132 | // response, client->server, link control, 1 byte of data 133 | TRANSMIT_PACKET(true, true, 11, 0x42); 134 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x15, 0x0b, 0x42, 0x73, 0xd0, 0x7e); 135 | } 136 | 137 | TEST_F(LinkLayerTransmitClientTest, EncodingEscape) { 138 | // two flag data bytes 139 | TRANSMIT_PACKET(false, false, 11, 0x7e, 0x7e); 140 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x10, 0x0b, 0x7d, 0x5e, 0x7d, 0x5e, 0x99, 0xdb, 0x7e); 141 | 142 | // two flag data bytes with other data mixed in 143 | TRANSMIT_PACKET(false, false, 11, 0x11, 0x7e, 0x22, 0x7e, 0x33); 144 | EXPECT_AND_CLEAR_SENT_DATA(0x7e, 0x10, 0x0b, 0x11, 0x7d, 0x5e, 0x22, 0x7d, 0x5e, 0x33, 0xf3, 0x8e, 0x7e); 145 | } 146 | -------------------------------------------------------------------------------- /sonar/tests/test_server.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include "test_common.h" 4 | 5 | extern "C" { 6 | 7 | #include "anchor/sonar/server.h" 8 | #include "src/link_layer/timeouts.h" 9 | 10 | }; 11 | 12 | #define PROCESS_RECEIVE_PACKET(...) do { \ 13 | BUILD_PACKET_BUFFER(_buffer, __VA_ARGS__); \ 14 | sonar_server_process(handle_, _buffer, sizeof(_buffer)); \ 15 | } while (0) 16 | 17 | #define EXPECT_WRITE_PACKET(...) do { \ 18 | BUILD_PACKET_BUFFER(_buffer, __VA_ARGS__); \ 19 | EXPECT_TRUE(DataMatches(m_write_data, _buffer, sizeof(_buffer))); \ 20 | m_write_data.clear(); \ 21 | } while (0) 22 | 23 | SONAR_SERVER_ATTR_DEF(TestAttr, TEST_ATTR, 0xfff, sizeof(uint32_t), RWN); 24 | 25 | static sonar_server_handle_t m_handle; 26 | static std::vector m_write_data; 27 | static uint64_t m_system_time; 28 | static int m_num_connections; 29 | static int m_num_disconnections; 30 | static int m_attr_num_read; 31 | static int m_attr_num_write; 32 | static uint32_t m_attr_write_data; 33 | static int m_attr_num_notify_complete; 34 | static bool m_attr_notify_complete_success; 35 | 36 | static void write_byte(uint8_t byte) { 37 | m_write_data.push_back(byte); 38 | } 39 | 40 | static uint64_t get_system_time_ms(void) { 41 | return m_system_time; 42 | } 43 | 44 | static void connection_changed_callback(sonar_server_handle_t handle, bool connected) { 45 | if (connected) { 46 | m_num_connections++; 47 | } else { 48 | m_num_disconnections++; 49 | } 50 | } 51 | 52 | static bool TestAttr_write_handler(const void* data, uint32_t length) { 53 | m_attr_num_write++; 54 | if (length != sizeof(m_attr_write_data)) { 55 | return false; 56 | } 57 | memcpy(&m_attr_write_data, data, length); 58 | return true; 59 | } 60 | 61 | static uint32_t TestAttr_read_handler(void* response_data, uint32_t response_max_size) { 62 | m_attr_num_read++; 63 | if (response_max_size == sizeof(uint32_t)) { 64 | *(uint32_t*)response_data = 0x11223344; 65 | return sizeof(uint32_t); 66 | } else { 67 | return 0; 68 | } 69 | } 70 | 71 | static void attribute_notify_complete_handler(sonar_server_handle_t handle, bool success) { 72 | m_attr_num_notify_complete++; 73 | m_attr_notify_complete_success = success; 74 | } 75 | 76 | class ServerTest : public ::testing::Test { 77 | protected: 78 | void SetUp() override { 79 | m_write_data.clear(); 80 | m_system_time = 0; 81 | m_num_connections = 0; 82 | m_num_disconnections = 0; 83 | m_attr_num_read = 0; 84 | m_attr_num_write = 0; 85 | m_attr_num_notify_complete = 0; 86 | 87 | SONAR_SERVER_DEF(handle, 1024); 88 | handle_ = handle; 89 | m_handle = handle_; 90 | const sonar_server_init_t init_server = { 91 | .write_byte = write_byte, 92 | .get_system_time_ms = get_system_time_ms, 93 | .connection_changed_callback = connection_changed_callback, 94 | .attribute_notify_complete_handler = attribute_notify_complete_handler, 95 | }; 96 | sonar_server_init(handle, &init_server); 97 | } 98 | 99 | void TearDown() override { 100 | EXPECT_TRUE(m_write_data.empty()); 101 | EXPECT_EQ(m_num_connections, 0); 102 | EXPECT_EQ(m_num_disconnections, 0); 103 | EXPECT_EQ(m_attr_num_read, 0); 104 | EXPECT_EQ(m_attr_num_write, 0); 105 | EXPECT_EQ(m_attr_num_notify_complete, 0); 106 | } 107 | sonar_server_handle_t handle_; 108 | }; 109 | 110 | TEST_F(ServerTest, Idle) { 111 | // run process a bunch of times (longer than any of our timeouts) with no data coming in 112 | for (int i = 0; i < CONNECTION_TIMEOUT_MS; i++) { 113 | sonar_server_process(handle_, NULL, 0); 114 | m_system_time += 5; 115 | } 116 | } 117 | 118 | TEST_F(ServerTest, Disconnection) { 119 | // receive a connection request and make sure we're connected and send a response 120 | PROCESS_RECEIVE_PACKET(0x14, 0x00, 0x80); 121 | EXPECT_WRITE_PACKET(0x17, 0x00); 122 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 123 | EXPECT_EQ(m_num_connections, 1); 124 | m_num_connections = 0; 125 | 126 | // increment the system time by more than our connection timeout and make sure we disconnect 127 | m_system_time += CONNECTION_TIMEOUT_MS; 128 | sonar_server_process(handle_, NULL, 0); 129 | EXPECT_EQ(m_num_disconnections, 1); 130 | m_num_disconnections = 0; 131 | 132 | // receive a new connection request and make sure we're connected and send a response 133 | PROCESS_RECEIVE_PACKET(0x14, 0x01, 0x80); 134 | EXPECT_WRITE_PACKET(0x17, 0x01); 135 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 136 | EXPECT_EQ(m_num_connections, 1); 137 | m_num_connections = 0; 138 | } 139 | 140 | TEST_F(ServerTest, Read) { 141 | // register our attribute 142 | sonar_server_register(handle_, TEST_ATTR); 143 | 144 | // connect (also tested by ServerTest.Connection) 145 | PROCESS_RECEIVE_PACKET(0x14, 0x00, 0x80); 146 | EXPECT_WRITE_PACKET(0x17, 0x00); 147 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 148 | EXPECT_EQ(m_num_connections, 1); 149 | m_num_connections = 0; 150 | 151 | // process a read request and make sure we send a response 152 | PROCESS_RECEIVE_PACKET(0x10, 0x01, 0xff, 0x1f); 153 | EXPECT_WRITE_PACKET(0x13, 0x01, 0x44, 0x33, 0x22, 0x11); 154 | EXPECT_EQ(m_attr_num_read, 1); 155 | m_attr_num_read = 0; 156 | } 157 | 158 | TEST_F(ServerTest, Write) { 159 | // register our attribute 160 | sonar_server_register(handle_, TEST_ATTR); 161 | 162 | // connect (also tested by ServerTest.Connection) 163 | PROCESS_RECEIVE_PACKET(0x14, 0x00, 0x80); 164 | EXPECT_WRITE_PACKET(0x17, 0x00); 165 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 166 | EXPECT_EQ(m_num_connections, 1); 167 | m_num_connections = 0; 168 | 169 | // process a write request and make sure we send a response 170 | PROCESS_RECEIVE_PACKET(0x10, 0x01, 0xff, 0x2f, 0x40, 0x30, 0x20, 0x10); 171 | EXPECT_WRITE_PACKET(0x13, 0x01); 172 | EXPECT_EQ(m_attr_write_data, 0x10203040); 173 | EXPECT_EQ(m_attr_num_write, 1); 174 | m_attr_num_write = 0; 175 | } 176 | 177 | TEST_F(ServerTest, Notify) { 178 | // register our attribute 179 | sonar_server_register(handle_, TEST_ATTR); 180 | 181 | // connect (also tested by ServerTest.Connection) 182 | PROCESS_RECEIVE_PACKET(0x14, 0x00, 0x80); 183 | EXPECT_WRITE_PACKET(0x17, 0x00); 184 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 185 | EXPECT_EQ(m_num_connections, 1); 186 | m_num_connections = 0; 187 | 188 | // send a notify request 189 | const uint32_t data1 = 0x01020304; 190 | EXPECT_TRUE(sonar_server_notify(handle_, TEST_ATTR, &data1, sizeof(data1))); 191 | EXPECT_WRITE_PACKET(0x12, 0x80, 0xff, 0x3f, 0x04, 0x03, 0x02, 0x1); 192 | 193 | // time-out on receiving the response 194 | m_system_time += REQUEST_TIMEOUT_MS; 195 | sonar_server_process(handle_, NULL, 0); 196 | EXPECT_FALSE(m_attr_notify_complete_success); 197 | EXPECT_EQ(m_attr_num_notify_complete, 1); 198 | m_attr_num_notify_complete = 0; 199 | 200 | // send another notify request 201 | const uint32_t data2 = 0x01020304; 202 | EXPECT_TRUE(sonar_server_notify(handle_, TEST_ATTR, &data2, sizeof(data2))); 203 | EXPECT_WRITE_PACKET(0x12, 0x81, 0xff, 0x3f, 0x04, 0x03, 0x02, 0x1); 204 | 205 | // process the response 206 | PROCESS_RECEIVE_PACKET(0x11, 0x81); 207 | EXPECT_TRUE(m_attr_notify_complete_success); 208 | EXPECT_EQ(m_attr_num_notify_complete, 1); 209 | m_attr_num_notify_complete = 0; 210 | } 211 | 212 | TEST_F(ServerTest, ConcurrentReadNotify) { 213 | // register our attribute 214 | sonar_server_register(handle_, TEST_ATTR); 215 | 216 | // connect (also tested by ServerTest.Connection) 217 | PROCESS_RECEIVE_PACKET(0x14, 0x00, 0x80); 218 | EXPECT_WRITE_PACKET(0x17, 0x00); 219 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 220 | EXPECT_EQ(m_num_connections, 1); 221 | m_num_connections = 0; 222 | 223 | // send a notify request 224 | const uint32_t data1 = 0x01020304; 225 | EXPECT_TRUE(sonar_server_notify(handle_, TEST_ATTR, &data1, sizeof(data1))); 226 | EXPECT_WRITE_PACKET(0x12, 0x80, 0xff, 0x3f, 0x04, 0x03, 0x02, 0x1); 227 | 228 | // retry the notify request 229 | m_system_time += REQUEST_RETRY_INTERVAL_MS; 230 | sonar_server_process(handle_, NULL, 0); 231 | EXPECT_WRITE_PACKET(0x12, 0x80, 0xff, 0x3f, 0x04, 0x03, 0x02, 0x1); 232 | 233 | // process a read request and make sure we send a response with the correct data from the read handler 234 | PROCESS_RECEIVE_PACKET(0x10, 0x01, 0xff, 0x1f); 235 | EXPECT_WRITE_PACKET(0x13, 0x01, 0x44, 0x33, 0x22, 0x11); 236 | EXPECT_EQ(m_attr_num_read, 1); 237 | m_attr_num_read = 0; 238 | 239 | // retry the notify request again (should still send the original notify data and not the data we just read) 240 | m_system_time += REQUEST_RETRY_INTERVAL_MS; 241 | sonar_server_process(handle_, NULL, 0); 242 | EXPECT_WRITE_PACKET(0x12, 0x80, 0xff, 0x3f, 0x04, 0x03, 0x02, 0x1); 243 | 244 | // process the notify response 245 | PROCESS_RECEIVE_PACKET(0x11, 0x80); 246 | EXPECT_TRUE(m_attr_notify_complete_success); 247 | EXPECT_EQ(m_attr_num_notify_complete, 1); 248 | m_attr_num_notify_complete = 0; 249 | } 250 | 251 | TEST_F(ServerTest, NotifyReadData) { 252 | // register our attribute 253 | sonar_server_register(handle_, TEST_ATTR); 254 | 255 | // connect (also tested by ServerTest.Connection) 256 | PROCESS_RECEIVE_PACKET(0x14, 0x00, 0x80); 257 | EXPECT_WRITE_PACKET(0x17, 0x00); 258 | EXPECT_TRUE(sonar_server_is_connected(handle_)); 259 | EXPECT_EQ(m_num_connections, 1); 260 | m_num_connections = 0; 261 | 262 | // send a notify request based on the read data 263 | EXPECT_TRUE(sonar_server_notify_read_data(handle_, TEST_ATTR)); 264 | EXPECT_WRITE_PACKET(0x12, 0x80, 0xff, 0x3f, 0x44, 0x33, 0x22, 0x11); 265 | EXPECT_EQ(m_attr_num_read, 1); 266 | m_attr_num_read = 0; 267 | 268 | // process the notify response 269 | PROCESS_RECEIVE_PACKET(0x11, 0x80); 270 | EXPECT_TRUE(m_attr_notify_complete_success); 271 | EXPECT_EQ(m_attr_num_notify_complete, 1); 272 | m_attr_num_notify_complete = 0; 273 | } 274 | --------------------------------------------------------------------------------