1 /*
2 * Copyright (c) 2018-2021 mcumgr authors
3 * Copyright (c) 2022-2023 Nordic Semiconductor ASA
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 /** SMP - Simple Management Protocol. */
9
10 #include <zephyr/sys/byteorder.h>
11 #include <zephyr/net_buf.h>
12 #include <zephyr/mgmt/mcumgr/mgmt/mgmt.h>
13 #include <zephyr/mgmt/mcumgr/smp/smp.h>
14 #include <zephyr/mgmt/mcumgr/smp/smp_client.h>
15 #include <zephyr/mgmt/mcumgr/transport/smp.h>
16 #include <assert.h>
17 #include <string.h>
18
19 #include <zcbor_common.h>
20 #include <zcbor_decode.h>
21 #include <zcbor_encode.h>
22
23 #include <mgmt/mcumgr/transport/smp_internal.h>
24
25 #ifdef CONFIG_MCUMGR_MGMT_NOTIFICATION_HOOKS
26 #include <zephyr/mgmt/mcumgr/mgmt/callbacks.h>
27 #endif
28
29 #ifdef CONFIG_MCUMGR_SMP_SUPPORT_ORIGINAL_PROTOCOL
30 /*
31 * @brief Translate SMP version 2 error code to legacy SMP version 1 MCUmgr error code.
32 *
33 * @param group #mcumgr_group_t group ID
34 * @param err Group-specific error code
35 *
36 * @return #mcumgr_err_t error code
37 */
smp_translate_error_code(uint16_t group,uint16_t err)38 static int smp_translate_error_code(uint16_t group, uint16_t err)
39 {
40 smp_translate_error_fn translate_error_function = NULL;
41
42 translate_error_function = mgmt_find_error_translation_function(group);
43
44 if (translate_error_function == NULL) {
45 return MGMT_ERR_EUNKNOWN;
46 }
47
48 return translate_error_function(err);
49 }
50 #endif
51
cbor_nb_reader_init(struct cbor_nb_reader * cnr,struct net_buf * nb)52 static void cbor_nb_reader_init(struct cbor_nb_reader *cnr, struct net_buf *nb)
53 {
54 cnr->nb = nb;
55 zcbor_new_decode_state(cnr->zs, ARRAY_SIZE(cnr->zs), nb->data,
56 nb->len, 1, NULL, 0);
57 }
58
cbor_nb_writer_init(struct cbor_nb_writer * cnw,struct net_buf * nb)59 static void cbor_nb_writer_init(struct cbor_nb_writer *cnw, struct net_buf *nb)
60 {
61 net_buf_reset(nb);
62 cnw->nb = nb;
63 cnw->nb->len = sizeof(struct smp_hdr);
64 zcbor_new_encode_state(cnw->zs, ARRAY_SIZE(cnw->zs), nb->data + sizeof(struct smp_hdr),
65 net_buf_tailroom(nb), 0);
66 }
67
68 /**
69 * Converts a request opcode to its corresponding response opcode.
70 */
smp_rsp_op(uint8_t req_op)71 static uint8_t smp_rsp_op(uint8_t req_op)
72 {
73 if (req_op == MGMT_OP_READ) {
74 return MGMT_OP_READ_RSP;
75 } else {
76 return MGMT_OP_WRITE_RSP;
77 }
78 }
79
smp_make_rsp_hdr(const struct smp_hdr * req_hdr,struct smp_hdr * rsp_hdr,size_t len)80 static void smp_make_rsp_hdr(const struct smp_hdr *req_hdr, struct smp_hdr *rsp_hdr, size_t len)
81 {
82 *rsp_hdr = (struct smp_hdr) {
83 .nh_len = sys_cpu_to_be16(len),
84 .nh_flags = 0,
85 .nh_op = smp_rsp_op(req_hdr->nh_op),
86 .nh_group = sys_cpu_to_be16(req_hdr->nh_group),
87 .nh_seq = req_hdr->nh_seq,
88 .nh_id = req_hdr->nh_id,
89 .nh_version = (req_hdr->nh_version > SMP_MCUMGR_VERSION_2 ? SMP_MCUMGR_VERSION_2 :
90 req_hdr->nh_version),
91 };
92 }
93
smp_read_hdr(const struct net_buf * nb,struct smp_hdr * dst_hdr)94 static int smp_read_hdr(const struct net_buf *nb, struct smp_hdr *dst_hdr)
95 {
96 if (nb->len < sizeof(*dst_hdr)) {
97 return MGMT_ERR_EINVAL;
98 }
99
100 memcpy(dst_hdr, nb->data, sizeof(*dst_hdr));
101 dst_hdr->nh_len = sys_be16_to_cpu(dst_hdr->nh_len);
102 dst_hdr->nh_group = sys_be16_to_cpu(dst_hdr->nh_group);
103
104 return 0;
105 }
106
smp_write_hdr(struct smp_streamer * streamer,const struct smp_hdr * src_hdr)107 static inline int smp_write_hdr(struct smp_streamer *streamer, const struct smp_hdr *src_hdr)
108 {
109 memcpy(streamer->writer->nb->data, src_hdr, sizeof(*src_hdr));
110 return 0;
111 }
112
smp_build_err_rsp(struct smp_streamer * streamer,const struct smp_hdr * req_hdr,int status,const char * rc_rsn)113 static int smp_build_err_rsp(struct smp_streamer *streamer, const struct smp_hdr *req_hdr,
114 int status, const char *rc_rsn)
115 {
116 struct smp_hdr rsp_hdr;
117 struct cbor_nb_writer *nbw = streamer->writer;
118 zcbor_state_t *zsp = nbw->zs;
119 bool ok;
120
121 ok = zcbor_map_start_encode(zsp, 2) &&
122 zcbor_tstr_put_lit(zsp, "rc") &&
123 zcbor_int32_put(zsp, status);
124
125 #ifdef CONFIG_MCUMGR_SMP_VERBOSE_ERR_RESPONSE
126 if (ok && rc_rsn != NULL) {
127 ok = zcbor_tstr_put_lit(zsp, "rsn") &&
128 zcbor_tstr_put_term(zsp, rc_rsn, CONFIG_ZCBOR_MAX_STR_LEN);
129 }
130 #else
131 ARG_UNUSED(rc_rsn);
132 #endif
133 ok &= zcbor_map_end_encode(zsp, 2);
134
135 if (!ok) {
136 return MGMT_ERR_EMSGSIZE;
137 }
138
139 smp_make_rsp_hdr(req_hdr, &rsp_hdr,
140 zsp->payload_mut - nbw->nb->data - MGMT_HDR_SIZE);
141 nbw->nb->len = zsp->payload_mut - nbw->nb->data;
142 smp_write_hdr(streamer, &rsp_hdr);
143
144 return 0;
145 }
146
147 /**
148 * Processes a single SMP request and generates a response payload (i.e.,
149 * everything after the management header). On success, the response payload
150 * is written to the supplied cbuf but not transmitted. On failure, no error
151 * response gets written; the caller is expected to build an error response
152 * from the return code.
153 *
154 * @param cbuf A cbuf containing the request and response buffer.
155 * @param req_hdr The management header belonging to the incoming request (host-byte order).
156 *
157 * @return A MGMT_ERR_[...] error code.
158 */
smp_handle_single_payload(struct smp_streamer * cbuf,const struct smp_hdr * req_hdr)159 static int smp_handle_single_payload(struct smp_streamer *cbuf, const struct smp_hdr *req_hdr)
160 {
161 const struct mgmt_group *group;
162 const struct mgmt_handler *handler;
163 mgmt_handler_fn handler_fn;
164 int rc;
165 #if defined(CONFIG_MCUMGR_SMP_COMMAND_STATUS_HOOKS)
166 enum mgmt_cb_return status;
167 struct mgmt_evt_op_cmd_arg cmd_recv;
168 int32_t err_rc;
169 uint16_t err_group;
170 #endif
171
172 group = mgmt_find_group(req_hdr->nh_group);
173 if (group == NULL) {
174 return MGMT_ERR_ENOTSUP;
175 }
176
177 handler = mgmt_get_handler(group, req_hdr->nh_id);
178 if (handler == NULL) {
179 return MGMT_ERR_ENOTSUP;
180 }
181
182 switch (req_hdr->nh_op) {
183 case MGMT_OP_READ:
184 handler_fn = handler->mh_read;
185 break;
186
187 case MGMT_OP_WRITE:
188 handler_fn = handler->mh_write;
189 break;
190
191 default:
192 return MGMT_ERR_EINVAL;
193 }
194
195 if (handler_fn) {
196 bool ok;
197
198 #if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
199 if (!group->custom_payload) {
200 #endif
201 ok = zcbor_map_start_encode(cbuf->writer->zs,
202 CONFIG_MCUMGR_SMP_CBOR_MAX_MAIN_MAP_ENTRIES);
203
204 MGMT_CTXT_SET_RC_RSN(cbuf, NULL);
205
206 if (!ok) {
207 return MGMT_ERR_EMSGSIZE;
208 }
209 #if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
210 }
211 #endif
212
213 #if defined(CONFIG_MCUMGR_SMP_COMMAND_STATUS_HOOKS)
214 cmd_recv.group = req_hdr->nh_group;
215 cmd_recv.id = req_hdr->nh_id;
216 cmd_recv.op = req_hdr->nh_op;
217
218 /* Send request to application to check if handler should run or not. */
219 status = mgmt_callback_notify(MGMT_EVT_OP_CMD_RECV, &cmd_recv, sizeof(cmd_recv),
220 &err_rc, &err_group);
221
222 /* Skip running the command if a handler reported an error and return that
223 * instead.
224 */
225 if (status != MGMT_CB_OK) {
226 if (status == MGMT_CB_ERROR_RC) {
227 rc = err_rc;
228 } else {
229 ok = smp_add_cmd_err(cbuf->writer->zs, err_group,
230 (uint16_t)err_rc);
231
232 rc = (ok ? MGMT_ERR_EOK : MGMT_ERR_EMSGSIZE);
233 }
234
235 goto end;
236 }
237 #endif
238
239 rc = handler_fn(cbuf);
240
241 #if defined(CONFIG_MCUMGR_SMP_COMMAND_STATUS_HOOKS)
242 end:
243 #endif
244 #if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
245 if (!group->custom_payload) {
246 #endif
247 /* End response payload. */
248 if (!zcbor_map_end_encode(cbuf->writer->zs,
249 CONFIG_MCUMGR_SMP_CBOR_MAX_MAIN_MAP_ENTRIES) &&
250 rc == 0) {
251 rc = MGMT_ERR_EMSGSIZE;
252 }
253 #if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
254 }
255 #endif
256 } else {
257 rc = MGMT_ERR_ENOTSUP;
258 }
259
260 return rc;
261 }
262
263 /**
264 * Processes a single SMP request and generates a complete response (i.e.,
265 * header and payload). On success, the response is written using the supplied
266 * streamer but not transmitted. On failure, no error response gets written;
267 * the caller is expected to build an error response from the return code.
268 *
269 * @param streamer The SMP streamer to use for reading the request and writing the response.
270 * @param req_hdr The management header belonging to the incoming request (host-byte order).
271 *
272 * @return A MGMT_ERR_[...] error code.
273 */
smp_handle_single_req(struct smp_streamer * streamer,const struct smp_hdr * req_hdr,const char ** rsn)274 static int smp_handle_single_req(struct smp_streamer *streamer, const struct smp_hdr *req_hdr,
275 const char **rsn)
276 {
277 struct smp_hdr rsp_hdr;
278 struct cbor_nb_writer *nbw = streamer->writer;
279 zcbor_state_t *zsp = nbw->zs;
280 int rc;
281
282 #ifdef CONFIG_MCUMGR_SMP_SUPPORT_ORIGINAL_PROTOCOL
283 nbw->error_group = 0;
284 nbw->error_ret = 0;
285 #else
286 if (req_hdr->nh_version == SMP_MCUMGR_VERSION_1) {
287 /* Support for the original version is excluded in this build */
288 return MGMT_ERR_UNSUPPORTED_TOO_OLD;
289 }
290 #endif
291
292 /* We do not currently support future versions of the protocol */
293 if (req_hdr->nh_version > SMP_MCUMGR_VERSION_2) {
294 return MGMT_ERR_UNSUPPORTED_TOO_NEW;
295 }
296
297 /* Process the request and write the response payload. */
298 rc = smp_handle_single_payload(streamer, req_hdr);
299 if (rc != 0) {
300 *rsn = MGMT_CTXT_RC_RSN(streamer);
301 return rc;
302 }
303
304 #ifdef CONFIG_MCUMGR_SMP_SUPPORT_ORIGINAL_PROTOCOL
305 /* If using the legacy protocol, translate the error code to a return code */
306 if (nbw->error_ret != 0 && req_hdr->nh_version == 0) {
307 rc = smp_translate_error_code(nbw->error_group, nbw->error_ret);
308 *rsn = MGMT_CTXT_RC_RSN(streamer);
309 return rc;
310 }
311 #endif
312
313 #if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
314 if (!mgmt_find_group(req_hdr->nh_group)->custom_payload) {
315 #endif
316 nbw->nb->len = zsp->payload_mut - nbw->nb->data;
317 #if defined(CONFIG_MCUMGR_MGMT_CUSTOM_PAYLOAD)
318 }
319 #endif
320
321 smp_make_rsp_hdr(req_hdr, &rsp_hdr, (nbw->nb->len - MGMT_HDR_SIZE));
322 smp_write_hdr(streamer, &rsp_hdr);
323
324 return 0;
325 }
326
327 /**
328 * Attempts to transmit an SMP error response. This function consumes both
329 * supplied buffers.
330 *
331 * @param streamer The SMP streamer for building and transmitting the response.
332 * @param req_hdr The header of the request which elicited the error.
333 * @param req The buffer holding the request.
334 * @param rsp The buffer holding the response, or NULL if none was allocated.
335 * @param status The status to indicate in the error response.
336 * @param rsn The text explanation to @status encoded as "rsn" into CBOR
337 * response.
338 */
smp_on_err(struct smp_streamer * streamer,const struct smp_hdr * req_hdr,void * req,void * rsp,int status,const char * rsn)339 static void smp_on_err(struct smp_streamer *streamer, const struct smp_hdr *req_hdr,
340 void *req, void *rsp, int status, const char *rsn)
341 {
342 int rc;
343
344 /* Prefer the response buffer for holding the error response. If no
345 * response buffer was allocated, use the request buffer instead.
346 */
347 if (rsp == NULL) {
348 rsp = req;
349 req = NULL;
350 }
351
352 /* Clear the partial response from the buffer, if any. */
353 cbor_nb_writer_init(streamer->writer, rsp);
354
355 /* Build and transmit the error response. */
356 rc = smp_build_err_rsp(streamer, req_hdr, status, rsn);
357 if (rc == 0) {
358 streamer->smpt->functions.output(rsp);
359 rsp = NULL;
360 }
361
362 /* Free any extra buffers. */
363 smp_free_buf(req, streamer->smpt);
364 smp_free_buf(rsp, streamer->smpt);
365 }
366
367 /**
368 * Processes all SMP requests in an incoming packet. Requests are processed
369 * sequentially from the start of the packet to the end. Each response is sent
370 * individually in its own packet. If a request elicits an error response,
371 * processing of the packet is aborted. This function consumes the supplied
372 * request buffer regardless of the outcome.
373 * The function will return MGMT_ERR_EOK (0) when given an empty input stream,
374 * and will also release the buffer from the stream; it does not return
375 * MTMT_ERR_ECORRUPT, or any other MGMT error, because there was no error while
376 * processing of the input stream, it is callers fault that an empty stream has
377 * been passed to the function.
378 *
379 * @param streamer The streamer to use for reading, writing, and transmitting.
380 * @param req A buffer containing the request packet.
381 *
382 * @return 0 on success or when input stream is empty;
383 * MGMT_ERR_ECORRUPT if buffer starts with non SMP data header or there
384 * is not enough bytes to process header, or other MGMT_ERR_[...] code on
385 * failure.
386 */
smp_process_request_packet(struct smp_streamer * streamer,void * vreq)387 int smp_process_request_packet(struct smp_streamer *streamer, void *vreq)
388 {
389 struct smp_hdr req_hdr = { 0 };
390 void *rsp;
391 struct net_buf *req = vreq;
392 bool valid_hdr = false;
393 bool handler_found = false;
394 int rc = 0;
395 const char *rsn = NULL;
396
397 #if defined(CONFIG_MCUMGR_SMP_COMMAND_STATUS_HOOKS)
398 struct mgmt_evt_op_cmd_arg cmd_done_arg;
399 int32_t err_rc;
400 uint16_t err_group;
401 #endif
402
403 rsp = NULL;
404
405 while (req->len > 0) {
406 handler_found = false;
407 valid_hdr = false;
408
409 /* Read the management header and strip it from the request. */
410 rc = smp_read_hdr(req, &req_hdr);
411 if (rc != 0) {
412 rc = MGMT_ERR_ECORRUPT;
413 break;
414 }
415
416 valid_hdr = true;
417 /* Skip the smp_hdr */
418 net_buf_pull(req, sizeof(struct smp_hdr));
419 /* Does buffer contain whole message? */
420 if (req->len < req_hdr.nh_len) {
421 rc = MGMT_ERR_ECORRUPT;
422 break;
423 }
424
425 if (req_hdr.nh_op == MGMT_OP_READ || req_hdr.nh_op == MGMT_OP_WRITE) {
426 rsp = smp_alloc_rsp(req, streamer->smpt);
427 if (rsp == NULL) {
428 rc = MGMT_ERR_ENOMEM;
429 break;
430 }
431
432 cbor_nb_reader_init(streamer->reader, req);
433 cbor_nb_writer_init(streamer->writer, rsp);
434
435 /* Process the request payload and build the response. */
436 rc = smp_handle_single_req(streamer, &req_hdr, &rsn);
437 handler_found = (rc != MGMT_ERR_ENOTSUP);
438 if (rc != 0) {
439 break;
440 }
441
442 /* Send the response. */
443 rc = streamer->smpt->functions.output(rsp);
444 rsp = NULL;
445 } else if (IS_ENABLED(CONFIG_SMP_CLIENT) && (req_hdr.nh_op == MGMT_OP_READ_RSP ||
446 req_hdr.nh_op == MGMT_OP_WRITE_RSP)) {
447 rc = smp_client_single_response(req, &req_hdr);
448
449 if (rc == MGMT_ERR_EOK) {
450 handler_found = true;
451 } else {
452 /* Server shuold not send error response for response */
453 valid_hdr = false;
454 }
455
456 } else {
457 rc = MGMT_ERR_ENOTSUP;
458 }
459
460 if (rc != 0) {
461 break;
462 }
463 /* Trim processed request to free up space for subsequent responses. */
464 net_buf_pull(req, req_hdr.nh_len);
465
466 #if defined(CONFIG_MCUMGR_SMP_COMMAND_STATUS_HOOKS)
467 cmd_done_arg.group = req_hdr.nh_group;
468 cmd_done_arg.id = req_hdr.nh_id;
469 cmd_done_arg.err = MGMT_ERR_EOK;
470
471 (void)mgmt_callback_notify(MGMT_EVT_OP_CMD_DONE, &cmd_done_arg,
472 sizeof(cmd_done_arg), &err_rc, &err_group);
473 #endif
474 }
475
476 if (rc != 0 && valid_hdr) {
477 smp_on_err(streamer, &req_hdr, req, rsp, rc, rsn);
478
479 if (handler_found) {
480 #if defined(CONFIG_MCUMGR_SMP_COMMAND_STATUS_HOOKS)
481 cmd_done_arg.group = req_hdr.nh_group;
482 cmd_done_arg.id = req_hdr.nh_id;
483 cmd_done_arg.err = rc;
484
485 (void)mgmt_callback_notify(MGMT_EVT_OP_CMD_DONE, &cmd_done_arg,
486 sizeof(cmd_done_arg), &err_rc, &err_group);
487 #endif
488 }
489
490 return rc;
491 }
492
493 smp_free_buf(req, streamer->smpt);
494 smp_free_buf(rsp, streamer->smpt);
495
496 return rc;
497 }
498
smp_add_cmd_err(zcbor_state_t * zse,uint16_t group,uint16_t ret)499 bool smp_add_cmd_err(zcbor_state_t *zse, uint16_t group, uint16_t ret)
500 {
501 bool ok = true;
502
503 if (ret != 0) {
504 #ifdef CONFIG_MCUMGR_SMP_SUPPORT_ORIGINAL_PROTOCOL
505 struct cbor_nb_writer *container = CONTAINER_OF(zse, struct cbor_nb_writer, zs[0]);
506
507 container->error_group = group;
508 container->error_ret = ret;
509 #endif
510
511 ok = zcbor_tstr_put_lit(zse, "err") &&
512 zcbor_map_start_encode(zse, 2) &&
513 zcbor_tstr_put_lit(zse, "group") &&
514 zcbor_uint32_put(zse, (uint32_t)group) &&
515 zcbor_tstr_put_lit(zse, "rc") &&
516 zcbor_uint32_put(zse, (uint32_t)ret) &&
517 zcbor_map_end_encode(zse, 2);
518 }
519
520 return ok;
521 }
522
