![]() ![]() | ![]() |
start implement read* message commands & ut
1: #include "global.h" 2: 3: 4: /* ------------------------------------------------------------------- */ 5: 6: /* 7: * mqtt_msgCONNECT() Create CONNECT message 8: * 9: * @buf = Message buffer 10: * @csConnID = ConnectID 11: * @csUser = Username if !=NULL 12: * @csPass = Password for Username, only if csUser is set 13: * @csWillTopic = Will Topic if !=NULL Will Flags set into message 14: * @csWillMessage = Will Message, may be NULL 15: * @ClrSess = Clear Session subscriptions after disconnect 16: * @WillQOS = Will QOS if csWillTopic is set 17: * @WillRetain = Will Retain Will Message if csWillTopic is set 18: * return: -1 error or >-1 message size for send 19: */ 20: int 21: mqtt_msgCONNECT(mqtt_msg_t * __restrict buf, const char *csConnID, 22: const char *csUser, const char *csPass, 23: const char *csWillTopic, const char *csWillMessage, 24: u_char ClrSess, u_char WillQOS, u_char WillRetain) 25: { 26: int siz = 0; 27: struct mqtthdr *hdr; 28: mqtthdr_var_t *var, *cid, *topic, *wmsg, *user, *pass; 29: mqtthdr_protover_t *proto; 30: mqtthdr_connflgs_t *flags; 31: mqtt_v_t *ka; 32: 33: if (!buf || !csConnID) 34: return -1; 35: if (strlen(csConnID) > 23) { 36: mqtt_SetErr(EINVAL, "Error:: invalid argument ConnID is too long (max 23 bytes)"); 37: return -1; 38: } 39: if (csUser && strlen(csUser) > 12) { 40: mqtt_SetErr(EINVAL, "Error:: invalid argument Username is too long (max 12 bytes)"); 41: return -1; 42: } 43: if (csPass && strlen(csPass) > 12) { 44: mqtt_SetErr(EINVAL, "Error:: invalid argument Password is too long (max 12 bytes)"); 45: return -1; 46: } 47: if (WillQOS > MQTT_QOS_EXACTLY) { 48: mqtt_SetErr(EINVAL, "Error:: invalid argument WillQOS - unknown QOS value"); 49: return -1; 50: } 51: 52: if (mqtt_msgRealloc(buf, BUFSIZ) == -1) 53: return -1; 54: else { 55: hdr = (struct mqtthdr *) (buf->msg_base + siz); 56: siz += sizeof(struct mqtthdr); 57: var = (mqtthdr_var_t*) (buf->msg_base + siz); 58: siz += 8; 59: proto = buf->msg_base + siz; 60: siz++; 61: flags = (mqtthdr_connflgs_t*) (buf->msg_base + siz); 62: siz++; 63: ka = (mqtt_v_t*) (buf->msg_base + siz); 64: siz += sizeof(mqtt_v_t); 65: } 66: 67: /* fixed header */ 68: MQTTHDR_MSGINIT(hdr); 69: hdr->mqtt_msg.type = MQTT_TYPE_CONNECT; 70: *hdr->mqtt_len = 0; 71: 72: /* variable header */ 73: var->var_sb.sb.l = 6; 74: memcpy(var->var_data, MQTT_CONN_STR, 6); 75: 76: *proto = MQTT_PROTO_VER; 77: 78: /* CONNECT header */ 79: flags->clean_sess = ClrSess ? 1 : 0; 80: if (csUser) { 81: flags->username = 1; 82: flags->password = csPass ? 1 : 0; 83: } else { 84: flags->username = 0; 85: flags->password = 0; 86: } 87: if (csWillTopic) { 88: flags->will_flg = 1; 89: flags->will_qos = WillQOS; 90: flags->will_retain = WillRetain ? 1 : 0; 91: } else { 92: flags->will_flg = 0; 93: flags->will_qos = 0; 94: flags->will_retain = 0; 95: } 96: 97: ka->sb.l = MQTT_KEEPALIVE; 98: 99: /* ConnID */ 100: cid = (mqtthdr_var_t*) (buf->msg_base + siz); 101: cid->var_sb.val = htons(strlen(csConnID)); 102: siz += MQTTHDR_VAR_SIZEOF(cid); 103: memcpy(cid->var_data, csConnID, ntohs(cid->var_sb.val)); 104: 105: /* If Will Flags setup */ 106: if (csWillTopic) { 107: topic = (mqtthdr_var_t*) (buf->msg_base + siz); 108: topic->var_sb.val = htons(strlen(csWillTopic)); 109: memcpy(topic->var_data, csWillTopic, ntohs(topic->var_sb.val)); 110: siz += MQTTHDR_VAR_SIZEOF(topic); 111: 112: wmsg = (mqtthdr_var_t*) (buf->msg_base + siz); 113: if (csWillMessage) { 114: wmsg->var_sb.val = htons(strlen(csWillMessage)); 115: memcpy(wmsg->var_data, csWillMessage, ntohs(wmsg->var_sb.val)); 116: } else 117: wmsg->var_sb.val = 0; 118: siz += MQTTHDR_VAR_SIZEOF(wmsg); 119: } 120: 121: /* If defined Username & Password */ 122: if (csUser) { 123: user = (mqtthdr_var_t*) (buf->msg_base + siz); 124: user->var_sb.val = htons(strlen(csUser)); 125: memcpy(user->var_data, csUser, ntohs(user->var_sb.val)); 126: siz += MQTTHDR_VAR_SIZEOF(user); 127: 128: if (csPass) { 129: pass = (mqtthdr_var_t*) (buf->msg_base + siz); 130: pass->var_sb.val = htons(strlen(csPass)); 131: memcpy(pass->var_data, csPass, ntohs(pass->var_sb.val)); 132: siz += MQTTHDR_VAR_SIZEOF(pass); 133: } 134: } 135: 136: *hdr->mqtt_len = mqtt_encodeLen(siz - sizeof(struct mqtthdr)); 137: mqtt_msgRealloc(buf, siz); 138: return siz; 139: } 140: 141: /* 142: * mqtt_msgCONNACK() Create CONNACK message 143: * 144: * @buf = Message buffer 145: * @retcode = Return code 146: * return: -1 error or >-1 message size for send 147: */ 148: int 149: mqtt_msgCONNACK(mqtt_msg_t * __restrict buf, u_char retcode) 150: { 151: int siz = 0; 152: struct mqtthdr *hdr; 153: mqtthdr_connack_t *ack; 154: 155: if (!buf) 156: return -1; 157: if (retcode > MQTT_RETCODE_DENIED) { 158: mqtt_SetErr(EINVAL, "Error:: invalid retcode"); 159: return -1; 160: } 161: 162: if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr) + sizeof(mqtthdr_connack_t)) == -1) 163: return -1; 164: else { 165: hdr = (struct mqtthdr *) (buf->msg_base + siz); 166: siz += sizeof(struct mqtthdr); 167: ack = (mqtthdr_connack_t*) (buf->msg_base + siz); 168: siz += sizeof(mqtthdr_connack_t); 169: } 170: 171: /* fixed header */ 172: MQTTHDR_MSGINIT(hdr); 173: hdr->mqtt_msg.type = MQTT_TYPE_CONNACK; 174: *hdr->mqtt_len = sizeof(mqtthdr_connack_t); 175: 176: /* CONNACK header */ 177: ack->reserved = 0; 178: ack->retcode = retcode; 179: 180: return siz; 181: } 182: 183: static int 184: _mqtt_msgSIMPLE_(mqtt_msg_t * __restrict buf, u_char cmd) 185: { 186: int siz = 0; 187: struct mqtthdr *hdr; 188: 189: if (!buf) 190: return -1; 191: 192: if (mqtt_msgRealloc(buf, sizeof(struct mqtthdr)) == -1) 193: return -1; 194: else { 195: hdr = (struct mqtthdr *) (buf->msg_base + siz); 196: siz += sizeof(struct mqtthdr); 197: } 198: 199: /* fixed header */ 200: MQTTHDR_MSGINIT(hdr); 201: hdr->mqtt_msg.type = cmd; 202: *hdr->mqtt_len = 0; 203: 204: return siz; 205: } 206: 207: /* 208: * mqtt_msgPINGREQ() Create PINGREQ message 209: * 210: * @buf = Message buffer 211: * return: -1 error or >-1 message size for send 212: */ 213: int 214: mqtt_msgPINGREQ(mqtt_msg_t * __restrict buf) 215: { 216: return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGREQ); 217: } 218: 219: /* 220: * mqtt_msgPINGRESP() Create PINGRESP message 221: * 222: * @buf = Message buffer 223: * return: -1 error or >-1 message size for send 224: */ 225: int 226: mqtt_msgPINGRESP(mqtt_msg_t * __restrict buf) 227: { 228: return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_PINGRESP); 229: } 230: 231: /* 232: * mqtt_msgDISCONNECT() Create DISCONNECT message 233: * 234: * @buf = Message buffer 235: * return: -1 error or >-1 message size for send 236: */ 237: int 238: mqtt_msgDISCONNECT(mqtt_msg_t * __restrict buf) 239: { 240: return _mqtt_msgSIMPLE_(buf, MQTT_TYPE_DISCONNECT); 241: } 242: 243: /* ============= decode ============ */ 244: 245: /* 246: * mqtt_readCONNECT() Read elements from CONNECT message 247: * 248: * @buf = Message buffer 249: * @kasec = Keep Alive in seconds for current connection 250: * @psConnID = ConnectID 251: * @connLen = ConnectID length 252: * @psUser = Username if !=NULL 253: * @userLen = Username length 254: * @psPass = Password for Username, only if csUser is set 255: * @passLen = Password length 256: * @psWillTopic = Will Topic if !=NULL Will Flags set into message 257: * @topicLen = Will Topic length 258: * @psWillMessage = Will Message, may be NULL 259: * @msgLen = Will Message length 260: * return: .reserved == 1 is error or == 0 connection flags & msg ok 261: */ 262: mqtthdr_connflgs_t 263: mqtt_readCONNECT(mqtt_msg_t * __restrict buf, u_short *kasec, char * __restrict psConnID, int connLen, 264: char * __restrict psUser, int userLen, char * __restrict psPass, int passLen, 265: char * __restrict psWillTopic, int topicLen, char * __restrict psWillMessage, int msgLen) 266: { 267: mqtthdr_connflgs_t flg = MQTT_CONNFLGS_INIT; 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 flg; 277: 278: hdr = (struct mqtthdr*) buf->msg_base; 279: if (hdr->mqtt_msg.type != MQTT_TYPE_CONNECT) { 280: mqtt_SetErr(EINVAL, "Error:: wrong command #%d", hdr->mqtt_msg.type); 281: return flg; 282: } else 283: len = mqtt_decodeLen(hdr->mqtt_len, &ret); 284: if (len < 12) { 285: mqtt_SetErr(EINVAL, "Error:: short message length %d", len); 286: return flg; 287: } else { 288: pos = buf->msg_base + ret + 1; 289: var = (mqtthdr_var_t*) pos; 290: } 291: /* check init string & protocol */ 292: if (var->var_sb.sb.l != 6 || strncmp(var->var_data, MQTT_CONN_STR, 6)) { 293: mqtt_SetErr(EINVAL, "Error:: invalid init string %.6s(%d)", 294: var->var_data, var->var_sb.sb.l); 295: return flg; 296: } else { 297: pos += var->var_sb.sb.l + sizeof(mqtt_v_t); 298: proto = (mqtthdr_protover_t*) pos; 299: } 300: if (*proto != MQTT_PROTO_VER) { 301: mqtt_SetErr(EINVAL, "Error:: invalid protocol version %d", *pos); 302: return flg; 303: } else 304: pos++; 305: flg = *(mqtthdr_connflgs_t*) pos; 306: pos++; 307: ka = (mqtt_v_t*) pos; 308: *kasec = ntohs(ka->val); 309: pos += sizeof(mqtt_v_t); 310: 311: len -= pos - (caddr_t) var; 312: 313: /* get ConnID */ 314: var = (mqtthdr_var_t*) pos; 315: len -= MQTTHDR_VAR_SIZEOF(var); 316: if (len < 0) { 317: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Connection ID %d", len); 318: flg.reserved = 1; 319: return flg; 320: } else { 321: memset(psConnID, 0, connLen--); 322: memcpy(psConnID, var->var_data, ntohs(var->var_sb.val) > connLen ? connLen : ntohs(var->var_sb.val)); 323: pos += MQTTHDR_VAR_SIZEOF(var); 324: } 325: 326: /* get Willz */ 327: if (flg.will_flg) { 328: var = (mqtthdr_var_t*) pos; 329: len -= MQTTHDR_VAR_SIZEOF(var); 330: if (len < 0) { 331: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Will Topic %d", len); 332: flg.reserved = 1; 333: return flg; 334: } else { 335: if (psWillTopic && topicLen) { 336: memset(psWillTopic, 0, topicLen--); 337: memcpy(psWillTopic, var->var_data, 338: ntohs(var->var_sb.val) > topicLen ? topicLen : ntohs(var->var_sb.val)); 339: } 340: pos += MQTTHDR_VAR_SIZEOF(var); 341: } 342: 343: var = (mqtthdr_var_t*) pos; 344: len -= MQTTHDR_VAR_SIZEOF(var); 345: if (len < 0) { 346: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Will Message %d", len); 347: flg.reserved = 1; 348: return flg; 349: } else { 350: if (psWillMessage && msgLen) { 351: memset(psWillMessage, 0, msgLen--); 352: memcpy(psWillMessage, var->var_data, 353: ntohs(var->var_sb.val) > msgLen ? msgLen : ntohs(var->var_sb.val)); 354: } 355: pos += MQTTHDR_VAR_SIZEOF(var); 356: } 357: } 358: 359: /* get User/Pass */ 360: if (flg.username) { 361: var = (mqtthdr_var_t*) pos; 362: len -= MQTTHDR_VAR_SIZEOF(var); 363: if (len < 0) { 364: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Username %d", len); 365: flg.reserved = 1; 366: return flg; 367: } else { 368: if (psUser && userLen) { 369: memset(psUser, 0, userLen--); 370: memcpy(psUser, var->var_data, 371: ntohs(var->var_sb.val) > userLen ? userLen : ntohs(var->var_sb.val)); 372: } 373: pos += MQTTHDR_VAR_SIZEOF(var); 374: } 375: } 376: if (flg.password) { 377: var = (mqtthdr_var_t*) pos; 378: len -= MQTTHDR_VAR_SIZEOF(var); 379: if (len < 0) { 380: mqtt_SetErr(EINVAL, "Error:: unexpected EOM at Password %d", len); 381: flg.reserved = 1; 382: return flg; 383: } else { 384: if (psPass && passLen) { 385: memset(psPass, 0, passLen--); 386: memcpy(psPass, var->var_data, 387: ntohs(var->var_sb.val) > passLen ? passLen : ntohs(var->var_sb.val)); 388: } 389: pos += MQTTHDR_VAR_SIZEOF(var); 390: } 391: } 392: 393: return flg; 394: }