From 6721d64735343384fa48aa284a7b1591344e59e6 Mon Sep 17 00:00:00 2001 From: Christopher Collins Date: Wed, 17 Jan 2018 17:57:25 -0800 Subject: [PATCH] subsys: mgmt: SMP protocol for mcumgr. The Simple Management Protocol (SMP) is a basic protocol that sits on top of mcumgr's mgmt layer. This commit adds the functionality needed to hook into mcumgr's SMP layer. More information about SMP can be found at: `ext/lib/mgmt/mcumgr/smp/include/smp/smp.h`. Signed-off-by: Christopher Collins --- include/mgmt/buf.h | 58 +++++++++ include/mgmt/smp.h | 87 +++++++++++++ subsys/CMakeLists.txt | 1 + subsys/mgmt/buf.c | 168 +++++++++++++++++++++++++ subsys/mgmt/smp.c | 284 ++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 598 insertions(+) create mode 100644 include/mgmt/buf.h create mode 100644 include/mgmt/smp.h create mode 100644 subsys/mgmt/buf.c create mode 100644 subsys/mgmt/smp.c diff --git a/include/mgmt/buf.h b/include/mgmt/buf.h new file mode 100644 index 00000000000..a4a9feb4f85 --- /dev/null +++ b/include/mgmt/buf.h @@ -0,0 +1,58 @@ +/* + * Copyright Runtime.io 2018. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef H_MGMT_BUF +#define H_MGMT_BUF + +#include +#include "cbor_encoder_writer.h" +#include "cbor_decoder_reader.h" +struct net_buf; + +struct cbor_nb_reader { + struct cbor_decoder_reader r; + struct net_buf *nb; +}; + +struct cbor_nb_writer { + struct cbor_encoder_writer enc; + struct net_buf *nb; +}; + +/** + * @brief Allocates a net_buf for holding an mcumgr request or response. + * + * @return A newly-allocated buffer net_buf on success; + * NULL on failure. + */ +struct net_buf *mcumgr_buf_alloc(void); + +/** + * @brief Frees an mcumgr net_buf + * + * @param nb The net_buf to free. + */ +void mcumgr_buf_free(struct net_buf *nb); + +/** + * @brief Initializes a CBOR writer with the specified net_buf. + * + * @param cnw The writer to initialize. + * @param nb The net_buf that the writer will write to. + */ +void cbor_nb_writer_init(struct cbor_nb_writer *cnw, + struct net_buf *nb); + +/** + * @brief Initializes a CBOR reader with the specified net_buf. + * + * @param cnr The reader to initialize. + * @param nb The net_buf that the reader will read from. + */ +void cbor_nb_reader_init(struct cbor_nb_reader *cnr, + struct net_buf *nb); + +#endif diff --git a/include/mgmt/smp.h b/include/mgmt/smp.h new file mode 100644 index 00000000000..6916efc1a7c --- /dev/null +++ b/include/mgmt/smp.h @@ -0,0 +1,87 @@ +/* + * Copyright Runtime.io 2018. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef H_ZEPHYR_SMP_ +#define H_ZEPHYR_SMP_ + +#ifdef __cplusplus +extern "C" { +#endif + +struct zephyr_smp_transport; +struct net_buf; + +/** @typedef zephyr_smp_transport_out_fn + * @brief SMP transmit function for Zephyr. + * + * The supplied net_buf is always consumed, regardless of return code. + * + * @param mst The transport to send via. + * @param nb The net_buf to transmit. + * + * @return 0 on success, MGMT_ERR_[...] code on failure. + */ +typedef int zephyr_smp_transport_out_fn(struct zephyr_smp_transport *zst, + struct net_buf *nb); + +/** @typedef zephyr_smp_transport_get_mtu_fn + * @brief SMP MTU query function for Zephyr. + * + * The supplied net_buf should contain a request received from the peer whose + * MTU is being queried. This function takes a net_buf parameter because some + * transports store connection-specific information in the net_buf user header + * (e.g., the BLE transport stores the peer address). + * + * @param nb Contains a request from the relevant peer. + * + * @return The transport's MTU; + * 0 if transmission is currently not possible. + */ +typedef uint16_t zephyr_smp_transport_get_mtu_fn(const struct net_buf *nb); + +/** + * @brief Provides Zephyr-specific functionality for sending SMP responses. + */ +struct zephyr_smp_transport { + /* Must be the first member. */ + struct k_work zst_work; + + /* FIFO containing incoming requests to be processed. */ + struct k_fifo zst_fifo; + + zephyr_smp_transport_out_fn *zst_output; + zephyr_smp_transport_get_mtu_fn *zst_get_mtu; +}; + +/** + * @brief Initializes a Zephyr SMP transport object. + * + * @param zst The transport to construct. + * @param output_func The transport's send function. + * @param get_mtu_func The transport's get-MTU function. + * + * @return 0 on success, MGMT_ERR_[...] code on failure. + */ +void zephyr_smp_transport_init(struct zephyr_smp_transport *zst, + zephyr_smp_transport_out_fn *output_func, + zephyr_smp_transport_get_mtu_fn *get_mtu_func); + +/** + * @brief Enqueues an incoming SMP request packet for processing. + * + * This function always consumes the supplied net_buf. + * + * @param zst The transport to use to send the corresponding + * response(s). + * @param nb The request packet to process. + */ +void zephyr_smp_rx_req(struct zephyr_smp_transport *zst, struct net_buf *nb); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/subsys/CMakeLists.txt b/subsys/CMakeLists.txt index f80221a6f6c..cde373ef01e 100644 --- a/subsys/CMakeLists.txt +++ b/subsys/CMakeLists.txt @@ -9,5 +9,6 @@ add_subdirectory(fs) add_subdirectory_ifdef(CONFIG_MCUBOOT_IMG_MANAGER dfu) add_subdirectory_ifdef(CONFIG_NET_BUF net) add_subdirectory_ifdef(CONFIG_USB usb) +add_subdirectory(mgmt) add_subdirectory(random) add_subdirectory(storage) diff --git a/subsys/mgmt/buf.c b/subsys/mgmt/buf.c new file mode 100644 index 00000000000..01091b6353c --- /dev/null +++ b/subsys/mgmt/buf.c @@ -0,0 +1,168 @@ +/* + * Copyright Runtime.io 2018. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "net/buf.h" +#include "mgmt/buf.h" +#include "compilersupport_p.h" + +NET_BUF_POOL_DEFINE(pkt_pool, CONFIG_MCUMGR_BUF_COUNT, CONFIG_MCUMGR_BUF_SIZE, + CONFIG_MCUMGR_BUF_USER_DATA_SIZE, NULL); + +struct net_buf * +mcumgr_buf_alloc(void) +{ + return net_buf_alloc(&pkt_pool, K_NO_WAIT); +} + +void +mcumgr_buf_free(struct net_buf *nb) +{ + net_buf_unref(nb); +} + +static uint8_t +cbor_nb_reader_get8(struct cbor_decoder_reader *d, int offset) +{ + struct cbor_nb_reader *cnr; + + cnr = (struct cbor_nb_reader *) d; + + if (offset < 0 || offset >= cnr->nb->len) { + return UINT8_MAX; + } + + return cnr->nb->data[offset]; +} + +static uint16_t +cbor_nb_reader_get16(struct cbor_decoder_reader *d, int offset) +{ + struct cbor_nb_reader *cnr; + uint16_t val; + + cnr = (struct cbor_nb_reader *) d; + + if (offset < 0 || offset > cnr->nb->len - (int)sizeof(val)) { + return UINT16_MAX; + } + + memcpy(&val, cnr->nb->data + offset, sizeof(val)); + return cbor_ntohs(val); +} + +static uint32_t +cbor_nb_reader_get32(struct cbor_decoder_reader *d, int offset) +{ + struct cbor_nb_reader *cnr; + uint32_t val; + + cnr = (struct cbor_nb_reader *) d; + + if (offset < 0 || offset > cnr->nb->len - (int)sizeof(val)) { + return UINT32_MAX; + } + + memcpy(&val, cnr->nb->data + offset, sizeof(val)); + return cbor_ntohl(val); +} + +static uint64_t +cbor_nb_reader_get64(struct cbor_decoder_reader *d, int offset) +{ + struct cbor_nb_reader *cnr; + uint64_t val; + + cnr = (struct cbor_nb_reader *) d; + + if (offset < 0 || offset > cnr->nb->len - (int)sizeof(val)) { + return UINT64_MAX; + } + + memcpy(&val, cnr->nb->data + offset, sizeof(val)); + return cbor_ntohll(val); +} + +static uintptr_t +cbor_nb_reader_cmp(struct cbor_decoder_reader *d, char *buf, int offset, + size_t len) +{ + struct cbor_nb_reader *cnr; + + cnr = (struct cbor_nb_reader *) d; + + if (offset < 0 || offset > cnr->nb->len - (int)len) { + return -1; + } + + return memcmp(cnr->nb->data + offset, buf, len); +} + +static uintptr_t +cbor_nb_reader_cpy(struct cbor_decoder_reader *d, char *dst, int offset, + size_t len) +{ + struct cbor_nb_reader *cnr; + + cnr = (struct cbor_nb_reader *) d; + + if (offset < 0 || offset > cnr->nb->len - (int)len) { + return -1; + } + + return (uintptr_t)memcpy(dst, cnr->nb->data + offset, len); +} + +static uintptr_t +cbor_nb_get_string_chunk(struct cbor_decoder_reader *d, int offset, + size_t *len) +{ + struct cbor_nb_reader *cnr; + + cnr = (struct cbor_nb_reader *) d; + return (uintptr_t)cnr->nb->data + offset; +} + +void +cbor_nb_reader_init(struct cbor_nb_reader *cnr, + struct net_buf *nb) +{ + cnr->r.get8 = &cbor_nb_reader_get8; + cnr->r.get16 = &cbor_nb_reader_get16; + cnr->r.get32 = &cbor_nb_reader_get32; + cnr->r.get64 = &cbor_nb_reader_get64; + cnr->r.cmp = &cbor_nb_reader_cmp; + cnr->r.cpy = &cbor_nb_reader_cpy; + cnr->r.get_string_chunk = &cbor_nb_get_string_chunk; + + cnr->nb = nb; + cnr->r.message_size = nb->len; +} + +static int +cbor_nb_write(struct cbor_encoder_writer *writer, const char *data, int len) +{ + struct cbor_nb_writer *cnw; + + cnw = (struct cbor_nb_writer *) writer; + if (len > net_buf_tailroom(cnw->nb)) { + return CborErrorOutOfMemory; + } + + net_buf_add_mem(cnw->nb, data, len); + cnw->enc.bytes_written += len; + + return CborNoError; +} + +void +cbor_nb_writer_init(struct cbor_nb_writer *cnw, struct net_buf *nb) +{ + cnw->nb = nb; + cnw->enc.bytes_written = 0; + cnw->enc.write = &cbor_nb_write; +} diff --git a/subsys/mgmt/smp.c b/subsys/mgmt/smp.c new file mode 100644 index 00000000000..7b4c15d0c47 --- /dev/null +++ b/subsys/mgmt/smp.c @@ -0,0 +1,284 @@ +/* + * Copyright Runtime.io 2018. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include "net/buf.h" +#include "mgmt/mgmt.h" +#include "mgmt/buf.h" +#include "smp/smp.h" +#include "mgmt/smp.h" + +static mgmt_alloc_rsp_fn zephyr_smp_alloc_rsp; +static mgmt_trim_front_fn zephyr_smp_trim_front; +static mgmt_reset_buf_fn zephyr_smp_reset_buf; +static mgmt_write_at_fn zephyr_smp_write_at; +static mgmt_init_reader_fn zephyr_smp_init_reader; +static mgmt_init_writer_fn zephyr_smp_init_writer; +static mgmt_free_buf_fn zephyr_smp_free_buf; +static smp_tx_rsp_fn zephyr_smp_tx_rsp; + +static const struct mgmt_streamer_cfg zephyr_smp_cbor_cfg = { + .alloc_rsp = zephyr_smp_alloc_rsp, + .trim_front = zephyr_smp_trim_front, + .reset_buf = zephyr_smp_reset_buf, + .write_at = zephyr_smp_write_at, + .init_reader = zephyr_smp_init_reader, + .init_writer = zephyr_smp_init_writer, + .free_buf = zephyr_smp_free_buf, +}; + +void * +zephyr_smp_alloc_rsp(const void *req, void *arg) +{ + const struct net_buf_pool *pool; + const struct net_buf *req_nb; + struct net_buf *rsp_nb; + + req_nb = req; + + rsp_nb = mcumgr_buf_alloc(); + if (rsp_nb == NULL) { + return NULL; + } + + pool = net_buf_pool_get(req_nb->pool_id); + memcpy(net_buf_user_data(rsp_nb), + net_buf_user_data((void *)req_nb), + sizeof(req_nb->user_data)); + + return rsp_nb; +} + +static void +zephyr_smp_trim_front(void *buf, size_t len, void *arg) +{ + struct net_buf *nb; + + nb = buf; + if (len > nb->len) { + len = nb->len; + } + + net_buf_pull(nb, len); +} + +/** + * Splits an appropriately-sized fragment from the front of a net_buf, as + * neeeded. If the length of the net_buf is greater than specified maximum + * fragment size, a new net_buf is allocated, and data is moved from the source + * net_buf to the new net_buf. If the net_buf is small enough to fit in a + * single fragment, the source net_buf is returned unmodified, and the supplied + * pointer is set to NULL. + * + * This function is expected to be called in a loop until the entire source + * net_buf has been consumed. For example: + * + * struct net_buf *frag; + * struct net_buf *rsp; + * // [...] + * while (rsp != NULL) { + * frag = zephyr_smp_split_frag(&rsp, get_mtu()); + * if (frag == NULL) { + * net_buf_unref(nb); + * return SYS_ENOMEM; + * } + * send_packet(frag) + * } + * + * @param nb The packet to fragment. Upon fragmentation, + * this net_buf is adjusted such that the + * fragment data is removed. If the packet + * constitutes a single fragment, this gets + * set to NULL on success. + * @param mtu The maximum payload size of a fragment. + * + * @return The next fragment to send on success; + * NULL on failure. + */ +static struct net_buf * +zephyr_smp_split_frag(struct net_buf **nb, u16_t mtu) +{ + struct net_buf *frag; + struct net_buf *src; + + src = *nb; + + if (src->len <= mtu) { + *nb = NULL; + frag = src; + } else { + frag = zephyr_smp_alloc_rsp(src, NULL); + + /* Copy fragment payload into new buffer. */ + net_buf_add_mem(frag, src->data, mtu); + + /* Remove fragment from total response. */ + zephyr_smp_trim_front(src, mtu, NULL); + } + + return frag; +} + +static void +zephyr_smp_reset_buf(void *buf, void *arg) +{ + net_buf_reset(buf); +} + +static int +zephyr_smp_write_at(struct cbor_encoder_writer *writer, size_t offset, + const void *data, size_t len, void *arg) +{ + struct cbor_nb_writer *czw; + struct net_buf *nb; + + czw = (struct cbor_nb_writer *)writer; + nb = czw->nb; + + if (offset < 0 || offset > nb->len) { + return MGMT_ERR_EINVAL; + } + + if (len > net_buf_tailroom(nb)) { + return MGMT_ERR_EINVAL; + } + + memcpy(nb->data + offset, data, len); + if (nb->len < offset + len) { + nb->len = offset + len; + writer->bytes_written = nb->len; + } + + return 0; +} + +static int +zephyr_smp_tx_rsp(struct smp_streamer *ns, void *rsp, void *arg) +{ + struct zephyr_smp_transport *zst; + struct net_buf *frag; + struct net_buf *nb; + u16_t mtu; + int rc; + int i; + + zst = arg; + nb = rsp; + + mtu = zst->zst_get_mtu(rsp); + if (mtu == 0) { + /* The transport cannot support a transmission right now. */ + return MGMT_ERR_EUNKNOWN; + } + + i = 0; + while (nb != NULL) { + frag = zephyr_smp_split_frag(&nb, mtu); + if (frag == NULL) { + return MGMT_ERR_ENOMEM; + } + + rc = zst->zst_output(zst, frag); + if (rc != 0) { + return MGMT_ERR_EUNKNOWN; + } + } + + return 0; +} + +static void +zephyr_smp_free_buf(void *buf, void *arg) +{ + mcumgr_buf_free(buf); +} + +static int +zephyr_smp_init_reader(struct cbor_decoder_reader *reader, void *buf, + void *arg) +{ + struct cbor_nb_reader *czr; + + czr = (struct cbor_nb_reader *)reader; + cbor_nb_reader_init(czr, buf); + + return 0; +} + +static int +zephyr_smp_init_writer(struct cbor_encoder_writer *writer, void *buf, + void *arg) +{ + struct cbor_nb_writer *czw; + + czw = (struct cbor_nb_writer *)writer; + cbor_nb_writer_init(czw, buf); + + return 0; +} + +/** + * Processes a single SMP packet and sends the corresponding response(s). + */ +static int +zephyr_smp_process_packet(struct zephyr_smp_transport *zst, + struct net_buf *nb) +{ + struct cbor_nb_reader reader; + struct cbor_nb_writer writer; + struct smp_streamer streamer; + int rc; + + streamer = (struct smp_streamer) { + .mgmt_stmr = { + .cfg = &zephyr_smp_cbor_cfg, + .reader = &reader.r, + .writer = &writer.enc, + .cb_arg = zst, + }, + .tx_rsp_cb = zephyr_smp_tx_rsp, + }; + + rc = smp_process_request_packet(&streamer, nb); + return rc; +} + +/** + * Processes all received SNP request packets. + */ +static void +zephyr_smp_handle_reqs(struct k_work *work) +{ + struct zephyr_smp_transport *zst; + struct net_buf *nb; + + zst = (void *)work; + + while ((nb = k_fifo_get(&zst->zst_fifo, K_NO_WAIT)) != NULL) { + zephyr_smp_process_packet(zst, nb); + } +} + +void +zephyr_smp_transport_init(struct zephyr_smp_transport *zst, + zephyr_smp_transport_out_fn *output_func, + zephyr_smp_transport_get_mtu_fn *get_mtu_func) +{ + *zst = (struct zephyr_smp_transport) { + .zst_output = output_func, + .zst_get_mtu = get_mtu_func, + }; + + k_work_init(&zst->zst_work, zephyr_smp_handle_reqs); + k_fifo_init(&zst->zst_fifo); +} + +void +zephyr_smp_rx_req(struct zephyr_smp_transport *zst, struct net_buf *nb) +{ + k_fifo_put(&zst->zst_fifo, nb); + k_work_submit(&zst->zst_work); +}