Adafruit_MQTT::publishPacket: Protect against memory corruption.

Avoid memory corruption from happening when data payload provided
in Adafruit_MQTT::publishPacket is greater than MAXBUFFERSIZE.

In order to do that, a helper function is being added to calculate
how much space is available for the payload after subtracting what
is used as the header.

Pull request #166
Fixes #109
Fixes #122

Signed-off-by: Flavio Fernandes <flavio@flaviof.com>

Author: flavio-fernandes

Adafruit_MQTT.cpp

@@ -317,7 +317,8 @@ bool Adafruit_MQTT::publish(const char *topic, const char *data, uint8_t qos) {
 bool Adafruit_MQTT::publish(const char *topic, uint8_t *data, uint16_t bLen,
                             uint8_t qos) {
   // Construct and send publish packet.
-  uint16_t len = publishPacket(buffer, topic, data, bLen, qos);
+  uint16_t len = publishPacket(buffer, topic, data, bLen, qos,
+			       (uint16_t) sizeof(buffer));
   if (!sendPacket(buffer, len))
     return false;
 
@@ -665,11 +666,20 @@ uint8_t Adafruit_MQTT::connectPacket(uint8_t *packet) {
   return len;
 }
 
+uint16_t Adafruit_MQTT::packetAdditionalLen(uint16_t currLen)
+{
+  /* Increase length field based on current length */
+  if (currLen < 128) return 0;
+  if (currLen < 16384) return 1;
+  if (currLen < 2097151) return 2;
+  return 3;
+}
+
 // as per
 // http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040
 uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
                                       uint8_t *data, uint16_t bLen,
-                                      uint8_t qos) {
+                                      uint8_t qos, uint16_t maxPacketLen) {
   uint8_t *p = packet;
   uint16_t len = 0;
 
@@ -679,7 +689,21 @@ uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
   if (qos > 0) {
     len += 2; // qos packet id
   }
-  len += bLen; // payload length
+  // calculate additional bytes for length field (if any)
+  uint16_t additionalLen = packetAdditionalLen(len + bLen);
+
+  // payload length
+  if (len + bLen + 2 + additionalLen <= maxPacketLen) {
+    len += bLen + additionalLen;
+  } else {
+    // If we make it here, we got a pickle: the payload is not going
+    // to fit in the packet buffer. Instead of corrupting memory, let's
+    // do something less damaging by reducing the bLen to what we are
+    // able to accomodate. Alternatively, consider using a bigger
+    // maxPacketLen.
+    bLen = maxPacketLen - (len + 2 + packetAdditionalLen(maxPacketLen));
+    len = maxPacketLen - 4;
+  }
 
   // Now you can start generating the packet!
   p[0] = MQTT_CTRL_PUBLISH << 4 | qos << 1;

Adafruit_MQTT.h

@@ -257,8 +257,9 @@ class Adafruit_MQTT {
   // Functions to generate MQTT packets.
   uint8_t connectPacket(uint8_t *packet);
   uint8_t disconnectPacket(uint8_t *packet);
+  static uint16_t packetAdditionalLen(uint16_t currLen);
   uint16_t publishPacket(uint8_t *packet, const char *topic, uint8_t *payload,
-                         uint16_t bLen, uint8_t qos);
+                         uint16_t bLen, uint8_t qos, uint16_t maxPacketLen);
   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);