diff --git a/Adafruit_MQTT.cpp b/Adafruit_MQTT.cpp index 96bf77e..990e2ac 100644 --- a/Adafruit_MQTT.cpp +++ b/Adafruit_MQTT.cpp @@ -51,7 +51,7 @@ static uint8_t *stringprint(uint8_t *p, char *s) { uint16_t len = strlen(s); p[0] = len >> 8; p++; p[0] = len & 0xFF; p++; - memcpy(p, s, len); + memmove(p, s, len); return p+len; } */ @@ -240,17 +240,45 @@ bool Adafruit_MQTT::publish(const char *topic, const char *data, uint8_t qos) { len = readPacket(buffer, 4, PUBLISH_TIMEOUT_MS); DEBUG_PRINT(F("Publish QOS1+ reply:\t")); DEBUG_PRINTBUFFER(buffer, len); - if (len != 4) + if (len != 4) return false; - if ((buffer[0] >> 4) != MQTT_CTRL_PUBACK) + if ((buffer[0] >> 4) != MQTT_CTRL_PUBACK) return false; uint16_t packnum = buffer[2]; packnum <<= 8; packnum |= buffer[3]; // we increment the packet_id_counter right after publishing so inc here too to match - packnum++; - if (packnum != packet_id_counter) + packnum++; + if (packnum != packet_id_counter) + return false; + } + + return true; +} + +bool Adafruit_MQTT::publish(const char *topic, uint8_t *sData, uint8_t bLen, uint8_t qos) { + // Construct and send publish packet. + uint8_t len = publishPacket(buffer, topic, sData, bLen, qos); + if (!sendPacket(buffer, len)) + return false; + + // If QOS level is high enough verify the response packet. + if (qos > 0) { + len = readPacket(buffer, 4, PUBLISH_TIMEOUT_MS); + DEBUG_PRINT(F("Publish QOS1+ reply:\t")); + DEBUG_PRINTBUFFER(buffer, len); + if (len != 4) + return false; + if ((buffer[0] >> 4) != MQTT_CTRL_PUBACK) + return false; + uint16_t packnum = buffer[2]; + packnum <<= 8; + packnum |= buffer[3]; + + // we increment the packet_id_counter right after publishing so inc here too to match + packnum++; + if (packnum != packet_id_counter) return false; } @@ -378,7 +406,7 @@ Adafruit_MQTT_Subscribe *Adafruit_MQTT::readSubscription(int16_t timeout) { datalen = SUBSCRIPTIONDATALEN-1; // cut it off } // extract out just the data, into the subscription object itself - memcpy(subscriptions[i]->lastread, buffer+4+topiclen, datalen); + memmove(subscriptions[i]->lastread, buffer+4+topiclen, datalen); subscriptions[i]->datalen = datalen; DEBUG_PRINT(F("Data len: ")); DEBUG_PRINTLN(datalen); DEBUG_PRINT(F("Data: ")); DEBUG_PRINTLN((char *)subscriptions[i]->lastread); @@ -401,13 +429,13 @@ bool Adafruit_MQTT::ping(uint8_t num) { uint8_t len = pingPacket(buffer); if (!sendPacket(buffer, len)) continue; - + // Process ping reply. len = readPacket(buffer, 2, PING_TIMEOUT_MS); if (buffer[0] == (MQTT_CTRL_PINGRESP << 4)) return true; } - + return false; } @@ -502,8 +530,17 @@ uint8_t Adafruit_MQTT::connectPacket(uint8_t *packet) { } // as per http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040 -uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic, +uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic, const char *data, uint8_t qos) { + + return publishPacket(packet, topic, (uint8_t*)(data), strlen(data), qos); + +} + + +// as per http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040 +uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic, + uint8_t *data, uint8_t bLen, uint8_t qos) { uint8_t *p = packet; uint16_t len; @@ -524,16 +561,17 @@ uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic, packet_id_counter++; } - memcpy(p, data, strlen(data)); - p+=strlen(data); + memmove(p, data, bLen); + p+= bLen; len = p - packet; packet[1] = len-2; // don't include the 2 bytes of fixed header data DEBUG_PRINTLN(F("MQTT publish packet:")); DEBUG_PRINTBUFFER(buffer, len); return len; + } -uint8_t Adafruit_MQTT::subscribePacket(uint8_t *packet, const char *topic, +uint8_t Adafruit_MQTT::subscribePacket(uint8_t *packet, const char *topic, uint8_t qos) { uint8_t *p = packet; uint16_t len; @@ -562,6 +600,8 @@ uint8_t Adafruit_MQTT::subscribePacket(uint8_t *packet, const char *topic, return len; } + + uint8_t Adafruit_MQTT::unsubscribePacket(uint8_t *packet, const char *topic) { uint8_t *p = packet; @@ -607,7 +647,7 @@ uint8_t Adafruit_MQTT::disconnectPacket(uint8_t *packet) { // Adafruit_MQTT_Publish Definition //////////////////////////////////////////// -Adafruit_MQTT_Publish::Adafruit_MQTT_Publish(Adafruit_MQTT *mqttserver, +Adafruit_MQTT_Publish::Adafruit_MQTT_Publish(Adafruit_MQTT *mqttserver, const char *feed, uint8_t q) { mqtt = mqttserver; topic = feed; @@ -643,10 +683,15 @@ bool Adafruit_MQTT_Publish::publish(const char *payload) { return mqtt->publish(topic, payload, qos); } +bool Adafruit_MQTT_Publish::publish(uint8_t *payload, uint8_t bLen) { + + return mqtt->publish(topic, payload, bLen, qos); +} + // Adafruit_MQTT_Subscribe Definition ////////////////////////////////////////// -Adafruit_MQTT_Subscribe::Adafruit_MQTT_Subscribe(Adafruit_MQTT *mqttserver, +Adafruit_MQTT_Subscribe::Adafruit_MQTT_Subscribe(Adafruit_MQTT *mqttserver, const char *feed, uint8_t q) { mqtt = mqttserver; topic = feed; diff --git a/Adafruit_MQTT.h b/Adafruit_MQTT.h index b3ad71a..3dbbac5 100644 --- a/Adafruit_MQTT.h +++ b/Adafruit_MQTT.h @@ -160,6 +160,7 @@ class Adafruit_MQTT { // The topic must be stored in PROGMEM. It can either be a // char*, or a __FlashStringHelper* (the result of the F() macro). bool publish(const char *topic, const char *payload, uint8_t qos = 0); + bool publish(const char *topic, uint8_t *payload, uint8_t bLen, uint8_t qos = 0); bool publish(const __FlashStringHelper *topic, const char *payload, uint8_t qos = 0) { return publish((const char *)topic, payload, qos); } @@ -223,6 +224,7 @@ class Adafruit_MQTT { uint8_t connectPacket(uint8_t *packet); uint8_t disconnectPacket(uint8_t *packet); uint8_t publishPacket(uint8_t *packet, const char *topic, const char *payload, uint8_t qos); + uint8_t publishPacket(uint8_t *packet, const char *topic, uint8_t *payload, uint8_t bLen, uint8_t qos); uint8_t subscribePacket(uint8_t *packet, const char *topic, uint8_t qos); uint8_t unsubscribePacket(uint8_t *packet, const char *topic); uint8_t pingPacket(uint8_t *packet); @@ -239,6 +241,8 @@ class Adafruit_MQTT_Publish { // This might be ignored and a higher precision value sent. bool publish(int32_t i); bool publish(uint32_t i); + bool publish(uint8_t *b, uint8_t bLen); + private: Adafruit_MQTT *mqtt;