1 /*
2 * Copyright (c) 2023 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /** SMP - Simple Management Client Protocol. */
8
9 #include <string.h>
10 #include <zcbor_common.h>
11 #include <zcbor_decode.h>
12 #include <zcbor_encode.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/init.h>
15 #include <zephyr/sys/byteorder.h>
16 #include <zephyr/net_buf.h>
17
18 #include <zephyr/mgmt/mcumgr/mgmt/mgmt.h>
19 #include <zephyr/mgmt/mcumgr/smp/smp.h>
20 #include <zephyr/mgmt/mcumgr/smp/smp_client.h>
21 #include <zephyr/mgmt/mcumgr/transport/smp.h>
22 #include <mgmt/mcumgr/transport/smp_internal.h>
23
24 #include <zephyr/logging/log.h>
25 LOG_MODULE_REGISTER(mcumgr_smp_client, CONFIG_MCUMGR_SMP_CLIENT_LOG_LEVEL);
26
27 struct smp_client_cmd_req {
28 sys_snode_t node;
29 struct net_buf *nb;
30 struct smp_client_object *smp_client;
31 void *user_data;
32 smp_client_res_fn cb;
33 int64_t timestamp;
34 int retry_cnt;
35 };
36
37 struct smp_client_data_base {
38 struct k_work_delayable work_delay;
39 sys_slist_t cmd_free_list;
40 sys_slist_t cmd_list;
41 };
42
43 static struct smp_client_cmd_req smp_cmd_req_bufs[CONFIG_SMP_CLIENT_CMD_MAX];
44 static struct smp_client_data_base smp_client_data;
45
46 static void smp_read_hdr(const struct net_buf *nb, struct smp_hdr *dst_hdr);
47 static void smp_client_cmd_req_free(struct smp_client_cmd_req *cmd_req);
48
49 /**
50 * Send all SMP client request packets.
51 */
smp_client_handle_reqs(struct k_work * work)52 static void smp_client_handle_reqs(struct k_work *work)
53 {
54 struct smp_client_object *smp_client;
55 struct smp_transport *smpt;
56 struct net_buf *nb;
57
58 smp_client = (void *)work;
59 smpt = smp_client->smpt;
60
61 while ((nb = k_fifo_get(&smp_client->tx_fifo, K_NO_WAIT)) != NULL) {
62 smpt->functions.output(nb);
63 }
64 }
65
smp_header_init(struct smp_hdr * header,uint16_t group,uint8_t id,uint8_t op,uint16_t payload_len,uint8_t seq,enum smp_mcumgr_version_t version)66 static void smp_header_init(struct smp_hdr *header, uint16_t group, uint8_t id, uint8_t op,
67 uint16_t payload_len, uint8_t seq, enum smp_mcumgr_version_t version)
68 {
69 /* Pre config SMP header structure */
70 memset(header, 0, sizeof(struct smp_hdr));
71 header->nh_version = version;
72 header->nh_op = op;
73 header->nh_len = sys_cpu_to_be16(payload_len);
74 header->nh_group = sys_cpu_to_be16(group);
75 header->nh_id = id;
76 header->nh_seq = seq;
77 }
78
smp_client_transport_work_fn(struct k_work * work)79 static void smp_client_transport_work_fn(struct k_work *work)
80 {
81 struct smp_client_cmd_req *entry, *tmp;
82 smp_client_res_fn cb;
83 void *user_data;
84 int backoff_ms = CONFIG_SMP_CMD_RETRY_TIME;
85 int64_t time_stamp_cmp;
86 int64_t time_stamp_ref;
87 int64_t time_stamp_delta;
88
89 ARG_UNUSED(work);
90
91 if (sys_slist_is_empty(&smp_client_data.cmd_list)) {
92 /* No more packet for Transport */
93 return;
94 }
95
96 SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&smp_client_data.cmd_list, entry, tmp, node) {
97 time_stamp_ref = entry->timestamp;
98 /* Check Time delta and get current time to reference */
99 time_stamp_delta = k_uptime_delta(&time_stamp_ref);
100
101 if (time_stamp_delta < 0) {
102 time_stamp_cmp = entry->timestamp - time_stamp_ref;
103 if (time_stamp_cmp < CONFIG_SMP_CMD_RETRY_TIME &&
104 time_stamp_cmp < backoff_ms) {
105 /* Update new shorter shedule */
106 backoff_ms = time_stamp_cmp;
107 }
108 continue;
109 } else if (entry->retry_cnt) {
110 /* Increment reference for re-transmission */
111 entry->nb = net_buf_ref(entry->nb);
112 entry->retry_cnt--;
113 entry->timestamp = time_stamp_ref + CONFIG_SMP_CMD_RETRY_TIME;
114 k_fifo_put(&entry->smp_client->tx_fifo, entry->nb);
115 k_work_submit_to_queue(smp_get_wq(), &entry->smp_client->work);
116 continue;
117 }
118
119 cb = entry->cb;
120 user_data = entry->user_data;
121 smp_client_cmd_req_free(entry);
122 if (cb) {
123 cb(NULL, user_data);
124 }
125 }
126
127 if (!sys_slist_is_empty(&smp_client_data.cmd_list)) {
128 /* Re-schedule new timeout to next */
129 k_work_reschedule_for_queue(smp_get_wq(), &smp_client_data.work_delay,
130 K_MSEC(backoff_ms));
131 }
132 }
133
smp_client_init(void)134 static int smp_client_init(void)
135 {
136 k_work_init_delayable(&smp_client_data.work_delay, smp_client_transport_work_fn);
137 sys_slist_init(&smp_client_data.cmd_list);
138 sys_slist_init(&smp_client_data.cmd_free_list);
139 for (int i = 0; i < CONFIG_SMP_CLIENT_CMD_MAX; i++) {
140 sys_slist_append(&smp_client_data.cmd_free_list, &smp_cmd_req_bufs[i].node);
141 }
142 return 0;
143 }
144
smp_client_cmd_req_allocate(void)145 static struct smp_client_cmd_req *smp_client_cmd_req_allocate(void)
146 {
147 sys_snode_t *cmd_node;
148 struct smp_client_cmd_req *req;
149
150 cmd_node = sys_slist_get(&smp_client_data.cmd_free_list);
151 if (!cmd_node) {
152 return NULL;
153 }
154
155 req = SYS_SLIST_CONTAINER(cmd_node, req, node);
156
157 return req;
158 }
159
smp_cmd_add_to_list(struct smp_client_cmd_req * cmd_req)160 static void smp_cmd_add_to_list(struct smp_client_cmd_req *cmd_req)
161 {
162 if (sys_slist_is_empty(&smp_client_data.cmd_list)) {
163 /* Enable timer */
164 k_work_reschedule_for_queue(smp_get_wq(), &smp_client_data.work_delay,
165 K_MSEC(CONFIG_SMP_CMD_RETRY_TIME));
166 }
167 sys_slist_append(&smp_client_data.cmd_list, &cmd_req->node);
168 }
169
smp_client_cmd_req_free(struct smp_client_cmd_req * cmd_req)170 static void smp_client_cmd_req_free(struct smp_client_cmd_req *cmd_req)
171 {
172 smp_client_buf_free(cmd_req->nb);
173 cmd_req->nb = NULL;
174 sys_slist_find_and_remove(&smp_client_data.cmd_list, &cmd_req->node);
175 /* Add to free list */
176 sys_slist_append(&smp_client_data.cmd_free_list, &cmd_req->node);
177
178 if (sys_slist_is_empty(&smp_client_data.cmd_list)) {
179 /* cancel delay */
180 k_work_cancel_delayable(&smp_client_data.work_delay);
181 }
182 }
183
smp_client_response_discover(const struct smp_hdr * res_hdr)184 static struct smp_client_cmd_req *smp_client_response_discover(const struct smp_hdr *res_hdr)
185 {
186 struct smp_hdr smp_header;
187 enum mcumgr_op_t response;
188 struct smp_client_cmd_req *cmd_req;
189
190 if (sys_slist_is_empty(&smp_client_data.cmd_list)) {
191 return NULL;
192 }
193
194 SYS_SLIST_FOR_EACH_CONTAINER(&smp_client_data.cmd_list, cmd_req, node) {
195 smp_read_hdr(cmd_req->nb, &smp_header);
196 if (smp_header.nh_op == MGMT_OP_READ) {
197 response = MGMT_OP_READ_RSP;
198 } else {
199 response = MGMT_OP_WRITE_RSP;
200 }
201
202 if (smp_header.nh_seq != res_hdr->nh_seq) {
203 continue;
204 } else if (res_hdr->nh_op != response) {
205 continue;
206 }
207
208 return cmd_req;
209 }
210 return NULL;
211 }
212
smp_client_object_init(struct smp_client_object * smp_client,int smp_type)213 int smp_client_object_init(struct smp_client_object *smp_client, int smp_type)
214 {
215 smp_client->smpt = smp_client_transport_get(smp_type);
216 if (!smp_client->smpt) {
217 return MGMT_ERR_EINVAL;
218 }
219
220 /* Init TX FIFO */
221 k_work_init(&smp_client->work, smp_client_handle_reqs);
222 k_fifo_init(&smp_client->tx_fifo);
223
224 return MGMT_ERR_EOK;
225 }
226
smp_client_single_response(struct net_buf * nb,const struct smp_hdr * res_hdr)227 int smp_client_single_response(struct net_buf *nb, const struct smp_hdr *res_hdr)
228 {
229 struct smp_client_cmd_req *cmd_req;
230 smp_client_res_fn cb;
231 void *user_data;
232
233 /* Discover request for incoming response */
234 cmd_req = smp_client_response_discover(res_hdr);
235 LOG_DBG("Response Header len %d, flags %d OP: %d group %d id %d seq %d", res_hdr->nh_len,
236 res_hdr->nh_flags, res_hdr->nh_op, res_hdr->nh_group, res_hdr->nh_id,
237 res_hdr->nh_seq);
238
239 if (cmd_req) {
240 cb = cmd_req->cb;
241 user_data = cmd_req->user_data;
242 smp_client_cmd_req_free(cmd_req);
243 if (cb) {
244 cb(nb, user_data);
245 return MGMT_ERR_EOK;
246 }
247 }
248
249 return MGMT_ERR_ENOENT;
250 }
251
smp_client_buf_allocation(struct smp_client_object * smp_client,uint16_t group,uint8_t command_id,uint8_t op,enum smp_mcumgr_version_t version)252 struct net_buf *smp_client_buf_allocation(struct smp_client_object *smp_client, uint16_t group,
253 uint8_t command_id, uint8_t op,
254 enum smp_mcumgr_version_t version)
255 {
256 struct net_buf *nb;
257 struct smp_hdr smp_header;
258
259 nb = smp_packet_alloc();
260
261 if (nb) {
262 /* Write SMP header with payload length 0 */
263 smp_header_init(&smp_header, group, command_id, op, 0, smp_client->smp_seq++,
264 version);
265 memcpy(nb->data, &smp_header, sizeof(smp_header));
266 nb->len = sizeof(smp_header);
267 }
268 return nb;
269 }
270
smp_client_buf_free(struct net_buf * nb)271 void smp_client_buf_free(struct net_buf *nb)
272 {
273 smp_packet_free(nb);
274 }
275
smp_read_hdr(const struct net_buf * nb,struct smp_hdr * dst_hdr)276 static void smp_read_hdr(const struct net_buf *nb, struct smp_hdr *dst_hdr)
277 {
278 memcpy(dst_hdr, nb->data, sizeof(*dst_hdr));
279 dst_hdr->nh_len = sys_be16_to_cpu(dst_hdr->nh_len);
280 dst_hdr->nh_group = sys_be16_to_cpu(dst_hdr->nh_group);
281 }
282
smp_client_send_cmd(struct smp_client_object * smp_client,struct net_buf * nb,smp_client_res_fn cb,void * user_data,int timeout_in_sec)283 int smp_client_send_cmd(struct smp_client_object *smp_client, struct net_buf *nb,
284 smp_client_res_fn cb, void *user_data, int timeout_in_sec)
285 {
286 struct smp_hdr smp_header;
287 struct smp_client_cmd_req *cmd_req;
288
289 if (timeout_in_sec > 30) {
290 LOG_ERR("Command timeout can't be over 30 seconds");
291 return MGMT_ERR_EINVAL;
292 }
293
294 if (timeout_in_sec == 0) {
295 timeout_in_sec = CONFIG_SMP_CMD_DEFAULT_LIFE_TIME;
296 }
297
298 smp_read_hdr(nb, &smp_header);
299 if (nb->len < sizeof(smp_header)) {
300 return MGMT_ERR_EINVAL;
301 }
302 /* Update Length */
303 smp_header.nh_len = sys_cpu_to_be16(nb->len - sizeof(smp_header));
304 smp_header.nh_group = sys_cpu_to_be16(smp_header.nh_group),
305 memcpy(nb->data, &smp_header, sizeof(smp_header));
306
307 cmd_req = smp_client_cmd_req_allocate();
308 if (!cmd_req) {
309 return MGMT_ERR_ENOMEM;
310 }
311
312 LOG_DBG("Command send Header flags %d OP: %d group %d id %d seq %d", smp_header.nh_flags,
313 smp_header.nh_op, sys_be16_to_cpu(smp_header.nh_group), smp_header.nh_id,
314 smp_header.nh_seq);
315 cmd_req->nb = nb;
316 cmd_req->cb = cb;
317 cmd_req->smp_client = smp_client;
318 cmd_req->user_data = user_data;
319 cmd_req->retry_cnt = timeout_in_sec * (1000 / CONFIG_SMP_CMD_RETRY_TIME);
320 cmd_req->timestamp = k_uptime_get() + CONFIG_SMP_CMD_RETRY_TIME;
321 /* Increment reference for re-transmission and read smp header */
322 nb = net_buf_ref(nb);
323 smp_cmd_add_to_list(cmd_req);
324 k_fifo_put(&smp_client->tx_fifo, nb);
325 k_work_submit_to_queue(smp_get_wq(), &smp_client->work);
326 return MGMT_ERR_EOK;
327 }
328
329 SYS_INIT(smp_client_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY);
330
