Initial Release

src/common/CMakeLists.txt

@@ -0,0 +1,16 @@
+pico_add_subdirectory(boot_picoboot)
+pico_add_subdirectory(boot_uf2)
+pico_add_subdirectory(pico_base)
+
+# PICO_CMAKE_CONFIG: PICO_BARE_METAL, Flag to exclude anything except base headers from the build, type=bool, default=0, group=build
+if (NOT PICO_BARE_METAL)
+    pico_add_subdirectory(pico_bit_ops)
+    pico_add_subdirectory(pico_binary_info)
+    pico_add_subdirectory(pico_divider)
+    pico_add_subdirectory(pico_sync)
+    pico_add_subdirectory(pico_time)
+    pico_add_subdirectory(pico_util)
+    pico_add_subdirectory(pico_stdlib)
+endif()
+
+pico_add_doxygen(${CMAKE_CURRENT_LIST_DIR})

src/common/README.md

@@ -0,0 +1,3 @@
+This directory code that is common to all builds regardless of `PICO_PLATFORM`. It is a mix
+of common header files, or high level functionality built entirely using `hardware_` or `pico_` libraries provided
+by the actual target `PICO_PLATFORM`` 

src/common/boot_picoboot/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_library(boot_picoboot_headers INTERFACE)
+target_include_directories(boot_picoboot_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)

src/common/boot_picoboot/include/boot/picoboot.h

@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT_PICOBOOT_H
+#define _BOOT_PICOBOOT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <assert.h>
+
+#ifndef NO_PICO_PLATFORM
+#include "pico/platform.h"
+#endif
+
+/** \file picoboot.h
+*  \defgroup boot_picoboot boot_picoboot
+*
+* Header file for the PICOBOOT USB interface exposed by an RP2040 in BOOTSEL mode.
+*/
+
+#define PICOBOOT_MAGIC 0x431fd10bu
+
+// --------------------------------------------
+// CONTROL REQUESTS FOR THE PICOBOOT INTERFACE
+// --------------------------------------------
+
+// size 0 OUT - unstall EPs and reset
+#define PICOBOOT_IF_RESET 0x41
+
+// size 16 IN - return the status of the last command
+#define PICOBOOT_IF_CMD_STATUS 0x42
+
+// --------------------------------------------------
+// COMMAND REQUESTS SENT TO THE PICOBOOT OUT ENDPOINT
+// --------------------------------------------------
+//
+// picoboot_cmd structure of size 32 is sent to OUT endpoint
+// transfer_length bytes are transferred via IN/OUT
+// device responds on success with 0 length ACK packet set via OUT/IN
+// device may stall the transferring endpoint in case of error
+
+enum picoboot_cmd_id {
+    PC_EXCLUSIVE_ACCESS = 0x1,
+    PC_REBOOT = 0x2,
+    PC_FLASH_ERASE = 0x3,
+    PC_READ = 0x84, // either RAM or FLASH
+    PC_WRITE = 5, // either RAM or FLASH (does no erase)
+    PC_EXIT_XIP = 0x6,
+    PC_ENTER_CMD_XIP = 0x7,
+    PC_EXEC = 0x8,
+    PC_VECTORIZE_FLASH = 0x9
+};
+
+enum picoboot_status {
+    PICOBOOT_OK = 0,
+    PICOBOOT_UNKNOWN_CMD = 1,
+    PICOBOOT_INVALID_CMD_LENGTH = 2,
+    PICOBOOT_INVALID_TRANSFER_LENGTH = 3,
+    PICOBOOT_INVALID_ADDRESS = 4,
+    PICOBOOT_BAD_ALIGNMENT = 5,
+    PICOBOOT_INTERLEAVED_WRITE = 6,
+    PICOBOOT_REBOOTING = 7,
+    PICOBOOT_UNKNOWN_ERROR = 8,
+};
+
+struct __packed picoboot_reboot_cmd {
+    uint32_t dPC; // 0 means reset into bootrom
+    uint32_t dSP;
+    uint32_t dDelayMS;
+};
+
+// used for EXEC, VECTORIZE_FLASH
+struct __packed picoboot_address_only_cmd {
+    uint32_t dAddr;
+};
+
+// used for READ, WRITE, FLASH_ERASE
+struct __packed picoboot_range_cmd {
+    uint32_t dAddr;
+    uint32_t dSize;
+};
+
+enum picoboot_exclusive_type {
+    NOT_EXCLUSIVE = 0,
+    EXCLUSIVE,
+    EXCLUSIVE_AND_EJECT
+};
+
+struct __packed picoboot_exclusive_cmd {
+    uint8_t bExclusive;
+};
+
+// little endian
+struct __packed __aligned(4) picoboot_cmd {
+    uint32_t dMagic;
+    uint32_t dToken; // an identifier for this token to correlate with a status response
+    uint8_t bCmdId; // top bit set for IN
+    uint8_t bCmdSize; // bytes of actual data in the arg part of this structure
+    uint16_t _unused;
+    uint32_t dTransferLength; // length of IN/OUT transfer (or 0) if none
+    union {
+        uint8_t args[16];
+        struct picoboot_reboot_cmd reboot_cmd;
+        struct picoboot_range_cmd range_cmd;
+        struct picoboot_address_only_cmd address_only_cmd;
+        struct picoboot_exclusive_cmd exclusive_cmd;
+    };
+};
+
+static_assert(32 == sizeof(struct picoboot_cmd), "picoboot_cmd must be 32 bytes big");
+
+struct __packed __aligned(4) picoboot_cmd_status {
+    uint32_t dToken;
+    uint32_t dStatusCode;
+    uint8_t bCmdId;
+    uint8_t bInProgress;
+    uint8_t _pad[6];
+};
+
+static_assert(16 == sizeof(struct picoboot_cmd_status), "picoboot_cmd_status must be 16 bytes big");
+#endif

src/common/boot_uf2/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_library(boot_uf2_headers INTERFACE)
+target_include_directories(boot_uf2_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)

src/common/boot_uf2/include/boot/uf2.h

@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT_UF2_H
+#define _BOOT_UF2_H
+
+#include <stdint.h>
+#include <assert.h>
+
+/** \file uf2.h
+*  \defgroup boot_uf2 boot_uf2
+*
+* Header file for the UF2 format supported by an RP2040 in BOOTSEL mode.
+*/
+
+#define UF2_MAGIC_START0 0x0A324655u
+#define UF2_MAGIC_START1 0x9E5D5157u
+#define UF2_MAGIC_END    0x0AB16F30u
+
+#define UF2_FLAG_NOT_MAIN_FLASH     0x00000001u
+#define UF2_FLAG_FILE_CONTAINER     0x00001000u
+#define UF2_FLAG_FAMILY_ID_PRESENT  0x00002000u
+#define UF2_FLAG_MD5_PRESENT        0x00004000u
+
+#define RP2040_FAMILY_ID 0xe48bff56
+
+struct uf2_block {
+    // 32 byte header
+    uint32_t magic_start0;
+    uint32_t magic_start1;
+    uint32_t flags;
+    uint32_t target_addr;
+    uint32_t payload_size;
+    uint32_t block_no;
+    uint32_t num_blocks;
+    uint32_t file_size; // or familyID;
+    uint8_t  data[476];
+    uint32_t magic_end;
+};
+
+static_assert(sizeof(struct uf2_block) == 512, "uf2_block not sector sized");
+
+#endif

src/common/pico_base/CMakeLists.txt

@@ -0,0 +1,40 @@
+if (NOT TARGET pico_base_headers)
+    # build the auto gen config headers
+
+    set(header_content "// AUTOGENERATED FROM PICO_CONFIG_HEADER_FILES and then PICO_<PLATFORM>_CONFIG_HEADER_FILES\n// DO NOT EDIT!\n")
+    string(TOUPPER ${PICO_PLATFORM} PICO_PLATFORM_UPPER)
+
+    macro(add_header_content_from_var VAR)
+        set(header_content "${header_content}\n\n// based on ${VAR}:\n")
+        foreach(var IN LISTS ${VAR})
+            set(header_content "${header_content}\n#include \"${var}\"")
+        endforeach()
+    endmacro()
+
+    # PICO_CMAKE_CONFIG: PICO_CONFIG_HEADER_FILES, List of extra header files to include from pico/config.h for all platforms, type=list, default="", group=pico_base
+    add_header_content_from_var(PICO_CONFIG_HEADER_FILES)
+
+    # PICO_CMAKE_CONFIG: PICO_CONFIG_RP2040_HEADER_FILES, List of extra header files to include from pico/config.h for rp2040 platform, type=list, default="", group=pico_base
+    # PICO_CMAKE_CONFIG: PICO_CONFIG_HOST_HEADER_FILES, List of extra header files to include from pico/config.h for host platform, type=list, default="", group=pico_base
+    add_header_content_from_var(PICO_${PICO_PLATFORM_UPPER}_CONFIG_HEADER_FILES)
+
+    file(GENERATE
+            OUTPUT  ${CMAKE_BINARY_DIR}/generated/pico_base/pico/config_autogen.h
+            CONTENT "${header_content}"
+    )
+
+    configure_file( include/pico/version.h.in ${CMAKE_BINARY_DIR}/generated/pico_base/pico/version.h)
+
+    add_library(pico_base_headers INTERFACE)
+    target_include_directories(pico_base_headers INTERFACE include ${CMAKE_BINARY_DIR}/generated/pico_base)
+
+    foreach(DIR IN LISTS PICO_INCLUDE_DIRS)
+        target_include_directories(pico_base_headers INTERFACE ${DIR})
+    endforeach()
+
+    # PICO_BUILD_DEFINE: PICO_BOARD, Name of board, type=string, default=CMake PICO_BOARD variable, group=pico_base
+    target_compile_definitions(pico_base_headers INTERFACE
+            PICO_BOARD="${PICO_BOARD}")
+
+    target_link_libraries(pico_base_headers INTERFACE pico_platform_headers)
+endif()

src/common/pico_base/include/pico.h

@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PICO_H_
+#define PICO_H_
+
+/** \file pico.h
+*  \defgroup pico_base pico_base
+*
+* Core types and macros for the Pico SDK. This header is intended to be included by all source code
+*/
+
+#include "pico/types.h"
+#include "pico/version.h"
+#include "pico/config.h"
+#include "pico/platform.h"
+#include "pico/assert.h"
+#include "pico/error.h"
+
+#endif

src/common/pico_base/include/pico/assert.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_ASSERT_H
+#define _PICO_ASSERT_H
+
+#include "pico/types.h"
+
+#ifdef __cplusplus
+
+#include <cassert>
+
+extern "C" {
+#else
+#include <assert.h>
+#endif
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLE_ALL, Global assert enable, type=bool, default=0, group=pico_base
+// PICO_CONFIG: PARAM_ASSERTIONS_DISABLE_ALL, Global assert disable, type=bool, default=0, group=pico_base
+
+#ifndef PARAM_ASSERTIONS_ENABLE_ALL
+#define PARAM_ASSERTIONS_ENABLE_ALL 0
+#endif
+
+#ifndef PARAM_ASSERTIONS_DISABLE_ALL
+#define PARAM_ASSERTIONS_DISABLE_ALL 0
+#endif
+
+#define PARAM_ASSERTIONS_ENABLED(x) ((PARAM_ASSERTIONS_ENABLED_ ## x || PARAM_ASSERTIONS_ENABLE_ALL) && !PARAM_ASSERTIONS_DISABLE_ALL)
+
+#define invalid_params_if(x, test) ({if (PARAM_ASSERTIONS_ENABLED(x)) assert(!(test));})
+#define valid_params_if(x, test) ({if (PARAM_ASSERTIONS_ENABLED(x)) assert(test);})
+#define hard_assert_if(x, test) ({if (PARAM_ASSERTIONS_ENABLED(x)) hard_assert(!(test));})
+
+#ifdef NDEBUG
+extern void hard_assertion_failure();
+static inline void hard_assert(bool condition, ...) {
+    if (!condition)
+        hard_assertion_failure();
+}
+#else
+#define hard_assert assert
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_base/include/pico/config.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PICO_CONFIG_H_
+#define PICO_CONFIG_H_
+
+// -----------------------------------------------------
+// NOTE: THIS HEADER IS ALSO INCLUDED BY ASSEMBLER SO
+//       SHOULD ONLY CONSIST OF PREPROCESSOR DIRECTIVES
+//       OR USE #ifndef __ASSEMBLER__ guards
+// -------------
+
+// PICO_CONFIG_HEADER_FILES and then PICO_SDK_<PLATFORM>_CONFIG_INCLUDE_FILES
+// entries are dumped in order at build time into this generated header
+
+#include "pico/config_autogen.h"
+
+#endif

src/common/pico_base/include/pico/error.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_ERROR_H
+#define _PICO_ERROR_H
+
+/*!
+ * Common return codes from pico_sdk methods that return a status
+ */
+enum {
+    PICO_OK = 0,
+    PICO_ERROR_NONE = 0,
+    PICO_ERROR_TIMEOUT = -1,
+    PICO_ERROR_GENERIC = -2,
+    PICO_ERROR_NO_DATA = -3,
+};
+
+#endif

src/common/pico_base/include/pico/types.h

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_TYPES_H
+#define _PICO_TYPES_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+typedef unsigned int uint;
+
+#ifdef NDEBUG
+/*! \typedef absolute_time_t
+    \brief An opaque 64 bit timestamp in microseconds
+
+    The type is used instead of a raw uint64_t to prevent accidentally passing relative times or times in the wrong
+    time units where an absolute time is required. It is equivalent to uint64_t in release builds.
+
+    \see to_us_since_boot
+    \see update_us_since_boot
+*/
+typedef uint64_t absolute_time_t;
+
+/*! fn to_us_since_boot
+ * \brief convert an absolute_time_t into a number of microseconds since boot.
+ * \param t the number of microseconds since boot
+ * \return an absolute_time_t value equivalent to t
+ */
+static inline uint64_t to_us_since_boot(absolute_time_t t) {
+    return t;
+}
+
+/*! fn update_us_since_boot
+ * \brief update an absolute_time_t value to represent a given number of microseconds since boot
+ * \param t the absolute time value to update
+ * \param us_since_boot the number of microseconds since boot to represent
+ */
+static inline void update_us_since_boot(absolute_time_t *t, uint64_t us_since_boot) {
+    *t = us_since_boot;
+}
+
+#define ABSOLUTE_TIME_INITIALIZED_VAR(name, value) name = value
+#else
+typedef struct {
+    uint64_t _private_us_since_boot;
+} absolute_time_t;
+
+static inline uint64_t to_us_since_boot(absolute_time_t t) {
+    return t._private_us_since_boot;
+}
+
+static inline void update_us_since_boot(absolute_time_t *t, uint64_t us_since_boot) {
+    t->_private_us_since_boot = us_since_boot;
+}
+#define ABSOLUTE_TIME_INITIALIZED_VAR(name, value) name = {value}
+#endif
+
+/** \struct datetime_t
+ *  \ingroup util_datetime
+ *  \brief Structure containing date and time information
+ *
+ *    When setting an RTC alarm, set a field to -1 tells
+ *    the RTC to not match on this field
+ */
+typedef struct {
+    int16_t year;    ///< 0..4095
+    int8_t month;    ///< 1..12, 1 is January
+    int8_t day;      ///< 1..28,29,30,31 depending on month
+    int8_t dotw;     ///< 0..6, 0 is Sunday
+    int8_t hour;     ///< 0..23
+    int8_t min;      ///< 0..59
+    int8_t sec;      ///< 0..59
+} datetime_t;
+
+#endif

src/common/pico_base/include/pico/version.h.in

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+// ---------------------------------------
+// THIS FILE IS AUTOGENERATED; DO NOT EDIT
+// ---------------------------------------
+
+#ifndef _PICO_VERSION_H
+#define _PICO_VERSION_H
+
+#define PICO_SDK_VERSION_MAJOR    ${PICO_SDK_VERSION_MAJOR}
+#define PICO_SDK_VERSION_MINOR    ${PICO_SDK_VERSION_MINOR}
+#define PICO_SDK_VERSION_REVISION ${PICO_SDK_VERSION_REVISION}
+#define PICO_SDK_VERSION_STRING   "${PICO_SDK_VERSION_STRING}"
+
+#endif

src/common/pico_binary_info/CMakeLists.txt

@@ -0,0 +1,30 @@
+add_library(pico_binary_info_headers INTERFACE)
+
+target_include_directories(pico_binary_info_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+add_library(pico_binary_info INTERFACE)
+
+target_link_libraries(pico_binary_info INTERFACE pico_binary_info_headers)
+
+function(pico_set_program_name TARGET name)
+    # PICO_BUILD_DEFINE: PICO_PROGRAM_NAME, value passed to pico_set_program_name, type=string, default=none, group=pico_binary_info
+    target_compile_definitions(${TARGET} PRIVATE -DPICO_PROGRAM_NAME="${name}")
+endfunction()
+
+function(pico_set_program_description TARGET description)
+    # since this is the command line, we will remove newlines
+    string(REPLACE "\n" " " description ${description})
+    string(REPLACE "\"" "\\\"" description ${description})
+    # PICO_BUILD_DEFINE: PICO_PROGRAM_DESCRIPTION, value passed to pico_set_program_description, type=string, default=none, group=pico_binary_info
+    target_compile_definitions(${TARGET} PRIVATE -DPICO_PROGRAM_DESCRIPTION="${description}")
+endfunction()
+
+function(pico_set_program_url TARGET url)
+    # PICO_BUILD_DEFINE: PICO_PROGRAM_URL, value passed to pico_set_program_url, type=string, default=none, group=pico_binary_info
+    target_compile_definitions(${TARGET} PRIVATE -DPICO_PROGRAM_URL="${url}")
+endfunction()
+
+function(pico_set_program_version TARGET version)
+    # PICO_BUILD_DEFINE: PICO_PROGRAM_VERSION_STRING, value passed to pico_set_program_version, type=string, default=none, group=pico_binary_info
+    target_compile_definitions(${TARGET} PRIVATE -DPICO_PROGRAM_VERSION_STRING="${version}")
+endfunction()

src/common/pico_binary_info/include/pico/binary_info.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_BINARY_INFO_H
+#define _PICO_BINARY_INFO_H
+
+/**
+ * Binary info is intended for embedding machine readable information with the binary in FLASH.
+ *
+ * Example uses include:
+ *
+ * - Program identification / information
+ * - Pin layouts
+ * - Included features
+ * - Identifying flash regions used as block devices/storage
+ */
+
+#include "pico/binary_info/defs.h"
+#include "pico/binary_info/structure.h"
+#if PICO_ON_DEVICE
+#include "pico/binary_info/code.h"
+#endif
+
+
+#endif

src/common/pico_binary_info/include/pico/binary_info/code.h

@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_BINARY_INFO_CODE_H
+#define _PICO_BINARY_INFO_CODE_H
+
+#include "pico.h"
+#include "pico/binary_info/structure.h"
+
+#if !PICO_NO_BINARY_INFO
+#define __bi_decl(name, bi, section_prefix, attr) static const attr __attribute__((section(section_prefix __STRING(name)))) struct _binary_info_core *name = bi
+#define __bi_lineno_var_name __CONCAT(__bi_, __LINE__)
+#define __bi_ptr_lineno_var_name __CONCAT(__bi_ptr, __LINE__)
+#define __bi_enclosure_check_lineno_var_name __CONCAT(_error_bi_is_missing_enclosing_decl_,__LINE__)
+#define __bi_mark_enclosure static const __unused int __bi_enclosure_check_lineno_var_name=0;
+#if !defined(__GNUC__) || __cplusplus || __GNUC__ >= 8
+#define __bi_enclosure_check(x) (x + __bi_enclosure_check_lineno_var_name)
+#else
+// skip the version check on older GCC non C++, as it doesn't compile.. this is only here to catch the
+// user accidentally forgetting to enclose the binary item with bi_decl
+#define __bi_enclosure_check(x) (x)
+#endif
+/**
+ * Declare some binary information that will be included if the contain source file/line is compiled into the binary
+ */
+#define bi_decl(_decl) __bi_mark_enclosure _decl; __bi_decl(__bi_ptr_lineno_var_name, &__bi_lineno_var_name.core, ".binary_info.keep.", __used);
+/**
+ * Declare some binary information that will be included if the function containing the decl is linked into the binary.
+ * The SDK uses --gc-sections, so functions that are never called will be removed by the linker, and any associated
+ * binary information declared this way will also be stripped
+ */
+#define bi_decl_if_func_used(_decl) ({__bi_mark_enclosure _decl; __bi_decl(__bi_ptr_lineno_var_name, &__bi_lineno_var_name.core, ".binary_info.", ); *(volatile uint8_t *)&__bi_ptr_lineno_var_name;});
+
+#define bi_decl_with_attr(_decl, _attr) __bi_mark_enclosure _attr _decl; __bi_decl(__bi_ptr_lineno_var_name, &__bi_lineno_var_name.core, ".binary_info.keep.", __used);
+#define bi_decl_if_func_used_with_attr(_decl, _attr) ({__bi_mark_enclosure _attr _decl; __bi_decl(__bi_ptr_lineno_var_name, &__bi_lineno_var_name.core, ".binary_info.", ); *(volatile uint8_t *)&__bi_ptr_lineno_var_name;});
+#else
+#define __bi_decl(bi, name, attr)
+#define bi_decl_with_attr(_decl, _attr)
+#define bi_decl(_decl)
+#define bi_decl_if_func_used_with_attr(_decl, _attr) ((void)0);
+#define bi_decl_if_func_used(_decl) ((void)0);
+#endif
+
+#define bi_int(_tag, _id, _value) \
+     static const struct _binary_info_id_and_int __bi_lineno_var_name = { \
+        .core = { \
+            .type = __bi_enclosure_check(BINARY_INFO_TYPE_ID_AND_INT), \
+            .tag = _tag, \
+        },\
+        .id = _id, \
+        .value = _value \
+    };
+
+#define bi_string(_tag, _id, _value) \
+    static const struct _binary_info_id_and_string __bi_lineno_var_name = { \
+        .core = { \
+            .type = __bi_enclosure_check(BINARY_INFO_TYPE_ID_AND_STRING), \
+            .tag = _tag, \
+        },\
+        .id = _id, \
+        .value = _value, \
+    }
+
+#define bi_block_device(_tag, _name, _address, _size, _extra, _flags) \
+    static const struct _binary_info_block_device __bi_lineno_var_name = { \
+        .core = { \
+            .type = __bi_enclosure_check(BINARY_INFO_TYPE_BLOCK_DEVICE), \
+            .tag = _tag, \
+        },\
+        .name = _name, \
+        .address = _address, \
+        .size = _size, \
+        .extra = _extra, \
+        .flags = _flags, \
+    }
+
+#define __bi_encoded_pins_with_func(_encoding) \
+    static const struct _binary_info_pins_with_func __bi_lineno_var_name = { \
+        .core = { \
+            .type = __bi_enclosure_check(BINARY_INFO_TYPE_PINS_WITH_FUNC), \
+            .tag = BINARY_INFO_TAG_RASPBERRY_PI, \
+        },\
+        .pin_encoding = _encoding \
+    }
+
+#define __bi_pins_with_name(_mask, _label) \
+    static const struct _binary_info_pins_with_name __bi_lineno_var_name = { \
+        .core = { \
+            .type = __bi_enclosure_check(BINARY_INFO_TYPE_PINS_WITH_NAME), \
+            .tag = BINARY_INFO_TAG_RASPBERRY_PI, \
+        },\
+        .pin_mask = _mask, \
+        .label = _label \
+    }
+
+#define __bi_named_group(_parent_tag, _parent_id, _group_tag, _group_id, _label, _flags) \
+static const struct _binary_info_named_group __bi_lineno_var_name = { \
+        .core = { \
+            .type = __bi_enclosure_check(BINARY_INFO_TYPE_NAMED_GROUP), \
+            .tag = _parent_tag, \
+        },\
+        .parent_id = _parent_id, \
+        .group_tag = _group_tag, \
+        .flags = _flags, \
+        .group_id = _group_id, \
+        .label = _label \
+    }
+
+#define bi_binary_end(end) bi_int(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_BINARY_END, end)
+#define bi_program_name(name) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_NAME, name)
+#define bi_program_description(description) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_DESCRIPTION, description)
+#define bi_program_version_string(version_string) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_VERSION_STRING, version_string)
+#define bi_program_build_date_string(date_string) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_BUILD_DATE_STRING, date_string)
+#define bi_program_url(url) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_URL, url)
+// multiple of these may be added
+#define bi_program_feature(feature) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_FEATURE, feature)
+#define bi_program_build_attribute(attr) bi_string(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_BUILD_ATTRIBUTE, attr)
+#define bi_program_feature_group(tag, id, name) __bi_named_group(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_FEATURE, tag, id, name, 0)
+#define bi_program_feature_group_with_flags(tag, id, name, flags) __bi_named_group(BINARY_INFO_TAG_RASPBERRY_PI, BINARY_INFO_ID_RP_PROGRAM_FEATURE, tag, id, name, flags)
+
+#define bi_1pin_with_func(p0, func)                  __bi_encoded_pins_with_func(BI_PINS_ENCODING_MULTI | ((func << 3)) | ((p0) << 7) | ((p0) << 12))
+#define bi_2pins_with_func(p0, p1, func)             __bi_encoded_pins_with_func(BI_PINS_ENCODING_MULTI | ((func << 3)) | ((p0) << 7) | ((p1) << 12) | ((p1) << 17))
+#define bi_3pins_with_func(p0, p1, p2, func)         __bi_encoded_pins_with_func(BI_PINS_ENCODING_MULTI | ((func << 3)) | ((p0) << 7) | ((p1) << 12) | ((p2) << 17) | ((p2) << 22))
+#define bi_4pins_with_func(p0, p1, p2, p3, func)     __bi_encoded_pins_with_func(BI_PINS_ENCODING_MULTI | ((func << 3)) | ((p0) << 7) | ((p1) << 12) | ((p2) << 17) | ((p3) << 22) | ((p3) << 27))
+#define bi_5pins_with_func(p0, p1, p2, p3, p4, func) __bi_encoded_pins_with_func(BI_PINS_ENCODING_MULTI | ((func << 3)) | ((p0) << 7) | ((p1) << 12) | ((p2) << 17) | ((p3) << 22) | ((p4) << 27))
+#define bi_pin_range_with_func(plo, phi, func)       __bi_encoded_pins_with_func(BI_PINS_ENCODING_RANGE | ((func << 3)) | ((plo) << 7) | ((phi) << 12))
+
+#define bi_pin_mask_with_name(pmask, label)          __bi_pins_with_name((pmask), (label))
+// names are sperated by | ... i.e. "name1|name2|name3"
+#define bi_pin_mask_with_names(pmask, label)          __bi_pins_with_name((pmask), (label))
+#define bi_1pin_with_name(p0, name)                   bi_pin_mask_with_name(1u << (p0), name)
+#define bi_2pins_with_names(p0, name0, p1, name1)     bi_pin_mask_with_names((1u << (p0)) | (1u << (p1)), name0 "|" name1)
+#define bi_3pins_with_names(p0, name0, p1, name1, p2, name2)  bi_pin_mask_with_names((1u << (p0)) | (1u << (p1)) | (1u << (p2)), name0 "|" name1 "|" name2)
+#define bi_4pins_with_names(p0, name0, p1, name1, p2, name2, p3, name3)  bi_pin_mask_with_names((1u << (p0)) | (1u << (p1)) | (1u << (p2)) | (1u << (p3)), name0 "|" name1 "|" name2 "|" name3)
+
+#endif

src/common/pico_binary_info/include/pico/binary_info/defs.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_BINARY_INFO_DEFS_H
+#define _PICO_BINARY_INFO_DEFS_H
+
+// this file is for pre-processor definitions only
+
+// should be found within the first 256 bytes of the real binary (i.e. after the flash second stage if a flash binary)
+//
+// Note the layout is:
+//
+// addr      : BINARY_INFO_MARKER_START
+// addr+0x04 : __binary_info_start
+// addr+0x08 : __binary_info_end
+// addr+0x0c : __address_mapping_table
+// addr+0x10 | BINARY_INFO_MARKER_END
+//
+// __binary_info_start to __binary_info_end are the start, end (non inclusive) of an array
+// of pointers to binary_info_t structures
+//
+// __address_mapping_table is an array of the following items:
+//
+// uint32_t source_addr_start
+// uint32_t dest_addr_start
+// uint32_t dest_addr_end
+//
+// representing a mapping from the stored address in the binary/flash to addresses at runtime.
+// The linker will store pointers within the binary using their runtime values, however because of
+// "AT" mapping in the link script these addresses actually correspond to a different address in the binary
+// image. This mapping (which in the case of crt0.S is simply the data copy table used at initialization
+// to copy data into it's runtime location) can be used by picotool or others to reverse the mapping to find data
+// within the binary.
+//
+// Note the above array is terminated with a NULL source_addr_start
+
+#define BINARY_INFO_MARKER_START 0x7188ebf2
+#define BINARY_INFO_MARKER_END 0xe71aa390
+
+#endif

src/common/pico_binary_info/include/pico/binary_info/structure.h

@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_BINARY_INFO_STRUCTURE_H
+#define _PICO_BINARY_INFO_STRUCTURE_H
+
+// NOTE: This file may be included by non SDK code, so does not use SDK includes
+
+// NOTE: ALL CHANGES MUST BE BACKWARDS COMPATIBLE
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#ifndef __packed
+#define __packed __attribute__((packed))
+#endif
+
+typedef struct _binary_info_core binary_info_t;
+
+#define BINARY_INFO_TYPE_RAW_DATA 1
+#define BINARY_INFO_TYPE_SIZED_DATA 2
+#define BINARY_INFO_TYPE_BINARY_INFO_LIST_ZERO_TERMINATED 3
+#define BINARY_INFO_TYPE_BSON 4
+#define BINARY_INFO_TYPE_ID_AND_INT 5
+#define BINARY_INFO_TYPE_ID_AND_STRING 6
+// traditional block device
+#define BINARY_INFO_TYPE_BLOCK_DEVICE 7
+#define BINARY_INFO_TYPE_PINS_WITH_FUNC 8
+#define BINARY_INFO_TYPE_PINS_WITH_NAME 9
+#define BINARY_INFO_TYPE_PINS_WITH_NAMES 9
+#define BINARY_INFO_TYPE_NAMED_GROUP 10
+
+// note plan is to reserve c1 = 0->31 for "collision tags"; i.e.
+// for which you should always use random IDs with the binary_info,
+// giving you 4 + 8 + 32 = 44 bits to avoid collisions
+#define BINARY_INFO_MAKE_TAG(c1, c2) ((((uint)c2&0xffu)<<8u)|((uint)c1&0xffu))
+
+// Raspberry Pi defined. do not use
+#define BINARY_INFO_TAG_RASPBERRY_PI BINARY_INFO_MAKE_TAG('R','P')
+
+#define BINARY_INFO_ID_RP_PROGRAM_NAME 0x02031c86
+#define BINARY_INFO_ID_RP_PROGRAM_VERSION_STRING 0x11a9bc3a
+#define BINARY_INFO_ID_RP_PROGRAM_BUILD_DATE_STRING 0x9da22254
+#define BINARY_INFO_ID_RP_BINARY_END 0x68f465de
+#define BINARY_INFO_ID_RP_PROGRAM_URL 0x1856239a
+#define BINARY_INFO_ID_RP_PROGRAM_DESCRIPTION 0xb6a07c19
+#define BINARY_INFO_ID_RP_PROGRAM_FEATURE 0xa1f4b453
+#define BINARY_INFO_ID_RP_PROGRAM_BUILD_ATTRIBUTE 0x4275f0d3
+#define BINARY_INFO_ID_RP_SDK_VERSION 0x5360b3ab
+#define BINARY_INFO_ID_RP_PICO_BOARD 0xb63cffbb
+
+#if PICO_ON_DEVICE
+#define bi_ptr_of(x) x *
+#else
+#define bi_ptr_of(x) uint32_t
+#endif
+typedef struct __packed _binary_info_core {
+        uint16_t type;
+        uint16_t tag;
+} binary_info_core_t;
+
+typedef struct __packed _binary_info_raw_data {
+        struct _binary_info_core core;
+        uint8_t bytes[1];
+} binary_info_raw_data_t;
+
+typedef struct __packed _binary_info_sized_data {
+        struct _binary_info_core core;
+        uint32_t length;
+        uint8_t bytes[1];
+} binary_info_sized_data_t;
+
+typedef struct __packed _binary_info_list_zero_terminated {
+        struct _binary_info_core core;
+        bi_ptr_of(binary_info_t) list;
+} binary_info_list_zero_terminated_t;
+
+typedef struct __packed _binary_info_id_and_int {
+        struct _binary_info_core core;
+        uint32_t id;
+        int32_t value;
+} binary_info_id_and_int_t;
+
+typedef struct __packed _binary_info_id_and_string {
+        struct _binary_info_core core;
+        uint32_t id;
+        bi_ptr_of(const char) value;
+} binary_info_id_and_string_t;
+
+typedef struct __packed _binary_info_block_device {
+        struct _binary_info_core core;
+        bi_ptr_of(const char) name; // optional static name (independent of what is formatted)
+        uint32_t address;
+        uint32_t size;
+        bi_ptr_of(binary_info_t) extra; // additional info
+        uint16_t flags;
+} binary_info_block_device_t;
+
+#define BI_PINS_ENCODING_RANGE 1
+#define BI_PINS_ENCODING_MULTI 2
+
+typedef struct __packed _binary_info_pins_with_func {
+    struct _binary_info_core core;
+    // p4_5 : p3_5 : p2_5 : p1_5 : p0_5 : func_4 : 001_3 //individual pins p0,p1,p2,p3,p4 ... if fewer than 5 then duplicate p
+    //                    phi_5 : plo_5 : func_4 : 010_3 // pin range plo-phi inclusive
+    uint32_t pin_encoding;
+} binary_info_pins_with_func_t;
+
+typedef struct __packed _binary_info_pins_with_name {
+    struct _binary_info_core core;
+    uint32_t pin_mask;
+    bi_ptr_of(const char) label;
+} binary_info_pins_with_name_t;
+
+#define BI_NAMED_GROUP_SHOW_IF_EMPTY   0x0001  // default is to hide
+#define BI_NAMED_GROUP_SEPARATE_COMMAS 0x0002  // default is newlines
+#define BI_NAMED_GROUP_SORT_ALPHA      0x0004  // default is no sort
+#define BI_NAMED_GROUP_ADVANCED        0x0008  // if set, then only shown in say info -a
+
+typedef struct __packed _binary_info_named_group {
+    struct _binary_info_core core;
+    uint32_t parent_id;
+    uint16_t flags;
+    uint16_t group_tag;
+    uint32_t group_id;
+    bi_ptr_of(const char) label;
+} binary_info_named_group_t;
+
+enum {
+    BINARY_INFO_BLOCK_DEV_FLAG_READ =
+    1 << 0, // if not readable, then it is basically hidden, but tools may choose to avoid overwriting it
+    BINARY_INFO_BLOCK_DEV_FLAG_WRITE = 1 << 1,
+    BINARY_INFO_BLOCK_DEV_FLAG_REFORMAT = 1 << 2, // may be reformatted..
+
+    BINARY_INFO_BLOCK_DEV_FLAG_PT_UNKNOWN = 0 << 4, // unknown free to look
+    BINARY_INFO_BLOCK_DEV_FLAG_PT_MBR = 1 << 4, // expect MBR
+    BINARY_INFO_BLOCK_DEV_FLAG_PT_GPT = 2 << 4, // expect GPT
+    BINARY_INFO_BLOCK_DEV_FLAG_PT_NONE = 3 << 4, // no partition table
+};
+
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_bit_ops/CMakeLists.txt

@@ -0,0 +1,5 @@
+if (NOT TARGET pico_bit_ops_headers)
+    add_library(pico_bit_ops_headers INTERFACE)
+    target_include_directories(pico_bit_ops_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+    target_link_libraries(pico_bit_ops_headers INTERFACE pico_base_headers)
+endif()

src/common/pico_bit_ops/include/pico/bit_ops.h

@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_BIT_OPS_H
+#define _PICO_BIT_OPS_H
+
+#include "pico.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file bit_ops.h
+*  \defgroup pico_bit_ops pico_bit_ops
+*
+* Optimized bit manipulation functions.
+* Additionally provides  replacement implementations of the compiler built-ins __builtin_popcount, __builtin_clz
+* and __bulitin_ctz
+*/
+
+/*! \brief Reverse the bits in a 32 bit word
+ *  \ingroup pico_bit_ops
+ *
+ * \param bits 32 bit input
+ * \return the 32 input bits reversed
+ */
+uint32_t __rev(uint32_t bits);
+
+/*! \brief Reverse the bits in a 64 bit double word
+ *  \ingroup pico_bit_ops
+ *
+ * \param bits 64 bit input
+ * \return the 64 input bits reversed
+ */
+uint64_t __revll(uint64_t bits);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/common/pico_divider/CMakeLists.txt

@@ -0,0 +1,5 @@
+if (NOT TARGET pico_divider_headers)
+    add_library(pico_divider_headers INTERFACE)
+    target_include_directories(pico_divider_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+    target_link_libraries(pico_divider_headers INTERFACE pico_base_headers)
+endif()

src/common/pico_divider/include/pico/divider.h

@@ -0,0 +1,322 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_DIVIDER_H_
+#define _PICO_DIVIDER_H_
+
+#include "pico.h"
+#include "hardware/divider.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * \defgroup pico_divider pico_divider
+ * Optimized 32 and 64 bit division functions accelerated by the RP2040 hardware divider.
+ * Additionally provides integration with the C `/` and `%` operators
+ */
+
+/** \file pico/divider.h
+* \brief High level APIs including combined quotient and remainder functions for 32 and 64 bit accelerated by the hardware divider
+* \ingroup pico_divider
+*
+* These functions all call __aeabi_idiv0 or __aebi_ldiv0 on division by zero
+* passing the largest applicably signed value
+*
+* Functions with unsafe in their name do not save/restore divider state, so are unsafe to call from interrupts. Unsafe functions are slightly faster.
+*/
+
+/**
+ * \brief Integer divide of two signed 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient
+ */
+int32_t div_s32s32(int32_t a, int32_t b);
+
+/**
+ * \brief Integer divide of two signed 32-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ */
+static inline int32_t divmod_s32s32_rem(int32_t a, int32_t b, int32_t *rem) {
+    divmod_result_t r = hw_divider_divmod_s32(a, b);
+    *rem = to_remainder_s32(r);
+    return to_quotient_s32(r);
+}
+
+/**
+ * \brief Integer divide of two signed 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in low word/r0, remainder in high word/r1
+ */
+divmod_result_t divmod_s32s32(int32_t a, int32_t b);
+
+/**
+ * \brief Integer divide of two unsigned 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return Quotient
+ */
+uint32_t div_u32u32(uint32_t a, uint32_t b);
+
+/**
+ * \brief Integer divide of two unsigned 32-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ */
+static inline uint32_t divmod_u32u32_rem(uint32_t a, uint32_t b, uint32_t *rem) {
+    divmod_result_t r = hw_divider_divmod_u32(a, b);
+    *rem = to_remainder_u32(r);
+    return to_quotient_u32(r);
+}
+
+/**
+ * \brief Integer divide of two unsigned 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in low word/r0, remainder in high word/r1
+ */
+divmod_result_t divmod_u32u32(uint32_t a, uint32_t b);
+
+/**
+ * \brief Integer divide of two signed 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return Quotient
+ */
+int64_t div_s64s64(int64_t a, int64_t b);
+
+/**
+ * \brief Integer divide of two signed 64-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ */
+int64_t divmod_s64s64_rem(int64_t a, int64_t b, int64_t *rem);
+
+/**
+ * \brief Integer divide of two signed 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in result (r0,r1), remainder in regs (r2, r3)
+ */
+int64_t divmod_s64s64(int64_t a, int64_t b);
+
+/**
+ * \brief Integer divide of two unsigned 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return Quotient
+ */
+uint64_t div_u64u64(uint64_t a, uint64_t b);
+
+/**
+ * \brief Integer divide of two unsigned 64-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ */
+uint64_t divmod_u64u64_rem(uint64_t a, uint64_t b, uint64_t *rem);
+
+
+/**
+ * \brief Integer divide of two signed 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in result (r0,r1), remainder in regs (r2, r3)
+ */
+uint64_t divmod_u64u64(uint64_t a, uint64_t b);
+
+// -----------------------------------------------------------------------
+// these "unsafe" functions are slightly faster, but do not save the divider state,
+// so are not generally safe to be called from interrupts
+// -----------------------------------------------------------------------
+
+/**
+ * \brief Unsafe integer divide of two signed 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient
+ *
+ * Do not use in interrupts
+ */
+int32_t div_s32s32_unsafe(int32_t a, int32_t b);
+
+/**
+ * \brief Unsafe integer divide of two signed 32-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ *
+ * Do not use in interrupts
+ */
+int32_t divmod_s32s32_rem_unsafe(int32_t a, int32_t b, int32_t *rem);
+
+/**
+ * \brief Unsafe integer divide of two unsigned 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in low word/r0, remainder in high word/r1
+ *
+ * Do not use in interrupts
+ */
+int64_t divmod_s32s32_unsafe(int32_t a, int32_t b);
+
+/**
+ * \brief Unsafe integer divide of two unsigned 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return Quotient
+ *
+ * Do not use in interrupts
+ */
+uint32_t div_u32u32_unsafe(uint32_t a, uint32_t b);
+
+/**
+ * \brief Unsafe integer divide of two unsigned 32-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ *
+ * Do not use in interrupts
+ */
+uint32_t divmod_u32u32_rem_unsafe(uint32_t a, uint32_t b, uint32_t *rem);
+
+/**
+ * \brief Unsafe integer divide of two unsigned 32-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in low word/r0, remainder in high word/r1
+ *
+ * Do not use in interrupts
+ */
+uint64_t divmod_u32u32_unsafe(uint32_t a, uint32_t b);
+
+/**
+ * \brief Unsafe integer divide of two signed 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return Quotient
+ *
+ * Do not use in interrupts
+ */
+int64_t div_s64s64_unsafe(int64_t a, int64_t b);
+
+/**
+ * \brief Unsafe integer divide of two signed 64-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ *
+ * Do not use in interrupts
+ */
+int64_t divmod_s64s64_rem_unsafe(int64_t a, int64_t b, int64_t *rem);
+
+/**
+ * \brief Unsafe integer divide of two signed 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in result (r0,r1), remainder in regs (r2, r3)
+ *
+ * Do not use in interrupts
+ */
+int64_t divmod_s64s64_unsafe(int64_t a, int64_t b);
+
+/**
+ * \brief Unsafe integer divide of two unsigned 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return Quotient
+ *
+ * Do not use in interrupts
+ */
+uint64_t div_u64u64_unsafe(uint64_t a, uint64_t b);
+
+/**
+ * \brief Unsafe integer divide of two unsigned 64-bit values, with remainder
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \param [out] rem The remainder of dividend/divisor
+ * \return Quotient result of dividend/divisor
+ *
+ * Do not use in interrupts
+ */
+uint64_t divmod_u64u64_rem_unsafe(uint64_t a, uint64_t b, uint64_t *rem);
+
+/**
+ * \brief Unsafe integer divide of two signed 64-bit values
+ * \ingroup pico_divider
+ *
+ * \param a Dividend
+ * \param b Divisor
+ * \return quotient in result (r0,r1), remainder in regs (r2, r3)
+ *
+ * Do not use in interrupts
+ */
+uint64_t divmod_u64u64_unsafe(uint64_t a, uint64_t b);
+
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_stdlib/CMakeLists.txt

@@ -0,0 +1,11 @@
+if (NOT TARGET pico_stdlib_headers)
+    add_library(pico_stdlib_headers INTERFACE)
+    target_include_directories(pico_stdlib_headers INTERFACE include)
+    target_link_libraries(pico_stdlib_headers INTERFACE
+            hardware_gpio
+            hardware_uart
+            hardware_divider
+            pico_time
+            pico_util
+    )
+endif()

src/common/pico_stdlib/include/pico/stdlib.h

@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_STDLIB_H
+#define _PICO_STDLIB_H
+
+#include "pico.h"
+#include "pico/stdio.h"
+#include "pico/time.h"
+#include "hardware/gpio.h"
+#include "hardware/uart.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file stdlib.h
+ *  \defgroup pico_stdlib pico_stdlib
+ *
+ * Aggregation of a core subset of Pico SDK libraries used by most executables along with some additional
+ * utility methods. Including pico_stdlib gives you everything you need to get a basic program running
+ * which prints to stdout or flashes a LED
+ *
+ * This library aggregates:
+ *   - @ref hardware_uart
+ *   - @ref hardware_gpio
+ *   - @ref pico_binary_info
+ *   - @ref pico_runtime
+ *   - @ref pico_platform
+ *   - @ref pico_printf
+ *   - @ref pico_stdio
+ *   - @ref pico_standard_link
+ *   - @ref pico_util
+ *
+ * There are some basic default values used by these functions that will default to
+ * usable values, however, they can be customised in a board definition header via
+ * config.h or similar
+ */
+
+// Note PICO_STDIO_UART, PICO_STDIO_USB, PICO_STDIO_SEMIHOSTING are set by the
+// respective INTERFACE libraries, so these defines are set if the library
+// is included for the target executable
+
+#if PICO_STDIO_UART
+#include "pico/stdio_uart.h"
+#endif
+
+#if PICO_STDIO_USB
+#include "pico/stdio_usb.h"
+#endif
+
+#if PICO_STDIO_SEMIHOSTING
+#include "pico/stdio_semihosting.h"
+#endif
+
+/*! \brief Set up the default UART and assign it to the default GPIO's
+ *  \ingroup pico_stdlib
+ *
+ * By default this will use UART 0, with TX to pin GPIO 0,
+ * RX to pin GPIO 1, and the baudrate to 115200
+ *
+ * Calling this method also initializes stdin/stdout over UART if the
+ * @ref pico_stdio_uart library is linked.
+ *
+ * Defaults can be changed using configuration defines,
+ *  PICO_DEFAULT_UART_INSTANCE,
+ *  PICO_DEFAULT_UART_BAUD_RATE
+ *  PICO_DEFAULT_UART_TX_PIN
+ *  PICO_DEFAULT_UART_RX_PIN
+ */
+void setup_default_uart();
+
+/*! \brief Initialise the system clock to 48MHz
+ *  \ingroup pico_stdlib
+ *
+ *  Set the system clock to 48MHz, and set the peripheral clock to match.
+ */
+void set_sys_clock_48mhz();
+
+/*! \brief Initialise the system clock
+ *  \ingroup pico_stdlib
+ *
+ * \param vco_freq The voltage controller oscillator frequency to be used by the SYS PLL
+ * \param post_div1 The first post divider for the SYS PLL
+ * \param post_div2 The second post divider for the SYS PLL.
+ *
+ * See the PLL documentation in the datasheet for details of driving the PLLs.
+ */
+void set_sys_clock_pll(uint32_t vco_freq, uint post_div1, uint post_div2);
+
+/*! \brief Check if a given system clock frequency is valid/attainable
+ *  \ingroup pico_stdlib
+ *
+ * \param freq_khz Requested frequency
+ * \param vco_freq_out On success, the voltage controller oscillator frequeucny to be used by the SYS PLL
+ * \param post_div1_out On success, The first post divider for the SYS PLL
+ * \param post_div2_out On success, The second post divider for the SYS PLL.
+ * @return true if the frequency is possible and the output parameters have been written.
+ */
+bool check_sys_clock_khz(uint32_t freq_khz, uint *vco_freq_out, uint *post_div1_out, uint *post_div2_out);
+
+/*! \brief Attempt to set a system clock frequency in khz
+ *  \ingroup pico_stdlib
+ *
+ * Note that not all clock frequencies are possible; it is preferred that you
+ * use src/rp2_common/hardware_clocks/scripts/vcocalc.py to calculate the parameters
+ * for use with set_sys_clock_pll
+ *
+ * \param freq_khz Requested frequency
+ * \param required if true then this function will assert if the frequency is not attainable.
+ * \return true if the clock was configured
+ */
+static inline bool set_sys_clock_khz(uint32_t freq_khz, bool required) {
+    uint vco, postdiv1, postdiv2;
+    if (check_sys_clock_khz(freq_khz, &vco, &postdiv1, &postdiv2)) {
+        set_sys_clock_pll(vco, postdiv1, postdiv2);
+        return true;
+    } else if (required) {
+        panic("System clock of %u kHz cannot be exactly achieved", freq_khz);
+    }
+    return false;
+}
+
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_sync/CMakeLists.txt

@@ -0,0 +1,44 @@
+if (NOT TARGET pico_sync_headers)
+    add_library(pico_sync_headers INTERFACE)
+    target_include_directories(pico_sync_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+    target_link_libraries(pico_sync_headers INTERFACE hardware_sync pico_time)
+endif()
+
+if (NOT TARGET pico_sync_core)
+    add_library(pico_sync_core INTERFACE)
+    target_sources(pico_sync_core INTERFACE
+            ${CMAKE_CURRENT_LIST_DIR}/lock_core.c
+    )
+    target_link_libraries(pico_sync_core INTERFACE pico_sync_headers)
+endif()
+
+if (NOT TARGET pico_sync_sem)
+    add_library(pico_sync_sem INTERFACE)
+    target_sources(pico_sync_sem INTERFACE
+        ${CMAKE_CURRENT_LIST_DIR}/sem.c
+    )
+    target_link_libraries(pico_sync_sem INTERFACE pico_sync_core pico_time)
+endif()
+
+if (NOT TARGET pico_sync_mutex)
+    add_library(pico_sync_mutex INTERFACE)
+    target_sources(pico_sync_mutex INTERFACE
+            ${CMAKE_CURRENT_LIST_DIR}/mutex.c
+            )
+    target_link_libraries(pico_sync_mutex INTERFACE pico_sync_core pico_time)
+endif()
+
+if (NOT TARGET pico_sync_critical_section)
+    add_library(pico_sync_critical_section INTERFACE)
+    target_sources(pico_sync_critical_section INTERFACE
+            ${CMAKE_CURRENT_LIST_DIR}/critical_section.c
+            )
+    target_link_libraries(pico_sync_critical_section INTERFACE pico_sync_core pico_time)
+endif()
+
+if (NOT TARGET pico_sync)
+    add_library(pico_sync INTERFACE)
+    target_link_libraries(pico_sync INTERFACE pico_sync_sem pico_sync_mutex pico_sync_critical_section pico_sync_core)
+endif()
+
+

src/common/pico_sync/critical_section.c

@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/critical_section.h"
+
+#if !PICO_NO_HARDWARE
+static_assert(sizeof(critical_section_t) == 8, "");
+#endif
+
+void critical_section_init(critical_section_t *critsec) {
+    critical_section_init_with_lock_num(critsec, spin_lock_claim_unused(true));
+}
+
+void critical_section_init_with_lock_num(critical_section_t *critsec, uint lock_num) {
+    lock_init(&critsec->core, lock_num);
+    __mem_fence_release();
+}
+
+void critical_section_deinit(critical_section_t *critsec) {
+    spin_lock_unclaim(spin_lock_get_num(critsec->core.spin_lock));
+}

src/common/pico_sync/include/pico/critical_section.h

@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PLATFORM_CRITICAL_SECTION_H
+#define _PLATFORM_CRITICAL_SECTION_H
+
+#include "pico/lock_core.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file critical_section.h
+ *  \defgroup critical_section critical_section
+ *  \ingroup pico_sync
+ *  \brief Critical Section API for short-lived mutual exclusion safe for IRQ and multi-core
+ *
+ *  A critical section is non-reentrant, and provides mutual exclusion using a spin-lock to prevent access
+ *  from the other core, and from (higher priority) interrupts on the same core. It does the former
+ *  using a spin lock and the latter by disabling interrupts on the calling core.
+ *
+ *  Because interrupts are disabled by this function, uses of the critical_section should be as short as possible.
+ */
+
+typedef struct __packed_aligned critical_section {
+    lock_core_t core;
+    uint32_t save;
+} critical_section_t;
+
+/*! \brief  Initialise a critical_section structure allowing the system to assign a spin lock number
+ *  \ingroup critical_section
+ *
+ * The critical section is initialized ready for use, and will use a (possibly shared) spin lock
+ * number assigned by the system. Note that in general it is unlikely that you would be nesting
+ * critical sections, however if you do so you *must* use \ref critical_section_init_with_lock_num
+ * to ensure that the spin lock's used are different.
+ *
+ * \param critsec Pointer to critical_section structure
+ */
+void critical_section_init(critical_section_t *critsec);
+
+/*! \brief  Initialise a critical_section structure assigning a specific spin lock number
+ *  \ingroup critical_section
+ * \param critsec Pointer to critical_section structure
+ * \param lock_num the specific spin lock number to use
+ */
+void critical_section_init_with_lock_num(critical_section_t *critsec, uint lock_num);
+
+/*! \brief  Enter a critical_section
+ *  \ingroup critical_section
+ *
+ * If the spin lock associated with this critical section is in use, then this
+ * method will block until it is released.
+ *
+ * \param critsec Pointer to critical_section structure
+ */
+static inline void critical_section_enter_blocking(critical_section_t *critsec) {
+    critsec->save = spin_lock_blocking(critsec->core.spin_lock);
+}
+
+/*! \brief  Release a critical_section
+ *  \ingroup critical_section
+ *
+ * \param critsec Pointer to critical_section structure
+ */
+static inline void critical_section_exit(critical_section_t *critsec) {
+    spin_unlock(critsec->core.spin_lock, critsec->save);
+}
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_sync/include/pico/lock_core.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_LOCK_CORE_H
+#define _PICO_LOCK_CORE_H
+
+#include "pico.h"
+#include "hardware/sync.h"
+
+/** \file lock_core.h
+ *  \ingroup pico_sync
+ *
+ * Base implementation for locking primitives protected by a spin lock
+ */
+typedef struct lock_core {
+    // spin lock protecting this lock's state
+    spin_lock_t *spin_lock;
+
+    // note any lock members in containing structures need not be volatile;
+    // they are protected by memory/compiler barriers when gaining and release spin locks
+} lock_core_t;
+
+void lock_init(lock_core_t *core, uint lock_num);
+
+#endif

src/common/pico_sync/include/pico/mutex.h

@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PLATFORM_MUTEX_H
+#define _PLATFORM_MUTEX_H
+
+#include "pico/lock_core.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file mutex.h
+ *  \defgroup mutex mutex
+ *  \ingroup pico_sync
+ * \brief Mutex API for non IRQ mutual exclusion between cores
+ *
+ * Mutexes are application level locks usually used protecting data structures that might be used by
+ * multiple cores. Unlike critical sections, the mutex protected code is not necessarily
+ * required/expected to complete quickly, as no other sytemwide locks are held on account of a locked mutex.
+ *
+ * Because they are not re-entrant on the same core, blocking on a mutex should never be done in an IRQ
+ * handler. It is valid to call \ref mutex_try_enter from within an IRQ handler, if the operation
+ * that would be conducted under lock can be skipped if the mutex is locked (at least by the same core).
+ *
+ * See \ref critical_section.h for protecting access between multiple cores AND IRQ handlers
+ */
+
+typedef struct __packed_aligned mutex {
+    lock_core_t core;
+    bool owned;
+    int8_t owner;
+} mutex_t;
+
+/*! \brief  Initialise a mutex structure
+ *  \ingroup mutex
+ *
+ * \param mtx Pointer to mutex structure
+ */
+void mutex_init(mutex_t *mtx);
+
+/*! \brief  Take ownership of a mutex
+ *  \ingroup mutex
+ *
+ * This function will block until the calling core can claim ownership of the mutex.
+ * On return the caller core owns the mutex
+ *
+ * \param mtx Pointer to mutex structure
+ */
+void mutex_enter_blocking(mutex_t *mtx);
+
+/*! \brief Check to see if a mutex is available
+ *  \ingroup mutex
+ *
+ * Will return true if the mutex is unowned, false otherwise
+ *
+ * \param mtx Pointer to mutex structure
+ * \param owner_out If mutex is owned, and this pointer is non-zero, it will be filled in with the core number of the current owner of the mutex
+ */
+bool mutex_try_enter(mutex_t *mtx, uint32_t *owner_out);
+
+/*! \brief Wait for mutex with timeout
+ *  \ingroup mutex
+ *
+ * Wait for up to the specific time to take ownership of the mutex. If the calling
+ * core can take ownership of the mutex before the timeout expires, then true will be returned
+ * and the calling core will own the mutex, otherwise false will be returned and the calling
+ * core will *NOT* own the mutex.
+ *
+ * \param mtx Pointer to mutex structure
+ * \param timeout_ms The timeout in milliseconds.
+ * \return true if mutex now owned, false if timeout occurred before mutex became available
+ */
+bool mutex_enter_timeout_ms(mutex_t *mtx, uint32_t timeout_ms);
+
+/*! \brief Wait for mutex until a specific time
+ *  \ingroup mutex
+ *
+ * Wait until the specific time to take ownership of the mutex. If the calling
+ * core can take ownership of the mutex before the timeout expires, then true will be returned
+ * and the calling core will own the mutex, otherwise false will be returned and the calling
+ * core will *NOT* own the mutex.
+ *
+ * \param mtx Pointer to mutex structure
+ * \param until The time after which to return if the core cannot take owner ship of the mutex
+ * \return true if mutex now owned, false if timeout occurred before mutex became available
+ */
+bool mutex_enter_block_until(mutex_t *mtx, absolute_time_t until);
+
+/*! \brief  Release ownership of a mutex
+ *  \ingroup mutex
+ *
+ * \param mtx Pointer to mutex structure
+ */
+void mutex_exit(mutex_t *mtx);
+
+/*! \brief Test for mutex initialised state
+ *  \ingroup mutex
+ *
+ * \param mtx Pointer to mutex structure
+ * \return true if the mutex is initialised, false otherwise
+ */
+static inline bool mutex_is_initialzed(mutex_t *mtx) {
+    return mtx->core.spin_lock != 0;
+}
+
+/*! \brief Helper macro for static definition of mutexes
+ *  \ingroup mutex
+ *
+ * A mutex defined as follows:
+ *
+ * ```c
+ * auto_init_mutex(my_mutex);
+ * ```
+ *
+ * Is equivalent to doing
+ *
+ * ```c
+ * static mutex_t my_mutex;
+ *
+ * void my_init_function() {
+ *    mutex_init(&my_mutex);
+ * }
+ * ```
+ *
+ * But the initialization of the mutex is performed automatically during runtime initialization
+ */
+#define auto_init_mutex(name) static __attribute__((section(".mutex_array"))) mutex_t name
+
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_sync/include/pico/sem.h

@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PLATFORM_SEM_H
+#define _PLATFORM_SEM_H
+
+#include "pico/lock_core.h"
+
+/** \file sem.h
+ *  \defgroup sem sem
+ *  \ingroup pico_sync
+ *  \brief Semaphore API for restricting access to a resource
+ *
+ * A semaphore holds a number of available permits. `sem_acquire` methods will acquire a permit if available
+ * (reducing the available count by 1) or block if the number of available permits is 0.
+ * \ref sem_release() increases the number of available permits by one potentially unblocking a `sem_acquire` method.
+ *
+ * Note that \ref sem_release() may be called an arbitrary number of times, however the number of available
+ * permits is capped to the max_permit value specified during semaphore initialization.
+ *
+ * Although these semaphore related functions can be used from IRQ handlers, it is obviously preferable to only
+ * release semaphores from within an IRQ handler (i.e. avoid blocking)
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+typedef struct __packed_aligned semaphore {
+    struct lock_core core;
+    int16_t permits;
+    int16_t max_permits;
+} semaphore_t;
+
+
+/*! \brief  Initialise a semaphore structure
+ *  \ingroup sem
+ *
+ * \param sem Pointer to semaphore structure
+ * \param initial_permits How many permits are initially acquired
+ * \param max_permits  Total number of permits allowed for this semaphore
+ */
+void sem_init(semaphore_t *sem, int16_t initial_permits, int16_t max_permits);
+
+/*! \brief  Return number of available permits on the semaphore
+ *  \ingroup sem
+ *
+ * \param sem Pointer to semaphore structure
+ * \return The number of permits available on the semaphore.
+ */
+int sem_available(semaphore_t *sem);
+
+/*! \brief  Release a permit on a semaphore
+ *  \ingroup sem
+ *
+ * Increases the number of permits by one (unless the number of permits is already at the maximum).
+ * A blocked `sem_acquire` will be released if the number of permits is increased.
+ *
+ * \param sem Pointer to semaphore structure
+ * \return true if the number of permits available was increased.
+ */
+bool sem_release(semaphore_t *sem);
+
+/*! \brief  Reset semaphore to a specific number of available permits
+ *  \ingroup sem
+ *
+ * Reset value should be from 0 to the max_permits specified in the init function
+ *
+ * \param sem Pointer to semaphore structure
+ * \param permits the new number of available permits
+ */
+void sem_reset(semaphore_t *sem, int16_t permits);
+
+/*! \brief  Acquire a permit from the semaphore
+ *  \ingroup sem
+ *
+ * This function will block and wait if no permits are available.
+ *
+ * \param sem Pointer to semaphore structure
+ */
+void sem_acquire_blocking(semaphore_t *sem);
+
+/*! \brief  Acquire a permit from a semaphore, with timeout
+ *  \ingroup sem
+ *
+ * This function will block and wait if no permits are available, until the
+ * defined timeout has been reached. If the timeout is reached the function will
+ * return false, otherwise it will return true.
+ *
+ * \param sem Pointer to semaphore structure
+ * \param timeout_ms Time to wait to acquire the semaphore, in ms.
+ * \return false if timeout reached, true if permit was acquired.
+ */
+bool sem_acquire_timeout_ms(semaphore_t *sem, uint32_t timeout_ms);
+
+#ifdef __cplusplus
+}
+#endif
+#endif

src/common/pico_sync/include/pico/sync.h

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_SYNC_H
+#define _PICO_SYNC_H
+
+/** \file pico/sync.h
+ *  \defgroup pico_sync pico_sync
+ * Synchronization primitives and mutual exclusion
+ */
+
+#include "pico/sem.h"
+#include "pico/mutex.h"
+#include "pico/critical_section.h"
+
+#endif

src/common/pico_sync/lock_core.c

@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/lock_core.h"
+
+void lock_init(lock_core_t *core, uint lock_num) {
+    assert(lock_num >= 0 && lock_num < NUM_SPIN_LOCKS);
+    core->spin_lock = spin_lock_instance(lock_num);
+}
+

src/common/pico_sync/mutex.c

@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/mutex.h"
+#include "pico/time.h"
+
+#if !PICO_NO_HARDWARE
+static_assert(sizeof(mutex_t) == 8, "");
+#endif
+
+void mutex_init(mutex_t *mtx) {
+    lock_init(&mtx->core, next_striped_spin_lock_num());
+    __mem_fence_release();
+}
+
+void __time_critical_func(mutex_enter_blocking)(mutex_t *mtx) {
+    assert(mtx->core.spin_lock);
+    bool block = true;
+    do {
+        uint32_t save = spin_lock_blocking(mtx->core.spin_lock);
+        if (!mtx->owned) {
+            mtx->owned = true;
+            mtx->owner = get_core_num();
+            block = false;
+        }
+        spin_unlock(mtx->core.spin_lock, save);
+        if (block) {
+            __wfe();
+        }
+    } while (block);
+}
+
+bool __time_critical_func(mutex_try_enter)(mutex_t *mtx, uint32_t *owner_out) {
+    bool entered;
+    uint32_t save = spin_lock_blocking(mtx->core.spin_lock);
+    if (!mtx->owned) {
+        mtx->owned = true;
+        mtx->owner = get_core_num();
+        entered = true;
+    } else {
+        if (owner_out) *owner_out = mtx->owner;
+        entered = false;
+    }
+    spin_unlock(mtx->core.spin_lock, save);
+    return entered;
+}
+
+bool __time_critical_func(mutex_enter_timeout_ms)(mutex_t *mtx, uint32_t timeout_ms) {
+    return mutex_enter_block_until(mtx, make_timeout_time_ms(timeout_ms));
+}
+
+bool __time_critical_func(mutex_enter_block_until)(mutex_t *mtx, absolute_time_t until) {
+    assert(mtx->core.spin_lock);
+    bool block = true;
+    do {
+        uint32_t save = spin_lock_blocking(mtx->core.spin_lock);
+        if (!mtx->owned) {
+            mtx->owned = true;
+            mtx->owner = get_core_num();
+            block = false;
+        }
+        spin_unlock(mtx->core.spin_lock, save);
+        if (block) {
+            if (best_effort_wfe_or_timeout(until)) {
+                return false;
+            }
+        }
+    } while (block);
+    return true;
+}
+
+void __time_critical_func(mutex_exit)(mutex_t *mtx) {
+    uint32_t save = spin_lock_blocking(mtx->core.spin_lock);
+    assert(mtx->owned);
+    mtx->owned = 0;
+#ifndef NDEBUG
+    mtx->owner = -1;
+#endif
+    __sev();
+    spin_unlock(mtx->core.spin_lock, save);
+}

src/common/pico_sync/sem.c

@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/sem.h"
+#include "pico/time.h"
+
+void sem_init(semaphore_t *sem, int16_t initial_permits, int16_t max_permits) {
+    lock_init(&sem->core, next_striped_spin_lock_num());
+    sem->permits = initial_permits;
+    sem->max_permits = max_permits;
+    __mem_fence_release();
+}
+
+int __time_critical_func(sem_available)(semaphore_t *sem) {
+    return *(volatile typeof(sem->permits) *) &sem->permits;
+}
+
+void __time_critical_func(sem_acquire_blocking)(semaphore_t *sem) {
+    bool block = true;
+    do {
+        uint32_t save = spin_lock_blocking(sem->core.spin_lock);
+        if (sem->permits > 0) {
+            sem->permits--;
+            __sev();
+            block = false;
+        }
+        spin_unlock(sem->core.spin_lock, save);
+        if (block) {
+            __wfe();
+        }
+    } while (block);
+}
+
+bool __time_critical_func(sem_acquire_timeout_ms)(semaphore_t *sem, uint32_t timeout_ms) {
+    bool block = true;
+    absolute_time_t target = nil_time;
+    do {
+        uint32_t save = spin_lock_blocking(sem->core.spin_lock);
+        if (sem->permits > 0) {
+            sem->permits--;
+            __sev();
+            block = false;
+        }
+        spin_unlock(sem->core.spin_lock, save);
+        if (block) {
+            if (is_nil_time(target)) {
+                target = make_timeout_time_ms(timeout_ms);
+            }
+            if (best_effort_wfe_or_timeout(target)) {
+                return false;
+            }
+        }
+    } while (block);
+    return true;
+}
+
+// todo this should really have a blocking variant for when permits are maxed out
+bool __time_critical_func(sem_release)(semaphore_t *sem) {
+    bool rc;
+    uint32_t save = spin_lock_blocking(sem->core.spin_lock);
+    int32_t count = sem->permits;
+    if (count < sem->max_permits) {
+        sem->permits = count + 1;
+        __sev();
+        rc = true;
+    } else {
+        rc = false;
+    }
+    spin_unlock(sem->core.spin_lock, save);
+    return rc;
+}
+
+void __time_critical_func(sem_reset)(semaphore_t *sem, int16_t permits) {
+    assert(permits >= 0 && permits <= sem->max_permits);
+    uint32_t save = spin_lock_blocking(sem->core.spin_lock);
+    if (permits > sem->permits) __sev();
+    sem->permits = permits;
+    spin_unlock(sem->core.spin_lock, save);
+}

src/common/pico_time/CMakeLists.txt

@@ -0,0 +1,16 @@
+if (NOT TARGET pico_time_headers)
+    add_library(pico_time_headers INTERFACE)
+
+    target_include_directories(pico_time_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+
+    target_link_libraries(pico_time_headers INTERFACE hardware_timer)
+endif()
+
+if (NOT TARGET pico_time)
+    add_library(pico_time INTERFACE)
+
+    target_sources(pico_time INTERFACE
+            ${CMAKE_CURRENT_LIST_DIR}/time.c
+            ${CMAKE_CURRENT_LIST_DIR}/timeout_helper.c)
+    target_link_libraries(pico_time INTERFACE pico_time_headers pico_sync pico_util)
+endif()

src/common/pico_time/include/pico/time.h

@@ -0,0 +1,692 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_TIME_H
+#define _PICO_TIME_H
+
+#include "pico.h"
+#include "hardware/timer.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file time.h
+ *  \defgroup pico_time pico_time
+ *
+ * API for accurate timestamps, sleeping, and time based callbacks
+ *
+ * \note The functions defined here provide a much more powerful and user friendly wrapping around the
+ * low level hardware timer functionality. For these functions (and any other Pico SDK functionality
+ * e.g. timeouts, that relies on them) to work correctly, the hardware timer should not be modified. i.e. it is expected
+ * to be monotonically increasing once per microsecond. Fortunately there is no need to modify the hardware
+ * timer as any functionality you can think of that isn't already covered here can easily be modelled
+ * by adding or subtracting a constant value from the unmodified hardware timer.
+ *
+ * \sa \ref hardware_timer
+ */
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_TIME, Enable/disable assertions in the time module, type=bool, default=0, group=pico_time
+#ifndef PARAM_ASSERTIONS_ENABLED_TIME
+#define PARAM_ASSERTIONS_ENABLED_TIME 0
+#endif
+
+// PICO_CONFIG: PICO_TIME_SLEEP_OVERHEAD_ADJUST_US, How many microseconds to wake up early (and then busy_wait) to account for timer overhead when sleeping in low power mode, type=int, default=6, group=pico_time
+#ifndef PICO_TIME_SLEEP_OVERHEAD_ADJUST_US
+#define PICO_TIME_SLEEP_OVERHEAD_ADJUST_US 6
+#endif
+/*!
+ * \defgroup timestamp timestamp
+ *  \ingroup pico_time
+ * \brief Timestamp functions relating to points in time (including the current time)
+ *
+ * These are functions for dealing with timestamps (i.e. instants in time) represented by the type absolute_time_t. This opaque
+ * type is provided to help prevent accidental mixing of timestamps and relative time values.
+ */
+
+/*! \brief Return a representation of the current time.
+ * \ingroup timestamp
+ *
+ * Returns an opaque high fidelity representation of the current time sampled during the call.
+ *
+ * \return the absolute time (now) of the hardware timer
+ *
+ * \sa absolute_time_t
+ * \sa sleep_until()
+ * \sa time_us_64()
+ */
+static inline absolute_time_t get_absolute_time() {
+    absolute_time_t t;
+    update_us_since_boot(&t, time_us_64());
+    return t;
+}
+
+static inline uint32_t us_to_ms(uint64_t us) {
+    if (us >> 32u) {
+        return (uint32_t)(us / 1000u);
+    } else {
+        return ((uint32_t)us) / 1000u;
+    }
+}
+
+/*! fn to_ms_since_boot
+ * \ingroup timestamp
+ * \brief Convert a timestamp into a number of milliseconds since boot.
+ * \param t an absolute_time_t value to convert
+ * \return the number of microseconds since boot represented by t
+ * \sa to_us_since_boot
+ */
+static inline uint32_t to_ms_since_boot(absolute_time_t t) {
+    uint64_t us = to_us_since_boot(t);
+    return us_to_ms(us);
+}
+
+/*! \brief Return a timestamp value obtained by adding a number of microseconds to another timestamp
+ * \ingroup timestamp
+ *
+ * \param t the base timestamp
+ * \param us the number of microseconds to add
+ * \return the timestamp representing the resulting time
+ */
+static inline absolute_time_t delayed_by_us(const absolute_time_t t, uint64_t us) {
+    absolute_time_t t2;
+    uint64_t base = to_us_since_boot(t);
+    uint64_t delayed = base + us;
+    if (delayed < base) {
+        delayed = (uint64_t)-1;
+    }
+    update_us_since_boot(&t2, delayed);
+    return t2;
+}
+
+/*! \brief Return a timestamp value obtained by adding a number of milliseconds to another timestamp
+ * \ingroup timestamp
+ *
+ * \param t the base timestamp
+ * \param ms the number of milliseconds to add
+ * \return the timestamp representing the resulting time
+ */
+static inline absolute_time_t delayed_by_ms(const absolute_time_t t, uint32_t ms) {
+    absolute_time_t t2;
+    uint64_t base = to_us_since_boot(t);
+    uint64_t delayed = base + ms * 1000ull;
+    if (delayed < base) {
+        delayed = (uint64_t)-1;
+    }
+    update_us_since_boot(&t2, delayed);
+    return t2;
+}
+
+/*! \brief Convenience method to get the timestamp a number of microseconds from the current time
+ * \ingroup timestamp
+ *
+ * \param us the number of microseconds to add to the current timestamp
+ * \return the future timestamp
+ */
+static inline absolute_time_t make_timeout_time_us(uint64_t us) {
+    return delayed_by_us(get_absolute_time(), us);
+}
+
+/*! \brief Convenience method to get the timestamp a number of milliseconds from the current time
+ * \ingroup timestamp
+ *
+ * \param ms the number of milliseconds to add to the current timestamp
+ * \return the future timestamp
+ */
+static inline absolute_time_t make_timeout_time_ms(uint32_t ms) {
+    return delayed_by_ms(get_absolute_time(), ms);
+}
+
+/*! \brief Return the difference in microseconds between two timestamps
+ * \ingroup timestamp
+ *
+ * \param from the first timestamp
+ * \param to the second timestamp
+ * \return the number of microseconds between the two timestamps (positive if `to` is after `from`)
+ */
+static inline int64_t absolute_time_diff_us(absolute_time_t from, absolute_time_t to) {
+    return to_us_since_boot(to) - to_us_since_boot(from);
+}
+
+/*! \brief The timestamp representing the end of time; no timestamp is after this
+ * \ingroup timestamp
+ */
+extern const absolute_time_t at_the_end_of_time;
+
+/*! \brief The timestamp representing a null timestamp
+ * \ingroup timestamp
+ */
+extern const absolute_time_t nil_time;
+
+/*! \brief Determine if the given timestamp is nil
+ * \ingroup timestamp
+ *  \param t the timestamp
+ *  \return true if the timestamp is nil
+ *  \sa nil_time()
+ */
+static inline bool is_nil_time(absolute_time_t t) {
+    return !to_us_since_boot(t);
+}
+
+/*!
+ * \defgroup sleep sleep
+ * \ingroup pico_time
+ * \brief Sleep functions for delaying execution in a lower power state.
+ *
+ * These functions allow the calling core to sleep. This is a lower powered sleep; waking and re-checking time on every processor
+ * event (WFE)
+ *
+ * \note  These functions should not be called from an IRQ handler.
+ *
+ * \note  Lower powered sleep requires use of the \link alarm_pool_get_default default alarm pool\endlink which may
+ * be disabled by the #PICO_TIME_DEFAULT_ALARM_POOL_DISABLED define or currently full in which case these functions
+ * become busy waits instead.
+ *
+ * \note  Whilst \a sleep_ functions are preferable to \a busy_wait functions from a power perspective, the \a busy_wait equivalent function
+ * may return slightly sooner after the target is reached.
+ *
+ * \sa busy_wait_until() \sa busy_wait_us() \sa busy_wait_us_32()
+ */
+
+/*! \brief Wait until after the given timestamp to return
+ * \ingroup sleep
+ *
+ * \note  This method attempts to perform a lower power (WFE) sleep
+ *
+ * \param target the time after which to return
+ * \sa sleep_us()
+ * \sa busy_wait_until()
+ * */
+void sleep_until(absolute_time_t target);
+
+/*! \brief Wait for the given number of microseconds before returning
+ * \ingroup sleep
+ *
+ * \note This method attempts to perform a lower power (WFE) sleep
+ *
+ * \param us the number of microseconds to sleep
+ * \sa busy_wait_us()
+ */
+void sleep_us(uint64_t us);
+
+/*! \brief Wait for the given number of milliseconds before returning
+ * \ingroup sleep
+ *
+ * \note This method attempts to perform a lower power sleep (using WFE) as much as possible.
+ *
+ * \param ms the number of milliseconds to sleep
+ */
+void sleep_ms(uint32_t ms);
+
+/*! \brief Helper method for blocking on a timeout
+ * \ingroup sleep
+ *
+ * This method will return in response to a an event (as per __wfe) or
+ * when the target time is reached, or at any point before.
+ *
+ * This method can be used to implement a lower power polling loop waiting on
+ * some condition signalled by an event (__sev()).
+ *
+ * This is called \a best_effort because under certain circumstances (notably the default timer pool
+ * being disabled or full) the best effort is simply to return immediately without a __wfe, thus turning the calling
+ * code into a busy wait.
+ *
+ * Example usage:
+ * ```c
+ * bool my_function_with_timeout_us(uint64_t timeout_us) {
+ *     absolute_time_t timeout_time = make_timeout_time_us(timeout_us);
+ *     do {
+ *         // each time round the loop, we check to see if the condition
+ *         // we are waiting on has happened
+ *         if (my_check_done()) {
+ *             // do something
+ *             return true;
+ *         }
+ *         // will try to sleep until timeout or the next processor event 
+ *     } while (!best_effort_wfe_or_timeout(timeout_time));
+ *     return false; // timed out
+ * }
+ * ```
+ *
+ * @param timeout_timestamp the timeout time
+ * @return true if the target time is reached, false otherwise
+ */
+bool best_effort_wfe_or_timeout(absolute_time_t timeout_timestamp);
+
+/*!
+ * \defgroup alarm alarm
+ * \ingroup pico_time
+ * \brief Alarm functions for scheduling future execution
+ *
+ *  Alarms are added to alarm pools, which may hold a certain fixed number of active alarms. Each alarm pool
+ *  utilizes one of four underlying hardware alarms, thus you may have up to four alarm pools. An alarm pool
+ *  calls (except when the callback would happen before or during being set) the callback on the core from which
+ *  the alarm pool was created. Callbacks are called from the hardware alarm IRQ handler, so care must
+ *  be taken in their implementation.
+ *
+ *  A default pool is created  the core specified by PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM
+ *  on core 0, and may be used by the method variants that take no alarm pool parameter.
+ *
+ * \sa struct alarm_pool
+ * \sa hardware_timer
+ */
+
+// PICO_CONFIG: PICO_TIME_DEFAULT_ALARM_POOL_DISABLED, Disable the default alarm pool, type=bool, default=0, advanced=true, group=pico_time
+#ifndef PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+/*!
+ * \brief If 1 then the default alarm pool is disabled (so no hardware alarm is claimed for the pool)
+ *
+ * \note Setting to 1 may cause some code not to compile as default timer pool related methods are removed
+ *
+ * \note When the default alarm pool is disabled, \a sleep_ methods and timeouts are no longer lower powered
+ * (they become \a busy_wait_)
+ *
+ * \ingroup alarm
+ * \sa #PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM
+ * \sa alarm_pool_get_default()
+ */
+#define PICO_TIME_DEFAULT_ALARM_POOL_DISABLED 0
+#endif
+
+// PICO_CONFIG: PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM, Select which HW alarm is used for the default alarm pool, min=0, max=3, default=3, advanced=true, group=pico_time
+#ifndef PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM
+/*!
+ * \brief Selects which hardware alarm is used for the default alarm pool
+ * \ingroup alarm
+ * \sa alarm_pool_get_default()
+ */
+#define PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM 3
+#endif
+
+// PICO_CONFIG: PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS, Selects the maximum number of concurrent timers in the default alarm pool, min=0, max=255, default=16, advanced=true, group=pico_time
+#ifndef PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS
+/*!
+ * \brief Selects the maximum number of concurrent timers in the default alarm pool
+ * \ingroup alarm
+ *
+ * \note For implementation reasons this is limited to PICO_PHEAP_MAX_ENTRIES which defaults to 255
+ * \sa #PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM
+ * \sa alarm_pool_get_default()
+ */
+#define PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS 16
+#endif
+
+/**
+ * \brief The identifier for an alarm
+ *
+ * \note this identifier is signed because -1 is used as an error condition when creating alarms
+ *
+ * \note alarm ids may be reused, however for convenience the implementation makes an attempt to defer
+ * reusing as long as possible. You should certainly expect it to be hundreds of ids before one is
+ * reused, although in most cases it is more. Nonetheless care must still be taken when cancelling
+ * alarms or other functionality based on alarms when the alarm may have expired, as eventually
+ * the alarm id may be reused for another alarm.
+ *
+ * \ingroup alarm
+ */
+typedef int32_t alarm_id_t; // note this is signed because we use -1 as a meaningful error value
+
+/**
+ * \brief User alarm callback
+ * \ingroup alarm
+ * \param id the alarm_id as returned when the alarm was added
+ * \param user_data the user data passed when the alarm was added
+ * \return <0 to reschedule the same alarm this many us from the time the alarm was previously scheduled to fire
+ * \return >0 to reschedule the same alarm this many us from the time this method returns
+ * \return 0 to not reschedule the alarm
+ */
+typedef int64_t (*alarm_callback_t)(alarm_id_t id, void *user_data);
+
+typedef struct alarm_pool alarm_pool_t;
+
+/**
+ * \brief Create the default alarm pool (if not already created or disabled)
+ * \ingroup alarm
+ */
+void alarm_pool_init_default();
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+/*!
+ * \brief The default alarm pool used when alarms are added without specifying an alarm pool,
+ *        and also used by the Pico SDK to support lower power sleeps and timeouts.
+ *
+ * \ingroup alarm
+ * \sa #PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM
+ */
+alarm_pool_t *alarm_pool_get_default();
+#endif
+
+/**
+ * \brief Create an alarm pool
+ *
+ * The alarm pool will call callbacks from an alarm IRQ Handler on the core of this function is called from.
+ *
+ * In many situations there is never any need for anything other than the default alarm pool, however you
+ * might want to create another if you want alarm callbacks on core 1 or require alarm pools of
+ * different priority (IRQ priority based preemption of callbacks)
+ *
+ * \note This method will hard assert if the hardware alarm is already claimed.
+ *
+ * \ingroup alarm
+ * \param hardware_alarm_num the hardware alarm to use to back this pool
+ * \param max_timers the maximum number of timers
+ *        \note For implementation reasons this is limited to PICO_PHEAP_MAX_ENTRIES which defaults to 255
+ * \sa alarm_pool_get_default()
+ * \sa hardware_claiming
+ */
+alarm_pool_t *alarm_pool_create(uint hardware_alarm_num, uint max_timers);
+
+/**
+ * \brief Return the hardware alarm used by an alarm pool
+ * \ingroup alarm
+ * \param pool the pool
+ * \return the hardware alarm used by the pool
+ */
+uint alarm_pool_hardware_alarm_num(alarm_pool_t *pool);
+
+/**
+ * \brief Destroy the alarm pool, cancelling all alarms and freeing up the underlying hardware alarm
+ * \ingroup alarm
+ * \param pool the pool
+ * \return the hardware alarm used by the pool
+ */
+void alarm_pool_destroy(alarm_pool_t *pool);
+
+/*!
+ * \brief Add an alarm callback to be called at a specific time
+ * \ingroup alarm
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core the alarm pool was created on. If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param pool the alarm pool to use for scheduling the callback (this determines which hardware alarm is used, and which core calls the callback)
+ * @param time the timestamp when (after which) the callback should fire
+ * @param callback the callback function
+ * @param user_data user data to pass to the callback function
+ * @param fire_if_past if true, this method will call the callback itself before returning 0 if the timestamp happens before or during this method call
+ * @return >0 the alarm id
+ * @return 0 the target timestamp was during or before this method call (whether the callback was called depends on fire_if_past)
+ * @return -1 if there were no alarm slots available
+ */
+alarm_id_t alarm_pool_add_alarm_at(alarm_pool_t *pool, absolute_time_t time, alarm_callback_t callback, void *user_data, bool fire_if_past);
+
+/*!
+ * \brief Add an alarm callback to be called after a delay specified in microseconds
+ * \ingroup alarm
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core the alarm pool was created on. If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param pool the alarm pool to use for scheduling the callback (this determines which hardware alarm is used, and which core calls the callback)
+ * @param us the delay (from now) in microseconds when (after which) the callback should fire
+ * @param callback the callback function
+ * @param user_data user data to pass to the callback function
+ * @param fire_if_past if true, this method will call the callback itself before returning 0 if the timestamp happens before or during this method call
+ * @return >0 the alarm id
+ * @return 0 the target timestamp was during or before this method call (whether the callback was called depends on fire_if_past)
+ * @return -1 if there were no alarm slots available
+ */
+static inline alarm_id_t alarm_pool_add_alarm_in_us(alarm_pool_t *pool, uint64_t us, alarm_callback_t callback, void *user_data, bool fire_if_past) {
+    return alarm_pool_add_alarm_at(pool, delayed_by_us(get_absolute_time(), us), callback, user_data, fire_if_past);
+}
+
+/*!
+ * \brief Add an alarm callback to be called after a delay specified in milliseconds
+ * \ingroup alarm
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core the alarm pool was created on. If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param pool the alarm pool to use for scheduling the callback (this determines which hardware alarm is used, and which core calls the callback)
+ * @param ms the delay (from now) in milliseconds when (after which) the callback should fire
+ * @param callback the callback function
+ * @param user_data user data to pass to the callback function
+ * @param fire_if_past if true, this method will call the callback itself before returning 0 if the timestamp happens before or during this method call
+ * @return >0 the alarm id
+ * @return 0 the target timestamp was during or before this method call (whether the callback was called depends on fire_if_past)
+ * @return -1 if there were no alarm slots available
+ */
+static inline alarm_id_t alarm_pool_add_alarm_in_ms(alarm_pool_t *pool, uint32_t ms, alarm_callback_t callback, void *user_data, bool fire_if_past) {
+    return alarm_pool_add_alarm_at(pool, delayed_by_ms(get_absolute_time(), ms), callback, user_data, fire_if_past);
+}
+
+/*!
+ * \brief Cancel an alarm
+ * \ingroup alarm
+ * \param pool the alarm_pool containing the alarm
+ * \param alarm_id the alarm
+ * \return true if the alarm was cancelled, false if it didn't exist
+ * \sa alarm_id_t for a note on reuse of IDs
+ */
+bool alarm_pool_cancel_alarm(alarm_pool_t *pool, alarm_id_t alarm_id);
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+/*!
+ * \brief Add an alarm callback to be called at a specific time
+ * \ingroup alarm
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core of the default alarm pool (generally core 0). If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param time the timestamp when (after which) the callback should fire
+ * @param callback the callback function
+ * @param user_data user data to pass to the callback function
+ * @param fire_if_past if true, this method will call the callback itself before returning 0 if the timestamp happens before or during this method call
+ * @return >0 the alarm id
+ * @return 0 the target timestamp was during or before this method call (whether the callback was called depends on fire_if_past)
+ * @return -1 if there were no alarm slots available
+ */
+static inline alarm_id_t add_alarm_at(absolute_time_t time, alarm_callback_t callback, void *user_data, bool fire_if_past) {
+    return alarm_pool_add_alarm_at(alarm_pool_get_default(), time, callback, user_data, fire_if_past);
+}
+
+/*!
+ * \brief Add an alarm callback to be called after a delay specified in microseconds
+ * \ingroup alarm
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core of the default alarm pool (generally core 0). If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param us the delay (from now) in microseconds when (after which) the callback should fire
+ * @param callback the callback function
+ * @param user_data user data to pass to the callback function
+ * @param fire_if_past if true, this method will call the callback itself before returning 0 if the timestamp happens before or during this method call
+ * @return >0 the alarm id
+ * @return 0 the target timestamp was during or before this method call (whether the callback was called depends on fire_if_past)
+ * @return -1 if there were no alarm slots available
+ */
+static inline alarm_id_t add_alarm_in_us(uint64_t us, alarm_callback_t callback, void *user_data, bool fire_if_past) {
+    return alarm_pool_add_alarm_in_us(alarm_pool_get_default(), us, callback, user_data, fire_if_past);
+}
+
+/*!
+ * \brief Add an alarm callback to be called after a delay specified in milliseconds
+ * \ingroup alarm
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core of the default alarm pool (generally core 0). If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param ms the delay (from now) in milliseconds when (after which) the callback should fire
+ * @param callback the callback function
+ * @param user_data user data to pass to the callback function
+ * @param fire_if_past if true, this method will call the callback itself before returning 0 if the timestamp happens before or during this method call
+ * @return >0 the alarm id
+ * @return 0 the target timestamp was during or before this method call (whether the callback was called depends on fire_if_past)
+ * @return -1 if there were no alarm slots available
+ */
+static inline alarm_id_t add_alarm_in_ms(uint32_t ms, alarm_callback_t callback, void *user_data, bool fire_if_past) {
+    return alarm_pool_add_alarm_in_ms(alarm_pool_get_default(), ms, callback, user_data, fire_if_past);
+}
+/*!
+ * \brief Cancel an alarm from the default alarm pool
+ * \ingroup alarm
+ * \param alarm_id the alarm
+ * \return true if the alarm was cancelled, false if it didn't exist
+ * \sa alarm_id_t for a note on reuse of IDs
+ */
+static inline bool cancel_alarm(alarm_id_t alarm_id) {
+    return alarm_pool_cancel_alarm(alarm_pool_get_default(), alarm_id);
+}
+
+#endif
+
+/*!
+ * \defgroup repeating_timer repeating_timer
+ * \ingroup pico_time
+ * \brief Repeating Timer functions for simple scheduling of repeated execution
+ *
+ * \note The regular \a alarm_ functionality can be used to make repeating alarms (by return non zero from the callback),
+ * however these methods abstract that further (at the cost of a user structure to store the repeat delay in (which
+ * the alarm framework does not have space for).
+ */
+
+typedef struct repeating_timer repeating_timer_t;
+
+/**
+ * \brief Callback for a repeating timer
+ * \ingroup repeating_timer
+ * \param rt repeating time structure containing information about the repeating time. user_data is of primary important to the user
+ * \return true to continue repeating, false to stop.
+ */
+typedef bool (*repeating_timer_callback_t)(repeating_timer_t *rt);
+
+/**
+ * \brief Information about a repeating timer
+ * \ingroup repeating_timer
+ * \return
+ */
+struct repeating_timer {
+    int64_t delay_us;
+    alarm_pool_t *pool;
+    alarm_id_t alarm_id;
+    repeating_timer_callback_t callback;
+    void *user_data;
+};
+
+/*!
+ * \brief Add a repeating timer that is called repeatedly at the specified interval in microseconds
+ * \ingroup repeating_timer
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core the alarm pool was created on. If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param pool the alarm pool to use for scheduling the repeating timer (this determines which hardware alarm is used, and which core calls the callback)
+ * @param delay_us the repeat delay in microseconds; if >0 then this is the delay between one callback ending and the next starting; if <0 then this is the negative of the time between the starts of the callbacks. The value of 0 is treated as 1
+ * @param callback the repeating timer callback function
+ * @param user_data user data to pass to store in the repeating_timer structure for use by the callback.
+ * @param out the pointer to the user owned structure to store the repeating timer info in. BEWARE this storage location must outlive the repeating timer, so be careful of using stack space
+ * @return false if there were no alarm slots available to create the timer, true otherwise.
+ */
+bool alarm_pool_add_repeating_timer_us(alarm_pool_t *pool, int64_t delay_us, repeating_timer_callback_t callback, void *user_data, repeating_timer_t *out);
+
+/*!
+ * \brief Add a repeating timer that is called repeatedly at the specified interval in milliseconds
+ * \ingroup repeating_timer
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core the alarm pool was created on. If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param pool the alarm pool to use for scheduling the repeating timer (this determines which hardware alarm is used, and which core calls the callback)
+ * @param delay_ms the repeat delay in milliseconds; if >0 then this is the delay between one callback ending and the next starting; if <0 then this is the negative of the time between the starts of the callbacks. The value of 0 is treated as 1 microsecond
+ * @param callback the repeating timer callback function
+ * @param user_data user data to pass to store in the repeating_timer structure for use by the callback.
+ * @param out the pointer to the user owned structure to store the repeating timer info in. BEWARE this storage location must outlive the repeating timer, so be careful of using stack space
+ * @return false if there were no alarm slots available to create the timer, true otherwise.
+ */
+static inline bool alarm_pool_add_repeating_timer_ms(alarm_pool_t *pool, int32_t delay_ms, repeating_timer_callback_t callback, void *user_data, repeating_timer_t *out) {
+    return alarm_pool_add_repeating_timer_us(pool, delay_ms * (int64_t)1000, callback, user_data, out);
+}
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+/*!
+ * \brief Add a repeating timer that is called repeatedly at the specified interval in microseconds
+ * \ingroup repeating_timer
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core of the default alarm pool (generally core 0). If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param delay_us the repeat delay in microseconds; if >0 then this is the delay between one callback ending and the next starting; if <0 then this is the negative of the time between the starts of the callbacks. The value of 0 is treated as 1
+ * @param callback the repeating timer callback function
+ * @param user_data user data to pass to store in the repeating_timer structure for use by the callback.
+ * @param out the pointer to the user owned structure to store the repeating timer info in. BEWARE this storage location must outlive the repeating timer, so be careful of using stack space
+ * @return false if there were no alarm slots available to create the timer, true otherwise.
+ */
+static inline bool add_repeating_timer_us(int64_t delay_us, repeating_timer_callback_t callback, void *user_data, repeating_timer_t *out) {
+    return alarm_pool_add_repeating_timer_us(alarm_pool_get_default(), delay_us, callback, user_data, out);
+}
+
+/*!
+ * \brief Add a repeating timer that is called repeatedly at the specified interval in milliseconds
+ * \ingroup repeating_timer
+ *
+ * Generally the callback is called as soon as possible after the time specified from an IRQ handler
+ * on the core of the default alarm pool (generally core 0). If the callback is in the past or happens before
+ * the alarm setup could be completed, then this method will optionally call the callback itself
+ * and then return a return code to indicate that the target time has passed.
+ *
+ * \note It is safe to call this method from an IRQ handler (including alarm callbacks), and from either core.
+ *
+ * @param delay_ms the repeat delay in milliseconds; if >0 then this is the delay between one callback ending and the next starting; if <0 then this is the negative of the time between the starts of the callbacks. The value of 0 is treated as 1 microsecond
+ * @param callback the repeating timer callback function
+ * @param user_data user data to pass to store in the repeating_timer structure for use by the callback.
+ * @param out the pointer to the user owned structure to store the repeating timer info in. BEWARE this storage location must outlive the repeating timer, so be careful of using stack space
+ * @return false if there were no alarm slots available to create the timer, true otherwise.
+ */
+static inline bool add_repeating_timer_ms(int32_t delay_ms, repeating_timer_callback_t callback, void *user_data, repeating_timer_t *out) {
+    return alarm_pool_add_repeating_timer_us(alarm_pool_get_default(), delay_ms * (int64_t)1000, callback, user_data, out);
+}
+#endif
+
+/**
+ * \brief Cancel a repeating timer
+ * \ingroup repeating_timer
+ * \param timer the repeating timer to cancel
+ * \return true if the repeating timer was cancelled, false if it didn't exist
+ * \sa alarm_id_t for a note on reuse of IDs
+ */
+bool cancel_repeating_timer(repeating_timer_t *timer);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/common/pico_time/include/pico/timeout_helper.h

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_TIMEOUT_HELPER_H
+#define _PICO_TIMEOUT_HELPER_H
+
+#include "pico/time.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct timeout_state {
+    absolute_time_t next_timeout;
+    uint64_t param;
+} timeout_state_t;
+
+typedef bool (*check_timeout_fn)(timeout_state_t *ts);
+
+check_timeout_fn init_single_timeout_until(timeout_state_t *ts, absolute_time_t target);
+check_timeout_fn init_per_iteration_timeout_us(timeout_state_t *ts, uint64_t per_iteration_timeout_us);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/common/pico_time/time.c

@@ -0,0 +1,361 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <limits.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "pico.h"
+#include "pico/time.h"
+#include "pico/util/pheap.h"
+#include "hardware/sync.h"
+#include "hardware/gpio.h"
+
+CU_REGISTER_DEBUG_PINS(core)
+CU_SELECT_DEBUG_PINS(core)
+
+const absolute_time_t ABSOLUTE_TIME_INITIALIZED_VAR(nil_time, 0);
+// use LONG_MAX not ULONG_MAX so we don't have sign overflow in time diffs
+const absolute_time_t ABSOLUTE_TIME_INITIALIZED_VAR(at_the_end_of_time, ULONG_MAX);
+
+typedef struct alarm_pool_entry {
+    absolute_time_t target;
+    alarm_callback_t callback;
+    void *user_data;
+} alarm_pool_entry_t;
+
+typedef struct alarm_pool {
+    pheap_t *heap;
+    spin_lock_t *lock;
+    alarm_pool_entry_t *entries;
+    // one byte per entry, used to provide more longevity to public IDs than heap node ids do
+    // (this is increment every time the heap node id is re-used)
+    uint8_t *entry_ids_high;
+    alarm_id_t alarm_in_progress; // this is set during a callback from the IRQ handler... it can be cleared by alarm_cancel to prevent repeats
+    uint8_t hardware_alarm_num;
+} alarm_pool_t;
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+static alarm_pool_t *default_alarm_pool;
+#endif
+static alarm_pool_t *pools[NUM_TIMERS];
+
+void alarm_pool_init_default() {
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+    // allow multiple calls for ease of use from host tests
+    if (!default_alarm_pool) {
+        default_alarm_pool = alarm_pool_create(PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM,
+                                               PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS);
+    }
+#endif
+}
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+alarm_pool_t *alarm_pool_get_default() {
+    assert(default_alarm_pool);
+    return default_alarm_pool;
+}
+#endif
+
+static inline alarm_pool_entry_t *get_entry(alarm_pool_t *pool, pheap_node_id_t id) {
+    assert(id && id <= pool->heap->max_nodes);
+    return pool->entries + id - 1;
+}
+
+static inline uint8_t *get_entry_id_high(alarm_pool_t *pool, pheap_node_id_t id) {
+    assert(id && id <= pool->heap->max_nodes);
+    return pool->entry_ids_high + id - 1;
+}
+
+bool timer_pool_entry_comparator(void *user_data, pheap_node_id_t a, pheap_node_id_t b) {
+    alarm_pool_t *pool = (alarm_pool_t *)user_data;
+    return to_us_since_boot(get_entry(pool, a)->target) < to_us_since_boot(get_entry(pool, b)->target);
+}
+
+static inline alarm_id_t make_public_id(uint8_t id_high, pheap_node_id_t id) {
+    return ((uint)id_high << 8u * sizeof(id)) | id;
+}
+
+static alarm_id_t add_alarm_under_lock(alarm_pool_t *pool, absolute_time_t time, alarm_callback_t callback,
+                                       void *user_data, alarm_id_t reuse_id, bool create_if_past, bool *missed) {
+    alarm_id_t id;
+    if (reuse_id) {
+        assert(!ph_contains(pool->heap, reuse_id));
+        id = reuse_id;
+    } else {
+        id = ph_new_node(pool->heap);
+    }
+    if (id) {
+        alarm_pool_entry_t *entry = get_entry(pool, id);
+        entry->target = time;
+        entry->callback = callback;
+        entry->user_data = user_data;
+        DEBUG_PINS_SET(core, 1);
+        if (id == ph_insert(pool->heap, id)) {
+            bool is_missed = hardware_alarm_set_target(pool->hardware_alarm_num, time);
+            if (is_missed && !create_if_past) {
+                ph_delete(pool->heap, id);
+            }
+            if (missed) *missed = is_missed;
+        }
+    }
+    return id;
+}
+
+static void alarm_pool_alarm_callback(uint alarm_num) {
+    // note this is called from timer IRQ handler
+    alarm_pool_t *pool = pools[alarm_num];
+    bool again;
+    do {
+        absolute_time_t now = get_absolute_time();
+        alarm_callback_t callback = NULL;
+        absolute_time_t target = nil_time;
+        void *user_data = NULL;
+        uint8_t id_high;
+        again = false;
+        uint32_t save = spin_lock_blocking(pool->lock);
+        pheap_node_id_t next_id = ph_peek_head(pool->heap);
+        if (next_id) {
+            alarm_pool_entry_t *entry = get_entry(pool, next_id);
+            if (absolute_time_diff_us(now, entry->target) <= 0) {
+                // we reserve the id in case we need to re-add the timer
+                pheap_node_id_t __unused removed_id = ph_remove_head_reserve(pool->heap, true);
+                assert(removed_id == next_id); // will be true under lock
+                target = entry->target;
+                callback = entry->callback;
+                user_data = entry->user_data;
+                assert(callback);
+                id_high = *get_entry_id_high(pool, next_id);
+                pool->alarm_in_progress = make_public_id(id_high, removed_id);
+            } else {
+                if (hardware_alarm_set_target(alarm_num, entry->target)) {
+                    again = true;
+                }
+            }
+        }
+        spin_unlock(pool->lock, save);
+        if (callback) {
+            DEBUG_PINS_SET(core, 4);
+            DEBUG_PINS_CLR(core, 1);
+            DEBUG_PINS_CLR(core, 4);
+            int64_t repeat = callback(make_public_id(id_high, next_id), user_data);
+            save = spin_lock_blocking(pool->lock);
+            // todo think more about whether we want to keep calling
+            if (repeat < 0 && pool->alarm_in_progress) {
+                assert(pool->alarm_in_progress == make_public_id(id_high, next_id));
+                add_alarm_under_lock(pool, delayed_by_us(target, -repeat), callback, user_data, next_id, true, NULL);
+            } else if (repeat > 0 && pool->alarm_in_progress) {
+                assert(pool->alarm_in_progress == make_public_id(id_high, next_id));
+                add_alarm_under_lock(pool, delayed_by_us(get_absolute_time(), repeat), callback, user_data, next_id,
+                                     true, NULL);
+            } else {
+                // need to return the id to the heap
+                ph_add_to_free_list(pool->heap, next_id);
+                (*get_entry_id_high(pool, next_id))++; // we bump it for next use of id
+            }
+            pool->alarm_in_progress = 0;
+            spin_unlock(pool->lock, save);
+            again = true;
+        }
+    } while (again);
+}
+
+// note the timer is create with IRQs on this core
+alarm_pool_t *alarm_pool_create(uint hardware_alarm_num, uint max_timers) {
+    hardware_alarm_claim(hardware_alarm_num);
+    hardware_alarm_cancel(hardware_alarm_num);
+    hardware_alarm_set_callback(hardware_alarm_num, alarm_pool_alarm_callback);
+    alarm_pool_t *pool = (alarm_pool_t *)malloc(sizeof(alarm_pool_t));
+    pool->lock = spin_lock_instance(next_striped_spin_lock_num());
+    pool->heap = ph_create(max_timers, timer_pool_entry_comparator, pool);
+    pool->entries = (alarm_pool_entry_t *)calloc(max_timers, sizeof(alarm_pool_entry_t));
+    pool->entry_ids_high = (uint8_t *)calloc(max_timers, sizeof(uint8_t));
+    pool->hardware_alarm_num = hardware_alarm_num;
+    pools[hardware_alarm_num] = pool;
+    return pool;
+}
+
+void alarm_pool_destroy(alarm_pool_t *pool) {
+    assert(pools[pool->hardware_alarm_num] == pool);
+    pools[pool->hardware_alarm_num] = NULL;
+    // todo clear out timers
+    ph_destroy(pool->heap);
+    hardware_alarm_set_callback(pool->hardware_alarm_num, NULL);
+    hardware_alarm_unclaim(pool->hardware_alarm_num);
+    free(pool->entry_ids_high);
+    free(pool->entries);
+    free(pool);
+}
+
+alarm_id_t alarm_pool_add_alarm_at(alarm_pool_t *pool, absolute_time_t time, alarm_callback_t callback,
+                                   void *user_data, bool fire_if_past) {
+    bool missed = false;
+
+    uint public_id;
+    do {
+        uint8_t id_high = 0;
+        uint32_t save = spin_lock_blocking(pool->lock);
+        pheap_node_id_t id = add_alarm_under_lock(pool, time, callback, user_data, 0, false, &missed);
+        if (id) id_high = *get_entry_id_high(pool, id);
+
+        spin_unlock(pool->lock, save);
+
+        if (!id) {
+            return -1;
+        }
+
+        public_id = missed ? 0 : make_public_id(id_high, id);
+        if (missed && fire_if_past) {
+            int64_t repeat = callback(public_id, user_data);
+            if (!repeat) {
+                public_id = 0;
+                break;
+            } else if (repeat < 0) {
+                time = delayed_by_us(time, -repeat);
+            } else {
+                time = delayed_by_us(get_absolute_time(), repeat);
+            }
+        } else {
+            break;
+        }
+    } while (true);
+    return public_id;
+}
+
+bool alarm_pool_cancel_alarm(alarm_pool_t *pool, alarm_id_t alarm_id) {
+    bool rc = false;
+    uint32_t save = spin_lock_blocking(pool->lock);
+    pheap_node_id_t id = (pheap_node_id_t) alarm_id;
+    if (ph_contains(pool->heap, id)) {
+        assert(alarm_id != pool->alarm_in_progress); // it shouldn't be in the heap if it is in progress
+        // check we have the right high value
+        uint8_t id_high = (uint8_t)((uint)alarm_id >> 8u * sizeof(pheap_node_id_t));
+        if (id_high == *get_entry_id_high(pool, id)) {
+            rc = ph_delete(pool->heap, id);
+            // note we don't bother to remove the actual hardware alarm timeout...
+            // it will either do callbacks or not depending on other alarms, and reset the next timeout itself
+            assert(rc);
+        }
+    } else {
+        if (alarm_id == pool->alarm_in_progress) {
+            // make sure the alarm doesn't repeat
+            pool->alarm_in_progress = 0;
+        }
+    }
+    spin_unlock(pool->lock, save);
+    return rc;
+}
+
+uint alarm_pool_hardware_alarm_num(alarm_pool_t *pool) {
+    return pool->hardware_alarm_num;
+}
+
+static void alarm_pool_dump_key(pheap_node_id_t id, void *user_data) {
+    alarm_pool_t *pool = (alarm_pool_t *)user_data;
+#if PICO_ON_DEVICE
+    printf("%lld (hi %02x)", to_us_since_boot(get_entry(pool, id)->target), *get_entry_id_high(pool, id));
+#else
+    printf("%ld", to_us_since_boot(get_entry(pool, id)->target));
+#endif
+}
+
+static int64_t repeating_timer_callback(alarm_id_t id, void *user_data) {
+    repeating_timer_t *rt = (repeating_timer_t *)user_data;
+    if (rt->callback(rt)) {
+        return rt->delay_us;
+    } else {
+        rt->alarm_id = 0;
+        return 0;
+    }
+}
+
+bool alarm_pool_add_repeating_timer_us(alarm_pool_t *pool, int64_t delay_us, repeating_timer_callback_t callback, void *user_data, repeating_timer_t *out) {
+    if (!delay_us) delay_us = 1;
+    out->pool = pool;
+    out->callback = callback;
+    out->delay_us = delay_us;
+    out->user_data = user_data;
+    out->alarm_id = alarm_pool_add_alarm_at(pool, make_timeout_time_us(delay_us >= 0 ? delay_us : -delay_us), repeating_timer_callback, out, true);
+    return out->alarm_id > 0;
+}
+
+bool cancel_repeating_timer(repeating_timer_t *timer) {
+    bool rc = false;
+    if (timer->alarm_id) {
+        rc = alarm_pool_cancel_alarm(timer->pool, timer->alarm_id);
+        timer->alarm_id = 0;
+    }
+    return rc;
+}
+
+void alarm_pool_dump(alarm_pool_t *pool) {
+    uint32_t save = spin_lock_blocking(pool->lock);
+    ph_dump(pool->heap, alarm_pool_dump_key, pool);
+    spin_unlock(pool->lock, save);
+}
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+static int64_t sev_callback(alarm_id_t id, void *user_data) {
+    __sev();
+    return 0;
+}
+#endif
+
+void sleep_until(absolute_time_t t) {
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+    uint64_t t_us = to_us_since_boot(t);
+    uint64_t t_before_us = t_us - PICO_TIME_SLEEP_OVERHEAD_ADJUST_US;
+    // needs to work in the first PICO_TIME_SLEEP_OVERHEAD_ADJUST_US of boot
+    if (t_before_us > t_us) t_before_us = 0;
+    absolute_time_t t_before;
+    update_us_since_boot(&t_before, t_before_us);
+    if (absolute_time_diff_us(get_absolute_time(), t_before) > 0) {
+        if (add_alarm_at(t_before, sev_callback, NULL, false) >= 0) {
+            // able to add alarm for just before the time
+            while (!time_reached(t_before)) {
+                __wfe();
+            }
+        }
+    }
+#endif
+    // now wait until the exact time
+    busy_wait_until(t);
+}
+
+void sleep_us(uint64_t us) {
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+    sleep_until(make_timeout_time_us(us));
+#else
+    if (us >> 32u) {
+        busy_wait_until(make_timeout_time_us(us));
+    } else {
+        busy_wait_us_32(us);
+    }
+#endif
+
+}
+
+void sleep_ms(uint32_t ms) {
+    sleep_us(ms * 1000ull);
+}
+
+bool best_effort_wfe_or_timeout(absolute_time_t timeout_timestamp) {
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+    alarm_id_t id;
+    id = add_alarm_at(timeout_timestamp, sev_callback, NULL, false);
+    if (id <= 0) {
+        tight_loop_contents();
+        return time_reached(timeout_timestamp);
+    } else {
+        __wfe();
+        // we need to clean up if it wasn't us that caused the wfe; if it was this will be a noop.
+        cancel_alarm(id);
+        return time_reached(timeout_timestamp);
+    }
+#else
+    tight_loop_contents();
+    return time_reached(timeout_timestamp);
+#endif
+}

src/common/pico_time/timeout_helper.c

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "pico/timeout_helper.h"
+
+static bool check_single_timeout_us(timeout_state_t *ts) {
+    return time_reached(ts->next_timeout);
+}
+
+check_timeout_fn init_single_timeout_until(timeout_state_t *ts, absolute_time_t target) {
+    ts->next_timeout = target;
+    return check_single_timeout_us;
+}
+
+static bool check_per_iteration_timeout_us(timeout_state_t *ts) {
+    if (time_reached(ts->next_timeout)) {
+        return true;
+    }
+    ts->next_timeout = make_timeout_time_us(ts->param);
+    return false;
+}
+
+check_timeout_fn init_per_iteration_timeout_us(timeout_state_t *ts, uint64_t per_iteration_timeout_us) {
+    ts->next_timeout = make_timeout_time_us(per_iteration_timeout_us);
+    ts->param = per_iteration_timeout_us;
+    return check_per_iteration_timeout_us;
+}

src/common/pico_util/CMakeLists.txt

@@ -0,0 +1,15 @@
+if (NOT TARGET pico_util_headers)
+    add_library(pico_util_headers INTERFACE)
+    target_include_directories(pico_util_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+    target_link_libraries(pico_util_headers INTERFACE pico_base_headers hardware_sync)
+endif()
+
+if (NOT TARGET pico_util)
+    add_library(pico_util INTERFACE)
+    target_sources(pico_util INTERFACE
+            ${CMAKE_CURRENT_LIST_DIR}/datetime.c
+            ${CMAKE_CURRENT_LIST_DIR}/pheap.c
+            ${CMAKE_CURRENT_LIST_DIR}/queue.c
+    )
+    target_link_libraries(pico_util INTERFACE pico_util_headers)
+endif()

src/common/pico_util/datetime.c

@@ -0,0 +1,41 @@
+#include "pico/util/datetime.h"
+
+#include <stdio.h>
+
+static const char *DATETIME_MONTHS[12] = {
+        "January",
+        "February",
+        "March",
+        "April",
+        "May",
+        "June",
+        "July",
+        "August",
+        "September",
+        "October",
+        "November",
+        "December"
+};
+
+static const char *DATETIME_DOWS[7] = {
+        "Sunday",
+        "Monday",
+        "Tuesday",
+        "Wednesday",
+        "Thursday",
+        "Friday",
+        "Saturday",
+};
+
+void datetime_to_str(char *buf, uint buf_size, const datetime_t *t) {
+    snprintf(buf,
+             buf_size,
+             "%s %d %s %d:%02d:%02d %d",
+             DATETIME_DOWS[t->dotw],
+             t->day,
+             DATETIME_MONTHS[t->month - 1],
+             t->hour,
+             t->min,
+             t->sec,
+             t->year);
+};

src/common/pico_util/doc.h

@@ -0,0 +1,4 @@
+/**
+ * \defgroup pico_util pico_util
+ * \brief Useful data structures and utility functions
+ */

src/common/pico_util/include/pico/util/datetime.h

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_DATETIME_H
+#define _PICO_DATETIME_H
+
+#include "pico.h"
+
+/** \file datetime.h
+ * \defgroup util_datetime datetime
+ * \brief Date/Time formatting
+ * \ingroup pico_util
+ */
+
+/*! \brief  Convert a datetime_t structure to a string
+ *  \ingroup util_datetime
+ *
+ * \param buf character buffer to accept generated string
+ * \param buf_size The size of the passed in buffer
+ * \param t The datetime to be converted.
+ */
+void datetime_to_str(char *buf, uint buf_size, const datetime_t *t);
+
+#endif

src/common/pico_util/include/pico/util/pheap.h

@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_UTIL_PHEAP_H
+#define _PICO_UTIL_PHEAP_H
+
+#include "pico.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_PHEAP, Enable/disable assertions in the pheap module, type=bool, default=0, group=pico_util
+#ifndef PARAM_ASSERTIONS_ENABLED_PHEAP
+#define PARAM_ASSERTIONS_ENABLED_PHEAP 0
+#endif
+
+/**
+ * \file pheap.h
+ * \defgroup util_pheap pheap
+ * Pairing Heap Implementation
+ * \ingroup pico_util
+ *
+ * pheap defines a simple pairing heap. the implementation simply tracks array indexes, it is up to
+ * the user to provide storage for heap entries and a comparison function.
+ *
+ * NOTE: this class is not safe for concurrent usage. It should be externally protected. Furthermore
+ * if used concurrently, the caller needs to protect around their use of the returned id.
+ * for example, ph_remove_head returns the id of an element that is no longer in the heap.
+ *
+ * The user can still use this to look at the data in their companion array, however obviously further operations
+ * on the heap may cause them to overwrite that data as the id may be reused on subsequent operations
+ *
+ */
+// PICO_CONFIG: PICO_PHEAP_MAX_ENTRIES, Maximum number of entries in the pheap, min=1, max=65534, default=255, group=pico_util
+#ifndef PICO_PHEAP_MAX_ENTRIES
+#define PICO_PHEAP_MAX_ENTRIES 255
+#endif
+
+// public heap_node ids are numbered from 1 (0 means none)
+#if PICO_PHEAP_MAX_ENTRIES < 256
+typedef uint8_t pheap_node_id_t;
+#elif PICO_PHEAP_MAX_ENTRIES < 65535
+typedef uint16_t pheap_node_id_t;
+#else
+#error invalid PICO_PHEAP_MAX_ENTRIES
+#endif
+
+typedef struct pheap_node {
+    pheap_node_id_t child, sibling, parent;
+} pheap_node_t;
+
+// return true if a < b in natural order
+typedef bool (*pheap_comparator)(void *user_data, pheap_node_id_t a, pheap_node_id_t b);
+
+typedef struct pheap {
+    pheap_node_t *nodes;
+    pheap_comparator comparator;
+    void *user_data;
+    pheap_node_id_t max_nodes;
+    pheap_node_id_t root_id;
+    // we remove from head and add to tail to stop reusing the same ids
+    pheap_node_id_t free_head_id;
+    pheap_node_id_t free_tail_id;
+} pheap_t;
+
+pheap_t *ph_create(uint max_nodes, pheap_comparator comparator, void *user_data);
+
+void ph_clear(pheap_t *heap);
+
+void ph_destroy(pheap_t *heap);
+
+static inline pheap_node_t *ph_get_node(pheap_t *heap, pheap_node_id_t id) {
+    assert(id && id <= heap->max_nodes);
+    return heap->nodes + id - 1;
+}
+
+static void ph_add_child_node(pheap_t *heap, pheap_node_id_t parent_id, pheap_node_id_t child_id) {
+    pheap_node_t *n = ph_get_node(heap, parent_id);
+    assert(parent_id);
+    assert(child_id);
+    assert(parent_id != child_id);
+    pheap_node_t *c = ph_get_node(heap, child_id);
+    c->parent = parent_id;
+    if (!n->child) {
+        n->child = child_id;
+    } else {
+        c->sibling = n->child;
+        n->child = child_id;
+    }
+}
+
+static pheap_node_id_t ph_merge_nodes(pheap_t *heap, pheap_node_id_t a, pheap_node_id_t b) {
+    if (!a) return b;
+    if (!b) return a;
+    if (heap->comparator(heap->user_data, a, b)) {
+        ph_add_child_node(heap, a, b);
+        return a;
+    } else {
+        ph_add_child_node(heap, b, a);
+        return b;
+    }
+}
+
+static inline pheap_node_id_t ph_new_node(pheap_t *heap) {
+    if (!heap->free_head_id) return 0;
+    pheap_node_id_t id = heap->free_head_id;
+    heap->free_head_id = ph_get_node(heap, id)->sibling;
+    if (!heap->free_head_id) heap->free_tail_id = 0;
+    return id;
+}
+
+// note this will callback the comparator for the node
+// returns the (new) root of the heap
+static inline pheap_node_id_t ph_insert(pheap_t *heap, pheap_node_id_t id) {
+    assert(id);
+    pheap_node_t *hn = ph_get_node(heap, id);
+    hn->child = hn->sibling = hn->parent = 0;
+    heap->root_id = ph_merge_nodes(heap, heap->root_id, id);
+    return heap->root_id;
+}
+
+static inline pheap_node_id_t ph_peek_head(pheap_t *heap) {
+    return heap->root_id;
+}
+
+pheap_node_id_t ph_remove_head_reserve(pheap_t *heap, bool reserve);
+
+static inline pheap_node_id_t ph_remove_head(pheap_t *heap) {
+    return ph_remove_head_reserve(heap, false);
+}
+
+static inline bool ph_contains(pheap_t *heap, pheap_node_id_t id) {
+    return id == heap->root_id || ph_get_node(heap, id)->parent;
+}
+
+bool ph_delete(pheap_t *heap, pheap_node_id_t id);
+
+static inline void ph_add_to_free_list(pheap_t *heap, pheap_node_id_t id) {
+    assert(id && !ph_contains(heap, id));
+    if (heap->free_tail_id) {
+        ph_get_node(heap, heap->free_tail_id)->sibling = id;
+    }
+    heap->free_tail_id = id;
+}
+
+void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t, void *), void *user_data);
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/common/pico_util/include/pico/util/queue.h

@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _UTIL_QUEUE_H
+#define _UTIL_QUEUE_H
+
+#include "pico.h"
+#include "hardware/sync.h"
+
+/** \file queue.h
+ * \defgroup queue queue
+ * Multi-core and IRQ safe queue implementation.
+ *
+ * Note that this queue stores values of a specified size, and pushed values are copied into the queue
+ * \ingroup pico_util
+ */
+
+typedef struct {
+    spin_lock_t *lock;
+    uint8_t *data;
+    uint16_t wptr;
+    uint16_t rptr;
+    uint16_t element_size;
+    uint16_t element_count;
+} queue_t;
+
+/*! \brief Initialise a queue with a specific spinlock for concurrency protection
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param element_size Size of each value in the queue
+ * \param element_count Maximum number of entries in the queue
+ * \param spinlock_num The spin ID used to protect the queue
+ */
+void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num);
+
+/*! \brief Initialise a queue, allocating a (possibly shared) spinlock
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param element_size Size of each value in the queue
+ * \param element_count Maximum number of entries in the queue
+ */
+static inline void queue_init(queue_t *q, uint element_size, uint element_count) {
+    return queue_init_with_spinlock(q, element_size, element_count, next_striped_spin_lock_num());
+}
+
+/*! \brief Destroy the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ *
+ * Does not deallocate the queue_t structure itself.
+ */
+void queue_free(queue_t *q);
+
+/*! \brief Unsafe check of level of the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return Number of entries in the queue
+ *
+ * This does not use the spinlock, so may return incorrect results if the
+ * spin lock is not externally locked
+ */
+static inline uint queue_get_level_unsafe(queue_t *q) {
+    int32_t rc = (int32_t)q->wptr - (int32_t)q->rptr;
+    if (rc < 0) {
+        rc += + q->element_count + 1;
+    }
+    return rc;
+}
+
+/*! \brief Check of level of the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return Number of entries in the queue
+ */
+static inline uint queue_get_level(queue_t *q) {
+    uint32_t save = spin_lock_blocking(q->lock);
+    uint level = queue_get_level_unsafe(q);
+    spin_unlock(q->lock, save);
+    return level;
+}
+
+/*! \brief Check if queue is empty
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return true if queue is empty, false otherwise
+ *
+ * This function is interrupt and multicore safe.
+ */
+static inline bool queue_is_empty(queue_t *q) {
+    return queue_get_level(q) == 0;
+}
+
+/*! \brief Check if queue is full
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return true if queue is full, false otherwise
+ *
+ * This function is interrupt and multicore safe.
+ */
+static inline bool queue_is_full(queue_t *q) {
+    return queue_get_level(q) == q->element_count;
+}
+
+// nonblocking queue access functions:
+
+/*! \brief Non-blocking add value queue if not full
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to value to be copied into the queue
+ * \return true if the value was added
+ *
+ * If the queue is full this function will return immediately with false, otherwise
+ * the data is copied into a new value added to the queue, and this function will return true.
+ */
+bool queue_try_add(queue_t *q, void *data);
+
+/*! \brief Non-blocking removal of entry from the queue if non empty
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the removed value
+ * \return true if a value was removed
+ *
+ * If the queue is not empty function will copy the removed value into the location provided and return
+ * immediately with true, otherwise the function will return immediately with false.
+ */
+bool queue_try_remove(queue_t *q, void *data);
+
+/*! \brief Non-blocking peek at the next item to be removed from the queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the peeked value
+ * \return true if there was a value to peek
+ *
+ * If the queue is not empty this function will return immediately with true with the peeked entry
+ * copied into the location specified by the data parameter, otherwise the function will return false.
+ */
+bool queue_try_peek(queue_t *q, void *data);
+
+// blocking queue access functions:
+
+/*! \brief Blocking add of value to queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to value to be copied into the queue
+ *
+ * If the queue is full this function will block, until a removal happens on the queue
+ */
+void queue_add_blocking(queue_t *q, void *data);
+
+/*! \brief Blocking remove entry from queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the removed value
+ *
+ * If the queue is empty this function will block until a value is added.
+ */
+void queue_remove_blocking(queue_t *q, void *data);
+
+/*! \brief Blocking peek at next value to be removed from queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the peeked value
+ *
+ * If the queue is empty function will block until a value is added
+ */
+void queue_peek_blocking(queue_t *q, void *data);
+
+#endif

src/common/pico_util/pheap.c

@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "pico/util/pheap.h"
+
+pheap_t *ph_create(uint max_nodes, pheap_comparator comparator, void *user_data) {
+    invalid_params_if(PHEAP, !max_nodes || max_nodes >= (1u << sizeof(pheap_node_id_t)));
+    pheap_t *heap = calloc(1, sizeof(pheap_t));
+    heap->max_nodes = max_nodes;
+    heap->comparator = comparator;
+    heap->nodes = calloc(max_nodes, sizeof(pheap_node_t));
+    heap->user_data = user_data;
+    ph_clear(heap);
+    return heap;
+}
+
+void ph_clear(pheap_t *heap) {
+    heap->root_id = 0;
+    heap->free_head_id = 1;
+    heap->free_tail_id = heap->max_nodes;
+    for(uint i = 1; i < heap->max_nodes; i++) {
+        ph_get_node(heap, i)->sibling = i + 1;
+    }
+    ph_get_node(heap, heap->max_nodes)->sibling = 0;
+}
+
+void ph_destroy(pheap_t *heap) {
+    free(heap->nodes);
+    free(heap);
+}
+
+pheap_node_id_t ph_merge_two_pass(pheap_t *heap, pheap_node_id_t id) {
+    if (!id || !ph_get_node(heap, id)->sibling) {
+        return id;
+    } else {
+        pheap_node_id_t a, b, new_node;
+        a = id;
+        b = ph_get_node(heap, id)->sibling;
+        new_node = ph_get_node(heap, b)->sibling;
+        ph_get_node(heap, a)->sibling = ph_get_node(heap, b)->sibling = 0;
+        return ph_merge_nodes(heap, ph_merge_nodes(heap, a, b), ph_merge_two_pass(heap, new_node));
+    }
+}
+
+static pheap_node_id_t ph_remove_any_head(pheap_t *heap, pheap_node_id_t root_id, bool reserve) {
+    assert(root_id);
+//    printf("Removing head %d (parent %d sibling %d)\n", root_id, ph_get_node(heap, root_id)->parent, ph_get_node(heap, root_id)->sibling);
+    assert(!ph_get_node(heap, root_id)->sibling);
+    assert(!ph_get_node(heap, root_id)->parent);
+    pheap_node_id_t new_root_id = ph_merge_two_pass(heap, ph_get_node(heap, root_id)->child);
+    if (!reserve) {
+        if (heap->free_tail_id) {
+            ph_get_node(heap, heap->free_tail_id)->sibling = root_id;
+        }
+        heap->free_tail_id = root_id;
+    }
+    if (new_root_id) ph_get_node(heap, new_root_id)->parent = 0;
+    ph_get_node(heap, root_id)->sibling = 0;
+    return new_root_id;
+}
+
+pheap_node_id_t ph_remove_head_reserve(pheap_t *heap, bool reserve) {
+    pheap_node_id_t old_root_id = ph_peek_head(heap);
+    heap->root_id = ph_remove_any_head(heap, old_root_id, reserve);
+    return old_root_id;
+}
+
+#include <stdio.h>
+bool ph_delete(pheap_t *heap, pheap_node_id_t id) {
+    // 1) trivial cases
+    if (!id) return false;
+    if (id == heap->root_id) {
+        ph_remove_head(heap);
+        return true;
+    }
+    // 2) unlink the node from the tree
+    pheap_node_t *node = ph_get_node(heap, id);
+    if (!node->parent) return false; // not in tree
+    pheap_node_t *parent = ph_get_node(heap, node->parent);
+    if (parent->child == id) {
+        parent->child = node->sibling;
+    } else {
+        pheap_node_id_t prev_sibling_id = parent->child;
+        bool __unused found = false;
+        do {
+            pheap_node_t *prev_sibling = ph_get_node(heap, prev_sibling_id);
+            if (prev_sibling->sibling == id) {
+                prev_sibling->sibling = node->sibling;
+                found = true;
+                break;
+            }
+            prev_sibling_id = prev_sibling->sibling;
+        } while (prev_sibling_id);
+        assert(found);
+    }
+    node->sibling = node->parent = 0;
+//    ph_dump(heap, NULL, NULL);
+    // 3) remove it from the head of its own subtree
+    pheap_node_id_t new_sub_tree = ph_remove_any_head(heap, id, false);
+    assert(new_sub_tree != heap->root_id);
+    heap->root_id = ph_merge_nodes(heap, heap->root_id, new_sub_tree);
+    return true;
+}
+
+static uint ph_dump_node(pheap_t *heap, pheap_node_id_t id, void (*dump_key)(pheap_node_id_t, void *), void *user_data, uint indent) {
+    uint count = 0;
+    if (id) {
+        count++;
+        for (uint i = 0; i < indent * 2; i++) {
+            putchar(' ');
+        }
+        pheap_node_t *node = ph_get_node(heap, id);
+        printf("%d (c=%d s=%d p=%d) ", id, node->child, node->sibling, node->parent);
+        if (dump_key) dump_key(id, user_data);
+        printf("\n");
+        count += ph_dump_node(heap, node->child, dump_key, user_data, indent + 1);
+        count += ph_dump_node(heap, node->sibling, dump_key, user_data, indent);
+    }
+    return count;
+}
+
+void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t, void *), void *user_data) {
+    uint count = ph_dump_node(heap, heap->root_id, dump_key, user_data, 0);
+    printf("node_count %d\n", count);
+}

src/common/pico_util/queue.c

@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include "pico/util/queue.h"
+
+void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num) {
+    q->lock = spin_lock_instance(spinlock_num);
+    q->data = (uint8_t *)calloc(element_count + 1, element_size);
+    q->element_count = element_count;
+    q->element_size = element_size;
+    q->wptr = 0;
+    q->rptr = 0;
+}
+
+void queue_free(queue_t *q) {
+    free(q->data);
+}
+
+static inline void *element_ptr(queue_t *q, uint index) {
+    assert(index <= q->element_count);
+    return q->data + index * q->element_size;
+}
+
+static inline uint16_t inc_index(queue_t *q, uint16_t index) {
+    if (++index > q->element_count) { // > because we have element_count + 1 elements
+        index = 0;
+    }
+    return index;
+}
+
+bool queue_try_add(queue_t *q, void *data) {
+    bool success = false;
+    uint32_t flags = spin_lock_blocking(q->lock);
+    if (queue_get_level_unsafe(q) != q->element_count) {
+        memcpy(element_ptr(q, q->wptr), data, q->element_size);
+        q->wptr = inc_index(q, q->wptr);
+        success = true;
+    }
+    spin_unlock(q->lock, flags);
+    if (success) __sev();
+    return success;
+}
+
+bool queue_try_remove(queue_t *q, void *data) {
+    bool success = false;
+    uint32_t flags = spin_lock_blocking(q->lock);
+    if (queue_get_level_unsafe(q) != 0) {
+        memcpy(data, element_ptr(q, q->rptr), q->element_size);
+        q->rptr = inc_index(q, q->rptr);
+        success = true;
+    }
+    spin_unlock(q->lock, flags);
+    if (success) __sev();
+    return success;
+}
+
+bool queue_try_peek(queue_t *q, void *data) {
+    bool success = false;
+    uint32_t flags = spin_lock_blocking(q->lock);
+    if (queue_get_level_unsafe(q) != 0) {
+        memcpy(data, element_ptr(q, q->rptr), q->element_size);
+        success = true;
+    }
+    spin_unlock(q->lock, flags);
+    return success;
+}
+
+void queue_add_blocking(queue_t *q, void *data) {
+    bool done;
+    do {
+        done = queue_try_add(q, data);
+        if (done) break;
+        __wfe();
+    } while (true);
+}
+
+void queue_remove_blocking(queue_t *q, void *data) {
+    bool done;
+    do {
+        done = queue_try_remove(q, data);
+        if (done) break;
+        __wfe();
+    } while (true);
+}
+
+void queue_peek_blocking(queue_t *q, void *data) {
+    bool done;
+    do {
+        done = queue_try_peek(q, data);
+        if (done) break;
+        __wfe();
+    } while (true);
+}