uint16_t mqtt_parse_publish_msg(const uint8_t* buf, uint8_t* msg) { const uint8_t* ptr; //printf("mqtt_parse_publish_msg\n"); uint16_t msg_len = mqtt_parse_pub_msg_ptr(buf, &ptr); if(msg_len != 0 && ptr != NULL) { memcpy(msg, ptr, msg_len); } return msg_len; }
err_t MqttClient::onReceive(pbuf *buf) { if (buf == NULL) { // Disconnected, close it TcpClient::onReceive(buf); } else { if (buf->len < 1) { // Bad packet? debugf("> MQTT WRONG PACKET? (len: %d)", buf->len); close(); return ERR_OK; } int received = 0; while (received < buf->tot_len) { int type = 0; if (waitingSize == 0) { // It's begining of new packet int pos = received; if (posHeader == 0) { //debugf("start posHeader"); pbuf_copy_partial(buf, &buffer[posHeader], 1, pos); pos++; posHeader = 1; } while (posHeader > 0 && pos < buf->tot_len) { //debugf("add posHeader"); pbuf_copy_partial(buf, &buffer[posHeader], 1, pos); if ((buffer[posHeader] & 128) == 0) posHeader = 0; // Remaining Length ended else posHeader++; pos++; } if (posHeader == 0) { //debugf("start len calc"); // Remaining Length field processed uint16_t rem_len = mqtt_parse_rem_len(buffer); uint8_t rem_len_bytes = mqtt_num_rem_len_bytes(buffer); // total packet length = remaining length + byte 1 of fixed header + remaning length part of fixed header waitingSize = rem_len + rem_len_bytes + 1; type = MQTTParseMessageType(buffer); debugPrintResponseType(type, waitingSize); // Prevent overflow if (waitingSize < MQTT_MAX_BUFFER_SIZE) { current = buffer; buffer[waitingSize] = 0; } else current = NULL; } else continue; } int available = min(waitingSize, buf->tot_len - received); waitingSize -= available; if (current != NULL) { pbuf_copy_partial(buf, current, available, received); current += available; if (waitingSize == 0) { // Full packet received if(type == MQTT_MSG_PUBLISH) { const uint8_t *ptrTopic, *ptrMsg; uint16_t lenTopic, lenMsg; lenTopic = mqtt_parse_pub_topic_ptr(buffer, &ptrTopic); lenMsg = mqtt_parse_pub_msg_ptr(buffer, &ptrMsg); // Additional check for wrong packet/parsing error if (lenTopic + lenMsg < MQTT_MAX_BUFFER_SIZE) { debugf("%d: %d\n", lenTopic, lenMsg); String topic, msg; topic.setString((char*)ptrTopic, lenTopic); msg.setString((char*)ptrMsg, lenMsg); if (callback) callback(topic, msg); } else { debugf("WRONG SIZES: %d: %d", lenTopic, lenMsg); } } } } else debugf("SKIP: %d (%d)", available, waitingSize + available); // To large! received += available; } // Fire ReadyToSend callback TcpClient::onReceive(buf); } return ERR_OK; }