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/img_mgmt/img_mgmt_client.h>
13 #include <zephyr/sys/byteorder.h>
14
15 #include <zcbor_common.h>
16 #include <zcbor_encode.h>
17 #include <zcbor_decode.h>
18 #include <mgmt/mcumgr/util/zcbor_bulk.h>
19 #include <mgmt/mcumgr/transport/smp_internal.h>
20 #include "smp_stub.h"
21 #include "img_gr_stub.h"
22
23 static struct mcumgr_image_data image_dummy_info[2];
24 static size_t test_offset;
25 static uint8_t *image_hash_ptr;
26
27 #ifdef CONFIG_MCUMGR_GRP_IMG_UPDATABLE_IMAGE_NUMBER
28 #define IMG_UPDATABLE_IMAGE_COUNT CONFIG_MCUMGR_GRP_IMG_UPDATABLE_IMAGE_NUMBER
29 #else
30 #define IMG_UPDATABLE_IMAGE_COUNT 1
31 #endif
32
33 #define ZCBOR_ENCODE_FLAG(zse, label, value) \
34 (zcbor_tstr_put_lit(zse, label) && zcbor_bool_put(zse, value))
35
img_upload_stub_init(void)36 void img_upload_stub_init(void)
37 {
38 test_offset = 0;
39 }
40
img_upload_response(size_t offset,int status)41 void img_upload_response(size_t offset, int status)
42 {
43 struct net_buf *nb;
44 zcbor_state_t zse[CONFIG_MCUMGR_SMP_CBOR_MAX_DECODING_LEVELS + 2];
45 bool ok;
46
47 nb = smp_response_buf_allocation();
48
49 if (!nb) {
50 return;
51 }
52
53 zcbor_new_encode_state(zse, ARRAY_SIZE(zse), nb->data, net_buf_tailroom(nb), 0);
54
55 /* Init map start and write image info and data */
56 if (status) {
57 ok = zcbor_map_start_encode(zse, 4) && zcbor_tstr_put_lit(zse, "rc") &&
58 zcbor_int32_put(zse, status) && zcbor_tstr_put_lit(zse, "off") &&
59 zcbor_size_put(zse, offset) && zcbor_map_end_encode(zse, 4);
60 } else {
61 /* Drop Status away from response */
62 ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "off") &&
63 zcbor_size_put(zse, offset) && zcbor_map_end_encode(zse, 4);
64 }
65
66
67 if (!ok) {
68 smp_client_response_buf_clean();
69 } else {
70 nb->len = zse->payload - nb->data;
71 }
72 }
73
img_fail_response(int status)74 void img_fail_response(int status)
75 {
76 struct net_buf *nb;
77 zcbor_state_t zse[CONFIG_MCUMGR_SMP_CBOR_MAX_DECODING_LEVELS + 2];
78 bool ok;
79
80 nb = smp_response_buf_allocation();
81
82 if (!nb) {
83 return;
84 }
85
86 zcbor_new_encode_state(zse, ARRAY_SIZE(zse), nb->data, net_buf_tailroom(nb), 0);
87
88 /* Init map start and write image info and data */
89 ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "rc") &&
90 zcbor_int32_put(zse, status) && zcbor_map_end_encode(zse, 2);
91 if (!ok) {
92 smp_client_response_buf_clean();
93 } else {
94 nb->len = zse->payload - nb->data;
95 }
96 }
97
img_read_response(int count)98 void img_read_response(int count)
99 {
100 struct net_buf *nb;
101 zcbor_state_t zse[CONFIG_MCUMGR_SMP_CBOR_MAX_DECODING_LEVELS + 2];
102 bool ok;
103
104 nb = smp_response_buf_allocation();
105
106 if (!nb) {
107 return;
108 }
109
110 zcbor_new_encode_state(zse, ARRAY_SIZE(zse), nb->data, net_buf_tailroom(nb), 0);
111
112 ok = zcbor_map_start_encode(zse, 15) && zcbor_tstr_put_lit(zse, "images") &&
113 zcbor_list_start_encode(zse, 2);
114
115 for (int i = 0; ok && i < count; i++) {
116
117 ok = zcbor_map_start_encode(zse, 15) &&
118 ((zcbor_tstr_put_lit(zse, "image") &&
119 zcbor_uint32_put(zse, image_dummy_info[i].img_num))) &&
120 zcbor_tstr_put_lit(zse, "slot") &&
121 zcbor_uint32_put(zse, image_dummy_info[i].slot_num) &&
122 zcbor_tstr_put_lit(zse, "version") &&
123 zcbor_tstr_put_term(zse, image_dummy_info[i].version,
124 sizeof(image_dummy_info[i].version)) &&
125
126 zcbor_tstr_put_lit(zse, "hash") &&
127 zcbor_bstr_encode_ptr(zse, image_dummy_info[i].hash, IMG_MGMT_DATA_SHA_LEN) &&
128 ZCBOR_ENCODE_FLAG(zse, "bootable", image_dummy_info[i].flags.bootable) &&
129 ZCBOR_ENCODE_FLAG(zse, "pending", image_dummy_info[i].flags.pending) &&
130 ZCBOR_ENCODE_FLAG(zse, "confirmed", image_dummy_info[i].flags.confirmed) &&
131 ZCBOR_ENCODE_FLAG(zse, "active", image_dummy_info[i].flags.active) &&
132 ZCBOR_ENCODE_FLAG(zse, "permanent", image_dummy_info[i].flags.permanent) &&
133 zcbor_map_end_encode(zse, 15);
134 }
135
136 ok = ok && zcbor_list_end_encode(zse, 2 * IMG_UPDATABLE_IMAGE_COUNT);
137
138 ok = ok && zcbor_map_end_encode(zse, 15);
139
140 if (!ok) {
141 smp_client_response_buf_clean();
142 } else {
143 nb->len = zse->payload - nb->data;
144 }
145 }
146
img_erase_response(int status)147 void img_erase_response(int status)
148 {
149 struct net_buf *nb;
150 zcbor_state_t zse[CONFIG_MCUMGR_SMP_CBOR_MAX_DECODING_LEVELS + 2];
151 bool ok;
152
153 nb = smp_response_buf_allocation();
154
155 if (!nb) {
156 return;
157 }
158
159 zcbor_new_encode_state(zse, ARRAY_SIZE(zse), nb->data, net_buf_tailroom(nb), 0);
160
161 /* Init map start and write image info and data */
162 ok = zcbor_map_start_encode(zse, 2) && zcbor_tstr_put_lit(zse, "rc") &&
163 zcbor_int32_put(zse, status) && zcbor_map_end_encode(zse, 2);
164 if (!ok) {
165 smp_client_response_buf_clean();
166 } else {
167 nb->len = zse->payload - nb->data;
168 }
169 }
170
img_state_write_verify(struct net_buf * nb)171 void img_state_write_verify(struct net_buf *nb)
172 {
173 /* Parse CBOR data: hash and confirm */
174 zcbor_state_t zsd[CONFIG_MCUMGR_SMP_CBOR_MAX_DECODING_LEVELS + 2];
175 int rc;
176 struct zcbor_string hash;
177 bool confirm;
178 size_t decoded;
179 struct zcbor_map_decode_key_val list_res_decode[] = {
180 ZCBOR_MAP_DECODE_KEY_DECODER("confirm", zcbor_bool_decode, &confirm),
181 ZCBOR_MAP_DECODE_KEY_DECODER("hash", zcbor_bstr_decode, &hash)
182 };
183
184 zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), nb->data + sizeof(struct smp_hdr), nb->len, 1,
185 NULL, 0);
186
187 decoded = 0;
188 /* Init buffer values */
189 confirm = false;
190 hash.len = 0;
191
192 rc = zcbor_map_decode_bulk(zsd, list_res_decode, ARRAY_SIZE(list_res_decode), &decoded);
193 if (rc) {
194 printf("Corrupted data %d\r\n", rc);
195 img_fail_response(MGMT_ERR_EINVAL);
196 return;
197 }
198 if (hash.len) {
199 printf("HASH %d", hash.len);
200 if (memcmp(hash.value, image_dummy_info[1].hash, 32) == 0) {
201 if (confirm) {
202 /* Set Permanent bit */
203 image_dummy_info[1].flags.permanent = true;
204 } else {
205 /* Set pending */
206 image_dummy_info[1].flags.pending = true;
207 }
208 img_read_response(2);
209 } else {
210 img_fail_response(MGMT_ERR_EINVAL);
211 }
212 } else {
213 if (confirm) {
214 image_dummy_info[0].flags.confirmed = true;
215 }
216 img_read_response(2);
217 }
218 }
219
img_upload_init_verify(struct net_buf * nb)220 void img_upload_init_verify(struct net_buf *nb)
221 {
222 /* Parse CBOR data: hash and confirm */
223 zcbor_state_t zsd[CONFIG_MCUMGR_SMP_CBOR_MAX_DECODING_LEVELS + 2];
224 int rc;
225 uint32_t image;
226 struct zcbor_string sha, data;
227 size_t decoded, length, offset;
228 struct zcbor_map_decode_key_val list_res_decode[] = {
229 ZCBOR_MAP_DECODE_KEY_DECODER("image", zcbor_uint32_decode, &image),
230 ZCBOR_MAP_DECODE_KEY_DECODER("data", zcbor_bstr_decode, &data),
231 ZCBOR_MAP_DECODE_KEY_DECODER("len", zcbor_size_decode, &length),
232 ZCBOR_MAP_DECODE_KEY_DECODER("off", zcbor_size_decode, &offset),
233 ZCBOR_MAP_DECODE_KEY_DECODER("sha", zcbor_bstr_decode, &sha)
234 };
235
236 zcbor_new_decode_state(zsd, ARRAY_SIZE(zsd), nb->data + sizeof(struct smp_hdr), nb->len, 1,
237 NULL, 0);
238
239 decoded = 0;
240 /* Init buffer values */
241 sha.len = 0;
242 data.len = 0;
243 length = SIZE_MAX;
244 offset = SIZE_MAX;
245 image = UINT32_MAX;
246
247 rc = zcbor_map_decode_bulk(zsd, list_res_decode, ARRAY_SIZE(list_res_decode), &decoded);
248 if (rc || data.len == 0 || offset == SIZE_MAX || image != TEST_IMAGE_NUM) {
249 printf("Corrupted data %d or %d data len\r\n", rc, data.len);
250 img_upload_response(0, MGMT_ERR_EINVAL);
251 return;
252 }
253
254 if (sha.len) {
255 if (memcmp(sha.value, image_hash_ptr, 32)) {
256 printf("Hash not same\r\n");
257 img_upload_response(0, MGMT_ERR_EINVAL);
258 return;
259 }
260 }
261
262 if (offset != test_offset) {
263 printf("Offset not exepected %d vs received %d\r\n", test_offset, offset);
264 }
265
266 if (offset == 0) {
267 if (length != TEST_IMAGE_SIZE) {
268 img_upload_response(0, MGMT_ERR_EINVAL);
269 }
270 }
271
272 test_offset += data.len;
273 printf("Upload offset %d\r\n", test_offset);
274 if (test_offset <= TEST_IMAGE_SIZE) {
275 img_upload_response(test_offset, MGMT_ERR_EOK);
276 } else {
277 img_upload_response(0, MGMT_ERR_EINVAL);
278 }
279 }
280
img_gr_stub_data_init(uint8_t * hash_ptr)281 void img_gr_stub_data_init(uint8_t *hash_ptr)
282 {
283 image_hash_ptr = hash_ptr;
284 for (int i = 0; i < 32; i++) {
285 image_hash_ptr[i] = i;
286 image_dummy_info[0].hash[i] = i + 32;
287 image_dummy_info[1].hash[i] = i + 64;
288 }
289 /* Set dummy image list data */
290 for (int i = 0; i < 2; i++) {
291 image_dummy_info[i].img_num = i;
292 image_dummy_info[i].slot_num = i;
293 /* Write version */
294 snprintf(image_dummy_info[i].version, IMG_MGMT_VER_MAX_STR_LEN, "1.1.%u", i);
295 image_dummy_info[i].version[sizeof(image_dummy_info[i].version) - 1] = '\0';
296 image_dummy_info[i].flags.bootable = true;
297 image_dummy_info[i].flags.pending = false;
298 if (i) {
299 image_dummy_info[i].flags.confirmed = false;
300 image_dummy_info[i].flags.active = false;
301 } else {
302 image_dummy_info[i].flags.confirmed = true;
303 image_dummy_info[i].flags.active = true;
304 }
305 image_dummy_info[i].flags.permanent = false;
306 }
307 }
308
