1: #include "global.h"
2:
3:
4: /* ------------------------------------------------------------------- */
5:
6: /*
7: * mqtt_msgCONNECT() Create CONNECT message
8: *
9: * @buf = Message buffer
10: * @csConnID = ConnectID
11: * @kasec = Keep alive timeout
12: * @csUser = Username if !=NULL
13: * @csPass = Password for Username, only if csUser is set
14: * @csWillTopic = Will Topic if !=NULL Will Flags set into message
15: * @csWillMessage = Will Message, may be NULL
16: * @ClrSess = Clear Session subscriptions after disconnect
17: * @WillQOS = Will QOS if csWillTopic is set
18: * @WillRetain = Will Retain Will Message if csWillTopic is set
19: * return: -1 error or >-1 message size for send
20: */
21: int
22: mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const char *csConnID,
23: u_short kasec, const char *csUser, const char *csPass,
24: const char *csWillTopic, const char *csWillMessage,
25: u_char ClrSess, u_char WillQOS, u_char WillRetain)
26: {
27: int siz = 0;
28: struct mqtthdr *hdr;
29: mqtthdr_var_t *var, *cid, *topic, *wmsg, *user, *pass;
30: mqtthdr_protover_t *proto;
31: mqtthdr_connflgs_t *flags;
32: mqtt_v_t *ka;
33:
34: if (!buf || !csConnID)
35: return -1;
36: if (strlen(csConnID) > 23) {
37: mqtt_SetErr(EINVAL, "Error:: invalid argument ConnID is too long (max 23 bytes)");
38: return -1;
39: }
40: if (csUser && strlen(csUser) > 12) {
41: mqtt_SetErr(EINVAL, "Error:: invalid argument Username is too long (max 12 bytes)");
42: return -1;
43: }
44: if (csPass && strlen(csPass) > 12) {
45: mqtt_SetErr(EINVAL, "Error:: invalid argument Password is too long (max 12 bytes)");
46: return -1;
47: }
48: if (WillQOS > MQTT_QOS_EXACTLY) {
49: mqtt_SetErr(EINVAL, "Error:: invalid argument WillQOS - unknown QOS value");
50: return -1;
51: }
52:
53: if (mqtt_msgRealloc(buf, BUFSIZ) == -1)
54: return -1;
55: else {
56: hdr = (struct mqtthdr *) (buf->msg_base + siz);
57: siz += sizeof(struct mqtthdr);
58: var = (mqtthdr_var_t*) (buf->msg_base + siz);
59: siz += 8;
60: proto = buf->msg_base + siz;
61: siz++;
62: flags = (mqtthdr_connflgs_t*) (buf->msg_base + siz);
63: siz++;
64: ka = (mqtt_v_t*) (buf->msg_base + siz);
65: siz += sizeof(mqtt_v_t);
66: }
67:
68: /* fixed header */
69: MQTTHDR_MSGINIT(hdr);
70: hdr->mqtt_msg.type = MQTT_TYPE_CONNECT;
71: *hdr->mqtt_len = 0;
72:
73: /* variable header */
74: var->var_sb.sb.l = 6;
75: memcpy(var->var_data, MQTT_CONN_STR, 6);
76:
77: *proto = MQTT_PROTO_VER;
78:
79: /* CONNECT header */
80: flags->clean_sess = ClrSess ? 1 : 0;
81: if (csUser && *csUser) {
82: flags->username = 1;
83: flags->password = csPass ? 1 : 0;
84: } else {
85: flags->username = 0;
86: flags->password = 0;
87: }
88: if (csWillTopic && *csWillTopic) {
89: flags->will_flg = 1;
90: flags->will_qos = WillQOS;
91: flags->will_retain = WillRetain ? 1 : 0;
92: } else {
93: flags->will_flg = 0;
94: flags->will_qos = 0;
95: flags->will_retain = 0;
96: }
97:
98: ka->val = kasec ? htons(kasec) : htons(MQTT_KEEPALIVE);
99:
100: /* ConnID */
101: cid = (mqtthdr_var_t*) (buf->msg_base + siz);
102: cid->var_sb.val = htons(strlen(csConnID));
103: siz += MQTTHDR_VAR_SIZEOF(cid);
104: memcpy(cid->var_data, csConnID, ntohs(cid->var_sb.val));
105:
106: /* If Will Flags setup */
107: if (csWillTopic && *csWillTopic) {
108: topic = (mqtthdr_var_t*) (buf->msg_base + siz);
109: topic->var_sb.val = htons(strlen(csWillTopic));
110: memcpy(topic->var_data, csWillTopic, ntohs(topic->var_sb.val));
111: siz += MQTTHDR_VAR_SIZEOF(topic);
112:
113: wmsg = (mqtthdr_var_t*) (buf->msg_base + siz);
114: if (csWillMessage && *csWillMessage) {
115: wmsg->var_sb.val = htons(strlen(csWillMessage));
116: memcpy(wmsg->var_data, csWillMessage, ntohs(wmsg->var_sb.val));
117: } else
118: wmsg->var_sb.val = 0;
119: siz += MQTTHDR_VAR_SIZEOF(wmsg);
120: }
121:
122: /* If defined Username & Password */
123: if (csUser && *csUser) {
124: user = (mqtthdr_var_t*) (buf->msg_base + siz);
125: user->var_sb.val = htons(strlen(csUser));
126: memcpy(user->var_data, csUser, ntohs(user->var_sb.val));
127: siz += MQTTHDR_VAR_SIZEOF(user);
128:
129: if (csPass && *csPass) {
130: pass = (mqtthdr_var_t*) (buf->msg_base + siz);
131: pass->var_sb.val = htons(strlen(csPass));
132: memcpy(pass->var_data, csPass, ntohs(pass->var_sb.val));
133: siz += MQTTHDR_VAR_SIZEOF(pass);
134: }
135: }
136:
137: *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr));
138: mqtt_msgRealloc(buf, siz);
139: return siz;
140: }
141:
142: /*
143: * mqtt_msgCONNACK() Create CONNACK message
144: *
145: * @buf = Message buffer
146: * @retcode = Return code
147: * return: -1 error or >-1 message size for send
148: */
149: int
150: mqtt_msgCONNACK(mqtt_msg_t * __restrict buf, u_char retcode)
151: {
152: int siz = 0;
153: struct mqtthdr *hdr;
154: mqtthdr_connack_t *ack;
155:
156: if (!buf)
157: return -1;
158: if (retcode > MQTT_RETCODE_DENIED) {
159: mqtt_SetErr(EINVAL, "Error:: invalid retcode");
160: return -1;
161: }
162:
163: if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr) + sizeof(mqtthdr_connack_t)) == -1)
164: return -1;
165: else {
166: hdr = (struct mqtthdr *) (buf->msg_base + siz);
167: siz += sizeof(struct mqtthdr);
168: ack = (mqtthdr_connack_t*) (buf->msg_base + siz);
169: siz += sizeof(mqtthdr_connack_t);
170: }
171:
172: /* fixed header */
173: MQTTHDR_MSGINIT(hdr);
174: hdr->mqtt_msg.type = MQTT_TYPE_CONNACK;
175: *hdr->mqtt_len = sizeof(mqtthdr_connack_t);
176:
177: /* CONNACK header */
178: ack->reserved = 0;
179: ack->retcode = retcode;
180:
181: return siz;
182: }
183:
184: static int
185: _mqtt_msgSIMPLE_(mqtt_msg_t * __restrict buf, u_char cmd)
186: {
187: int siz = 0;
188: struct mqtthdr *hdr;
189:
190: if (!buf)
191: return -1;
192:
193: if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr)) == -1)
194: return -1;
195: else {
196: hdr = (struct mqtthdr *) (buf->msg_base + siz);
197: siz += sizeof(struct mqtthdr);
198: }
199:
200: /* fixed header */
201: MQTTHDR_MSGINIT(hdr);
202: hdr->mqtt_msg.type = cmd;
203: *hdr->mqtt_len = 0;
204:
205: return siz;
206: }
207:
208: /*
209: * mqtt_msgPINGREQ() Create PINGREQ message
210: *
211: * @buf = Message buffer
212: * return: -1 error or >-1 message size for send
213: */
214: int
215: mqtt_msgPINGREQ(mqtt_msg_t * __restrict buf)
216: {
217: return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGREQ);
218: }
219:
220: /*
221: * mqtt_msgPINGRESP() Create PINGRESP message
222: *
223: * @buf = Message buffer
224: * return: -1 error or >-1 message size for send
225: */
226: int
227: mqtt_msgPINGRESP(mqtt_msg_t * __restrict buf)
228: {
229: return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGRESP);
230: }
231:
232: /*
233: * mqtt_msgDISCONNECT() Create DISCONNECT message
234: *
235: * @buf = Message buffer
236: * return: -1 error or >-1 message size for send
237: */
238: int
239: mqtt_msgDISCONNECT(mqtt_msg_t * __restrict buf)
240: {
241: return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_DISCONNECT);
242: }
243:
244: /* ============= decode ============ */
245:
246: /*
247: * mqtt_readCONNECT() Read elements from CONNECT message
248: *
249: * @buf = Message buffer
250: * @kasec = Keep Alive in seconds for current connection
251: * @psConnID = ConnectID
252: * @connLen = ConnectID length
253: * @psUser = Username if !=NULL
254: * @userLen = Username length
255: * @psPass = Password for Username, only if csUser is set
256: * @passLen = Password length
257: * @psWillTopic = Will Topic if !=NULL Will Flags set into message and must be free()
258: * @psWillMessage = Will Message, may be NULL if !NULL must be free() after use!
259: * return: .reserved == 1 is error or == 0 connection flags & msg ok
260: */
261: mqtthdr_connack_t
262: mqtt_readCONNECT(mqtt_msg_t * __restrict buf, u_short *kasec, char * __restrict psConnID, int connLen,
263: char * __restrict psUser, int userLen, char * __restrict psPass, int passLen,
264: char ** __restrict psWillTopic, char ** __restrict psWillMessage)
265: {
266: mqtthdr_connflgs_t flg = { MQTT_CONNFLGS_INIT };
267: mqtthdr_connack_t cack = { 1, MQTT_RETCODE_DENIED };
268: struct mqtthdr *hdr;
269: mqtthdr_var_t *var;
270: mqtt_v_t *ka;
271: mqtthdr_protover_t *proto;
272: int len, ret;
273: caddr_t pos;
274:
275: if (!buf || !buf->msg_base || !buf->msg_len || !psConnID || !connLen)
276: return cack;
277:
278: hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNECT, &ret, &len);
279: if (!hdr)
280: return cack;
281: if (len < 12) {
282: mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
283: return cack;
284: } else {
285: pos = buf->msg_base + ret + 1;
286: var = (mqtthdr_var_t*) pos;
287: }
288: /* check init string & protocol */
289: if (var->var_sb.sb.l != 6 || strncmp((char*) var->var_data, MQTT_CONN_STR, 6)) {
290: mqtt_SetErr(EINVAL, "Error:: invalid init string %.6s(%d)",
291: var->var_data, var->var_sb.sb.l);
292: cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
293: return cack;
294: } else {
295: pos += var->var_sb.sb.l + sizeof(mqtt_v_t);
296: proto = (mqtthdr_protover_t*) pos;
297: }
298: if (*proto != MQTT_PROTO_VER) {
299: mqtt_SetErr(EINVAL, "Error:: invalid protocol version %d", *pos);
300: cack.retcode = MQTT_RETCODE_REFUSE_VER;
301: return cack;
302: } else
303: pos++;
304: flg = *(mqtthdr_connflgs_t*) pos;
305: pos++;
306: ka = (mqtt_v_t*) pos;
307: *kasec = ntohs(ka->val);
308: pos += sizeof(mqtt_v_t);
309:
310: len -= pos - (caddr_t) var;
311:
312: /* get ConnID */
313: var = (mqtthdr_var_t*) pos;
314: len -= MQTTHDR_VAR_SIZEOF(var);
315: if (len < 0 || var->var_sb.sb.l > 23) {
316: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Connection ID %d", len);
317: cack.retcode = MQTT_RETCODE_REFUSE_ID;
318: return cack;
319: } else {
320: memset(psConnID, 0, connLen--);
321: memcpy(psConnID, var->var_data, ntohs(var->var_sb.val) > connLen ? connLen : ntohs(var->var_sb.val));
322: pos += MQTTHDR_VAR_SIZEOF(var);
323: }
324:
325: /* get Willz */
326: if (flg.will_flg) {
327: var = (mqtthdr_var_t*) pos;
328: len -= MQTTHDR_VAR_SIZEOF(var);
329: if (len < 0) {
330: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Will Topic %d", len);
331: cack.retcode = MQTT_RETCODE_REFUSE_ID;
332: return cack;
333: } else {
334: if (psWillTopic) {
335: *psWillTopic = malloc(ntohs(var->var_sb.val) + 1);
336: if (!*psWillTopic) {
337: LOGERR;
338: cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
339: return cack;
340: } else
341: memset(*psWillTopic, 0, ntohs(var->var_sb.val) + 1);
342: memcpy(*psWillTopic, var->var_data, ntohs(var->var_sb.val));
343: }
344: pos += MQTTHDR_VAR_SIZEOF(var);
345: }
346:
347: var = (mqtthdr_var_t*) pos;
348: len -= MQTTHDR_VAR_SIZEOF(var);
349: if (len < 0) {
350: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Will Message %d", len);
351: cack.retcode = MQTT_RETCODE_REFUSE_ID;
352: return cack;
353: } else {
354: if (psWillMessage) {
355: *psWillMessage = malloc(ntohs(var->var_sb.val) + 1);
356: if (!*psWillMessage) {
357: LOGERR;
358: cack.retcode = MQTT_RETCODE_REFUSE_UNAVAIL;
359: return cack;
360: } else
361: memset(*psWillMessage, 0, ntohs(var->var_sb.val) + 1);
362: memcpy(*psWillMessage, var->var_data, ntohs(var->var_sb.val));
363: }
364: pos += MQTTHDR_VAR_SIZEOF(var);
365: }
366: }
367:
368: /* get User/Pass */
369: if (flg.username) {
370: var = (mqtthdr_var_t*) pos;
371: len -= MQTTHDR_VAR_SIZEOF(var);
372: if (len < 0 || var->var_sb.sb.l > 12) {
373: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Username %d", len);
374: cack.retcode = MQTT_RETCODE_REFUSE_USERPASS;
375: return cack;
376: } else {
377: if (psUser && userLen) {
378: memset(psUser, 0, userLen--);
379: memcpy(psUser, var->var_data,
380: ntohs(var->var_sb.val) > userLen ? userLen : ntohs(var->var_sb.val));
381: }
382: pos += MQTTHDR_VAR_SIZEOF(var);
383: }
384: }
385: if (flg.password) {
386: var = (mqtthdr_var_t*) pos;
387: len -= MQTTHDR_VAR_SIZEOF(var);
388: if (len < 0 || var->var_sb.sb.l > 12) {
389: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Password %d", len);
390: cack.retcode = MQTT_RETCODE_REFUSE_USERPASS;
391: return cack;
392: } else {
393: if (psPass && passLen) {
394: memset(psPass, 0, passLen--);
395: memcpy(psPass, var->var_data,
396: ntohs(var->var_sb.val) > passLen ? passLen : ntohs(var->var_sb.val));
397: }
398: pos += MQTTHDR_VAR_SIZEOF(var);
399: }
400: }
401:
402: flg.reserved = 0;
403: cack.reserved = flg.flags;
404: cack.retcode = MQTT_RETCODE_ACCEPTED;
405: return cack;
406: }
407:
408: /*
409: * mqtt_readCONNACK() Read CONNACK message
410: *
411: * @buf = Message buffer
412: * return: -1 error or >-1 CONNECT message return code
413: */
414: u_char
415: mqtt_readCONNACK(mqtt_msg_t * __restrict buf)
416: {
417: int len, ret;
418: struct mqtthdr *hdr;
419: mqtthdr_connack_t *ack;
420: caddr_t pos;
421:
422: if (!buf || !buf->msg_base || !buf->msg_len)
423: return (u_char) -1;
424:
425: hdr = _mqtt_readHEADER(buf, MQTT_TYPE_CONNACK, &ret, &len);
426: if (!hdr)
427: return (u_char) -1;
428: if (len < sizeof(mqtthdr_connack_t)) {
429: mqtt_SetErr(EINVAL, "Error:: short message length %d", len);
430: return (u_char) -1;
431: } else {
432: pos = buf->msg_base + ret + 1;
433: ack = (mqtthdr_connack_t*) pos;
434: }
435:
436: if (ack->retcode > MQTT_RETCODE_DENIED) {
437: mqtt_SetErr(EINVAL, "Error:: invalid retcode %u", ack->retcode);
438: return (u_char) -1;
439: }
440:
441: return ack->retcode;
442: }
443:
444: /*
445: * mqtt_readDISCONNECT() Read DISCONNECT message
446: *
447: * @buf = Message buffer
448: * return: -1 error, 0 ok, >0 undefined result
449: */
450: int
451: mqtt_readDISCONNECT(mqtt_msg_t * __restrict buf)
452: {
453: int len, ret;
454: struct mqtthdr *hdr;
455:
456: hdr = _mqtt_readHEADER(buf, MQTT_TYPE_DISCONNECT, &ret, &len);
457: if (!hdr || ret != 1)
458: return -1;
459:
460: return len;
461: }
462:
463: /*
464: * mqtt_readPINGREQ() Read PINGREQ message
465: *
466: * @buf = Message buffer
467: * return: -1 error, 0 ok, >0 undefined result
468: */
469: int
470: mqtt_readPINGREQ(mqtt_msg_t * __restrict buf)
471: {
472: int len, ret;
473: struct mqtthdr *hdr;
474:
475: hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PINGREQ, &ret, &len);
476: if (!hdr || ret != 1)
477: return -1;
478:
479: return len;
480: }
481:
482: /*
483: * mqtt_readPINGRESP() Read PINGRESP message
484: *
485: * @buf = Message buffer
486: * return: -1 error, 0 ok, >0 undefined result
487: */
488: int
489: mqtt_readPINGRESP(mqtt_msg_t * __restrict buf)
490: {
491: int len, ret;
492: struct mqtthdr *hdr;
493:
494: hdr = _mqtt_readHEADER(buf, MQTT_TYPE_PINGRESP, &ret, &len);
495: if (!hdr || ret != 1)
496: return -1;
497:
498: return len;
499: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>
![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to conn.c CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [ELWIX - Embedded LightWeight unIX -] / mqtt / src