1 /*
2 * Copyright (c) 2023 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/net_buf.h>
8 #include <string.h>
9 #include <stdio.h>
10 #include <zephyr/mgmt/mcumgr/smp/smp_client.h>
11 #include <zephyr/mgmt/mcumgr/mgmt/mgmt.h>
12 #include <zephyr/mgmt/mcumgr/grp/os_mgmt/os_mgmt.h>
13 #include <zephyr/mgmt/mcumgr/grp/os_mgmt/os_mgmt_client.h>
14 #include <zephyr/sys/byteorder.h>
15
16 #include <zcbor_common.h>
17 #include <zcbor_encode.h>
18 #include <zcbor_decode.h>
19 #include <mgmt/mcumgr/util/zcbor_bulk.h>
20 #include <mgmt/mcumgr/transport/smp_internal.h>
21 #include "smp_stub.h"
22
23 static const char *echo_ptr;
24
os_stub_init(const char * echo_str)25 void os_stub_init(const char *echo_str)
26 {
27 echo_ptr = echo_str;
28 }
29
os_reset_response(void)30 void os_reset_response(void)
31 {
32 struct net_buf *nb;
33
34 nb = smp_response_buf_allocation();
35 if (nb) {
36 nb->len = 0;
37 }
38 }
39
os_echo_response(int status,struct zcbor_string * echo_data)40 static void os_echo_response(int status, struct zcbor_string *echo_data)
41 {
42 struct net_buf *nb;
43 zcbor_state_t zse[3 + 2];
44 bool ok;
45
46 nb = smp_response_buf_allocation();
47
48 if (!nb) {
49 return;
50 }
51
52 zcbor_new_encode_state(zse, ARRAY_SIZE(zse), nb->data, net_buf_tailroom(nb), 0);
53
54 if (status) {
55 /* Init map start and write image info and data */
56 ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "rc") &&
57 zcbor_int32_put(zse, status) && zcbor_map_end_encode(zse, 2);
58 } else {
59 /* Init map start and write image info and data */
60 ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "r") &&
61 zcbor_tstr_encode_ptr(zse, echo_data->value, echo_data->len) &&
62 zcbor_map_end_encode(zse, 2);
63 }
64
65 if (!ok) {
66 smp_client_response_buf_clean();
67 } else {
68 nb->len = zse->payload - nb->data;
69 }
70 }
71
os_echo_verify(struct net_buf * nb)72 void os_echo_verify(struct net_buf *nb)
73 {
74 /* Parse CBOR data: hash and confirm */
75 zcbor_state_t zsd[3 + 2];
76 int rc;
77 int response_status;
78 struct zcbor_string echo_data;
79 size_t decoded;
80 struct zcbor_map_decode_key_val list_res_decode[] = {
81 ZCBOR_MAP_DECODE_KEY_DECODER("d", zcbor_tstr_decode, &echo_data)
82 };
83
84 zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), nb->data + sizeof(struct smp_hdr), nb->len, 1,
85 NULL, 0);
86 echo_data.len = 0;
87
88 rc = zcbor_map_decode_bulk(zsd, list_res_decode, ARRAY_SIZE(list_res_decode), &decoded);
89 if (rc || !echo_data.len) {
90 printf("Corrupted data %d or no echo data %d\r\n", rc, echo_data.len);
91 response_status = MGMT_ERR_EINVAL;
92 } else if (memcmp(echo_data.value, echo_ptr, echo_data.len)) {
93 response_status = MGMT_ERR_EINVAL;
94 } else {
95 response_status = MGMT_ERR_EOK;
96 }
97
98 os_echo_response(response_status, &echo_data);
99 }
100
