1 /*
2 * Copyright (c) 2022 René Beckmann
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /** @file mqtt_sn_encoder.c
8 *
9 * @brief MQTT-SN message encoder.
10 */
11
12 #include "mqtt_sn_msg.h"
13
14 #include <zephyr/net/mqtt_sn.h>
15
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_DECLARE(net_mqtt_sn, CONFIG_MQTT_SN_LOG_LEVEL);
18
19 /**
20 * @brief Prepare a message
21 *
22 * @param buf Message struct to use
23 * @param sz The length of the message's payload without the length field and the type.
24 * @param type Message type
25 * @return 0 or negative error code
26 */
prepare_message(struct net_buf_simple * buf,size_t sz,enum mqtt_sn_msg_type type)27 static int prepare_message(struct net_buf_simple *buf, size_t sz, enum mqtt_sn_msg_type type)
28 {
29 /* Add one for the type field */
30 sz++;
31 /* add size of length field */
32 sz += (sz > 254 ? 3 : 1);
33
34 size_t maxlen = net_buf_simple_max_len(buf);
35
36 LOG_DBG("Preparing message of type %d with size %zu", type, sz);
37
38 /* Size must not be larger than an uint16_t can fit */
39 if (sz > UINT16_MAX) {
40 LOG_ERR("Message of size %zu is too large for MQTT-SN", sz);
41 return -EFBIG;
42 }
43
44 if (sz > maxlen) {
45 LOG_ERR("Message of size %zu does not fit in buffer of length %zu", sz, maxlen);
46 return -ENOMEM;
47 }
48
49 if (sz <= 255) {
50 net_buf_simple_add_u8(buf, (uint8_t)sz);
51 } else {
52 net_buf_simple_add_u8(buf, MQTT_SN_LENGTH_FIELD_EXTENDED_PREFIX);
53 net_buf_simple_add_be16(buf, (uint16_t)sz);
54 }
55
56 net_buf_simple_add_u8(buf, (uint8_t)type);
57
58 return 0;
59 }
60
encode_flags(struct net_buf_simple * buf,struct mqtt_sn_flags * flags)61 static void encode_flags(struct net_buf_simple *buf, struct mqtt_sn_flags *flags)
62 {
63 uint8_t b = 0;
64
65 LOG_DBG("Encode flags %d, %d, %d, %d, %d, %d", flags->dup, flags->retain, flags->will,
66 flags->clean_session, flags->qos, flags->topic_type);
67
68 b |= flags->dup ? MQTT_SN_FLAGS_DUP : 0;
69 b |= flags->retain ? MQTT_SN_FLAGS_RETAIN : 0;
70 b |= flags->will ? MQTT_SN_FLAGS_WILL : 0;
71 b |= flags->clean_session ? MQTT_SN_FLAGS_CLEANSESSION : 0;
72
73 b |= ((flags->qos << MQTT_SN_FLAGS_SHIFT_QOS) & MQTT_SN_FLAGS_MASK_QOS);
74 b |= ((flags->topic_type << MQTT_SN_FLAGS_SHIFT_TOPICID_TYPE) &
75 MQTT_SN_FLAGS_MASK_TOPICID_TYPE);
76
77 net_buf_simple_add_u8(buf, b);
78 }
79
mqtt_sn_encode_msg_searchgw(struct net_buf_simple * buf,struct mqtt_sn_param_searchgw * params)80 static int mqtt_sn_encode_msg_searchgw(struct net_buf_simple *buf,
81 struct mqtt_sn_param_searchgw *params)
82 {
83 size_t msgsz = 1;
84 int err;
85
86 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_SEARCHGW);
87 if (err) {
88 return err;
89 }
90
91 net_buf_simple_add_u8(buf, params->radius);
92
93 return 0;
94 }
95
mqtt_sn_encode_msg_gwinfo(struct net_buf_simple * buf,struct mqtt_sn_param_gwinfo * params)96 static int mqtt_sn_encode_msg_gwinfo(struct net_buf_simple *buf,
97 struct mqtt_sn_param_gwinfo *params)
98 {
99 size_t msgsz = 1 + params->gw_add.size;
100 int err;
101
102 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_GWINFO);
103 if (err) {
104 return err;
105 }
106
107 net_buf_simple_add_u8(buf, params->gw_id);
108 net_buf_simple_add_data(buf, ¶ms->gw_add);
109
110 return 0;
111 }
112
mqtt_sn_encode_msg_connect(struct net_buf_simple * buf,struct mqtt_sn_param_connect * params)113 static int mqtt_sn_encode_msg_connect(struct net_buf_simple *buf,
114 struct mqtt_sn_param_connect *params)
115 {
116 size_t msgsz = 4 + params->client_id.size;
117 struct mqtt_sn_flags flags = {.will = params->will, .clean_session = params->clean_session};
118 int err;
119
120 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_CONNECT);
121 if (err) {
122 return err;
123 }
124
125 encode_flags(buf, &flags);
126
127 net_buf_simple_add_u8(buf, MQTT_SN_PROTOCOL_ID);
128
129 net_buf_simple_add_be16(buf, params->duration);
130
131 net_buf_simple_add_data(buf, ¶ms->client_id);
132
133 return 0;
134 }
135
mqtt_sn_encode_msg_willtopic(struct net_buf_simple * buf,struct mqtt_sn_param_willtopic * params)136 static int mqtt_sn_encode_msg_willtopic(struct net_buf_simple *buf,
137 struct mqtt_sn_param_willtopic *params)
138 {
139 size_t msgsz = 1 + params->topic.size;
140 struct mqtt_sn_flags flags = {.qos = params->qos, .retain = params->retain};
141 int err;
142
143 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_WILLTOPIC);
144 if (err) {
145 return err;
146 }
147
148 encode_flags(buf, &flags);
149
150 net_buf_simple_add_data(buf, ¶ms->topic);
151
152 return 0;
153 }
154
mqtt_sn_encode_msg_willmsg(struct net_buf_simple * buf,struct mqtt_sn_param_willmsg * params)155 static int mqtt_sn_encode_msg_willmsg(struct net_buf_simple *buf,
156 struct mqtt_sn_param_willmsg *params)
157 {
158 size_t msgsz = params->msg.size;
159 int err;
160
161 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_WILLMSG);
162 if (err) {
163 return err;
164 }
165
166 net_buf_simple_add_data(buf, ¶ms->msg);
167
168 return 0;
169 }
170
mqtt_sn_encode_msg_register(struct net_buf_simple * buf,struct mqtt_sn_param_register * params)171 static int mqtt_sn_encode_msg_register(struct net_buf_simple *buf,
172 struct mqtt_sn_param_register *params)
173 {
174 size_t msgsz = 4 + params->topic.size;
175 int err;
176
177 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_REGISTER);
178 if (err) {
179 return err;
180 }
181
182 /* When sent by the client, the topic ID is always 0x0000 */
183 net_buf_simple_add_be16(buf, 0x00);
184 net_buf_simple_add_be16(buf, params->msg_id);
185 net_buf_simple_add_data(buf, ¶ms->topic);
186
187 return 0;
188 }
189
mqtt_sn_encode_msg_regack(struct net_buf_simple * buf,struct mqtt_sn_param_regack * params)190 static int mqtt_sn_encode_msg_regack(struct net_buf_simple *buf,
191 struct mqtt_sn_param_regack *params)
192 {
193 size_t msgsz = 5;
194 int err;
195
196 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_REGACK);
197 if (err) {
198 return err;
199 }
200
201 net_buf_simple_add_be16(buf, params->topic_id);
202 net_buf_simple_add_be16(buf, params->msg_id);
203 net_buf_simple_add_u8(buf, params->ret_code);
204
205 return 0;
206 }
207
mqtt_sn_encode_msg_publish(struct net_buf_simple * buf,struct mqtt_sn_param_publish * params)208 static int mqtt_sn_encode_msg_publish(struct net_buf_simple *buf,
209 struct mqtt_sn_param_publish *params)
210 {
211 size_t msgsz = 5 + params->data.size;
212 struct mqtt_sn_flags flags = {.dup = params->dup,
213 .retain = params->retain,
214 .qos = params->qos,
215 .topic_type = params->topic_type};
216 int err;
217
218 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_PUBLISH);
219 if (err) {
220 return err;
221 }
222 encode_flags(buf, &flags);
223
224 net_buf_simple_add_be16(buf, params->topic_id);
225
226 if (params->qos == MQTT_SN_QOS_1 || params->qos == MQTT_SN_QOS_2) {
227 net_buf_simple_add_be16(buf, params->msg_id);
228 } else {
229 /* Only relevant in case of QoS levels 1 and 2, otherwise coded 0x0000. */
230 net_buf_simple_add_be16(buf, 0x0000);
231 }
232
233 net_buf_simple_add_data(buf, ¶ms->data);
234
235 return 0;
236 }
237
mqtt_sn_encode_msg_puback(struct net_buf_simple * buf,struct mqtt_sn_param_puback * params)238 static int mqtt_sn_encode_msg_puback(struct net_buf_simple *buf,
239 struct mqtt_sn_param_puback *params)
240 {
241 size_t msgsz = 5;
242 int err;
243
244 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_PUBACK);
245 if (err) {
246 return err;
247 }
248
249 net_buf_simple_add_be16(buf, params->topic_id);
250 net_buf_simple_add_be16(buf, params->msg_id);
251 net_buf_simple_add_u8(buf, params->ret_code);
252
253 return 0;
254 }
255
mqtt_sn_encode_msg_pubrec(struct net_buf_simple * buf,struct mqtt_sn_param_pubrec * params)256 static int mqtt_sn_encode_msg_pubrec(struct net_buf_simple *buf,
257 struct mqtt_sn_param_pubrec *params)
258 {
259 size_t msgsz = 2;
260 int err;
261
262 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_PUBREC);
263 if (err) {
264 return err;
265 }
266
267 net_buf_simple_add_be16(buf, params->msg_id);
268
269 return 0;
270 }
271
mqtt_sn_encode_msg_pubrel(struct net_buf_simple * buf,struct mqtt_sn_param_pubrel * params)272 static int mqtt_sn_encode_msg_pubrel(struct net_buf_simple *buf,
273 struct mqtt_sn_param_pubrel *params)
274 {
275 size_t msgsz = 2;
276 int err;
277
278 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_PUBREL);
279 if (err) {
280 return err;
281 }
282
283 net_buf_simple_add_be16(buf, params->msg_id);
284
285 return 0;
286 }
287
mqtt_sn_encode_msg_pubcomp(struct net_buf_simple * buf,struct mqtt_sn_param_pubcomp * params)288 static int mqtt_sn_encode_msg_pubcomp(struct net_buf_simple *buf,
289 struct mqtt_sn_param_pubcomp *params)
290 {
291 size_t msgsz = 2;
292 int err;
293
294 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_PUBCOMP);
295 if (err) {
296 return err;
297 }
298
299 net_buf_simple_add_be16(buf, params->msg_id);
300
301 return 0;
302 }
303
mqtt_sn_encode_msg_subscribe(struct net_buf_simple * buf,struct mqtt_sn_param_subscribe * params)304 static int mqtt_sn_encode_msg_subscribe(struct net_buf_simple *buf,
305 struct mqtt_sn_param_subscribe *params)
306 {
307 size_t msgsz = 3;
308
309 struct mqtt_sn_flags flags = {
310 .dup = params->dup, .qos = params->qos, .topic_type = params->topic_type};
311 int err;
312
313 if (params->topic_type == MQTT_SN_TOPIC_TYPE_NORMAL) {
314 msgsz += params->topic.topic_name.size;
315 } else {
316 msgsz += 2;
317 }
318
319 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_SUBSCRIBE);
320 if (err) {
321 return err;
322 }
323 encode_flags(buf, &flags);
324
325 net_buf_simple_add_be16(buf, params->msg_id);
326
327 if (params->topic_type == MQTT_SN_TOPIC_TYPE_NORMAL) {
328 net_buf_simple_add_data(buf, ¶ms->topic.topic_name);
329 } else {
330 net_buf_simple_add_be16(buf, params->topic.topic_id);
331 }
332
333 return 0;
334 }
335
mqtt_sn_encode_msg_unsubscribe(struct net_buf_simple * buf,struct mqtt_sn_param_unsubscribe * params)336 static int mqtt_sn_encode_msg_unsubscribe(struct net_buf_simple *buf,
337 struct mqtt_sn_param_unsubscribe *params)
338 {
339 size_t msgsz = 3;
340
341 struct mqtt_sn_flags flags = {.topic_type = params->topic_type};
342
343 if (params->topic_type == MQTT_SN_TOPIC_TYPE_NORMAL) {
344 msgsz += params->topic.topic_name.size;
345 } else {
346 msgsz += 2;
347 }
348 int err;
349
350 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_UNSUBSCRIBE);
351 if (err) {
352 return err;
353 }
354 encode_flags(buf, &flags);
355
356 net_buf_simple_add_be16(buf, params->msg_id);
357
358 if (params->topic_type == MQTT_SN_TOPIC_TYPE_NORMAL) {
359 net_buf_simple_add_data(buf, ¶ms->topic.topic_name);
360 } else {
361 net_buf_simple_add_be16(buf, params->topic.topic_id);
362 }
363
364 return 0;
365 }
366
mqtt_sn_encode_msg_pingreq(struct net_buf_simple * buf,struct mqtt_sn_param_pingreq * params)367 static int mqtt_sn_encode_msg_pingreq(struct net_buf_simple *buf,
368 struct mqtt_sn_param_pingreq *params)
369 {
370 size_t msgsz = params->client_id.size;
371 int err;
372
373 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_PINGREQ);
374 if (err) {
375 return err;
376 }
377
378 if (params->client_id.size) {
379 net_buf_simple_add_data(buf, ¶ms->client_id);
380 }
381
382 return 0;
383 }
384
mqtt_sn_encode_msg_pingresp(struct net_buf_simple * buf)385 static int mqtt_sn_encode_msg_pingresp(struct net_buf_simple *buf)
386 {
387 return prepare_message(buf, 0, MQTT_SN_MSG_TYPE_PINGRESP);
388 }
389
mqtt_sn_encode_msg_disconnect(struct net_buf_simple * buf,struct mqtt_sn_param_disconnect * params)390 static int mqtt_sn_encode_msg_disconnect(struct net_buf_simple *buf,
391 struct mqtt_sn_param_disconnect *params)
392 {
393 size_t msgsz = params->duration ? 2 : 0;
394 int err;
395
396 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_DISCONNECT);
397 if (err) {
398 return err;
399 }
400
401 if (params->duration) {
402 net_buf_simple_add_be16(buf, params->duration);
403 }
404
405 return 0;
406 }
407
mqtt_sn_encode_msg_willtopicupd(struct net_buf_simple * buf,struct mqtt_sn_param_willtopicupd * params)408 static int mqtt_sn_encode_msg_willtopicupd(struct net_buf_simple *buf,
409 struct mqtt_sn_param_willtopicupd *params)
410 {
411 size_t msgsz = 0;
412 struct mqtt_sn_flags flags = {.qos = params->qos, .retain = params->retain};
413 int err;
414
415 if (params->topic.size) {
416 msgsz += 1 + params->topic.size;
417 }
418
419 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_WILLTOPICUPD);
420 if (err) {
421 return err;
422 }
423
424 /* If the topic is empty, send an empty message to delete the will topic & message. */
425 if (params->topic.size) {
426 encode_flags(buf, &flags);
427
428 net_buf_simple_add_data(buf, ¶ms->topic);
429 }
430
431 return 0;
432 }
433
mqtt_sn_encode_msg_willmsgupd(struct net_buf_simple * buf,struct mqtt_sn_param_willmsgupd * params)434 static int mqtt_sn_encode_msg_willmsgupd(struct net_buf_simple *buf,
435 struct mqtt_sn_param_willmsgupd *params)
436 {
437 size_t msgsz = params->msg.size;
438 int err;
439
440 err = prepare_message(buf, msgsz, MQTT_SN_MSG_TYPE_WILLMSGUPD);
441 if (err) {
442 return err;
443 }
444
445 net_buf_simple_add_data(buf, ¶ms->msg);
446
447 return 0;
448 }
449
mqtt_sn_encode_msg(struct net_buf_simple * buf,struct mqtt_sn_param * param)450 int mqtt_sn_encode_msg(struct net_buf_simple *buf, struct mqtt_sn_param *param)
451 {
452 int result;
453
454 if (buf->len) {
455 LOG_ERR("Buffer not clean - bug?");
456 return -EBUSY;
457 }
458
459 LOG_DBG("Encoding message of type %d", param->type);
460
461 switch (param->type) {
462 case MQTT_SN_MSG_TYPE_SEARCHGW:
463 result = mqtt_sn_encode_msg_searchgw(buf, ¶m->params.searchgw);
464 break;
465 case MQTT_SN_MSG_TYPE_GWINFO:
466 result = mqtt_sn_encode_msg_gwinfo(buf, ¶m->params.gwinfo);
467 break;
468 case MQTT_SN_MSG_TYPE_CONNECT:
469 result = mqtt_sn_encode_msg_connect(buf, ¶m->params.connect);
470 break;
471 case MQTT_SN_MSG_TYPE_WILLTOPIC:
472 result = mqtt_sn_encode_msg_willtopic(buf, ¶m->params.willtopic);
473 break;
474 case MQTT_SN_MSG_TYPE_WILLMSG:
475 result = mqtt_sn_encode_msg_willmsg(buf, ¶m->params.willmsg);
476 break;
477 case MQTT_SN_MSG_TYPE_REGISTER:
478 result = mqtt_sn_encode_msg_register(buf, ¶m->params.reg);
479 break;
480 case MQTT_SN_MSG_TYPE_REGACK:
481 result = mqtt_sn_encode_msg_regack(buf, ¶m->params.regack);
482 break;
483 case MQTT_SN_MSG_TYPE_PUBLISH:
484 result = mqtt_sn_encode_msg_publish(buf, ¶m->params.publish);
485 break;
486 case MQTT_SN_MSG_TYPE_PUBACK:
487 result = mqtt_sn_encode_msg_puback(buf, ¶m->params.puback);
488 break;
489 case MQTT_SN_MSG_TYPE_PUBREC:
490 result = mqtt_sn_encode_msg_pubrec(buf, ¶m->params.pubrec);
491 break;
492 case MQTT_SN_MSG_TYPE_PUBREL:
493 result = mqtt_sn_encode_msg_pubrel(buf, ¶m->params.pubrel);
494 break;
495 case MQTT_SN_MSG_TYPE_PUBCOMP:
496 result = mqtt_sn_encode_msg_pubcomp(buf, ¶m->params.pubcomp);
497 break;
498 case MQTT_SN_MSG_TYPE_SUBSCRIBE:
499 result = mqtt_sn_encode_msg_subscribe(buf, ¶m->params.subscribe);
500 break;
501 case MQTT_SN_MSG_TYPE_UNSUBSCRIBE:
502 result = mqtt_sn_encode_msg_unsubscribe(buf, ¶m->params.unsubscribe);
503 break;
504 case MQTT_SN_MSG_TYPE_PINGREQ:
505 result = mqtt_sn_encode_msg_pingreq(buf, ¶m->params.pingreq);
506 break;
507 case MQTT_SN_MSG_TYPE_PINGRESP:
508 result = mqtt_sn_encode_msg_pingresp(buf);
509 break;
510 case MQTT_SN_MSG_TYPE_DISCONNECT:
511 result = mqtt_sn_encode_msg_disconnect(buf, ¶m->params.disconnect);
512 break;
513 case MQTT_SN_MSG_TYPE_WILLTOPICUPD:
514 result = mqtt_sn_encode_msg_willtopicupd(buf, ¶m->params.willtopicupd);
515 break;
516 case MQTT_SN_MSG_TYPE_WILLMSGUPD:
517 result = mqtt_sn_encode_msg_willmsgupd(buf, ¶m->params.willmsgupd);
518 break;
519 default:
520 LOG_ERR("Unsupported msg type %d", param->type);
521 result = -ENOTSUP;
522 break;
523 }
524
525 return result;
526 }
527
