Adafruit_MQTT::publishPacket: Protect against memory corruption.

Fixes adafruit#109
Fixes adafruit#122

After spending a long time pulling my hair to understand why I was
hitting a panic when attempting to read from my registered
subscriptions, I found out that the subscriptions member of the
Adafruit_MQTT instance was corrupted. :(

Turns out the memory corruption was caused by my publish
call, where the payload I was providing was bigger than the
allocated space in the buffer for construction of the packet
(see buffer[MAXBUFFERSIZE]).

To protect myself from ever making this mistake again, I am
proposing a simple logic in publishPacket where instead of
silently corrupting memory, the code uses as much payload
as it can fit in the available space. By seeing the
truncated payload, user can decide whether he/she should
1)break it up into different topics, 2) put the payload on
a diet, or 3) increase MAXBUFFERSIZE.

Adafruit_MQTT.cpp

@@ -303,7 +303,7 @@ 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, (uint16_t) sizeof(buffer), topic, data, bLen, qos);
   if (!sendPacket(buffer, len))
     return false;
 
@@ -639,7 +639,7 @@ 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
-uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
+uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, uint16_t maxPacketLen, const char *topic,
                                      uint8_t *data, uint16_t bLen, uint8_t qos) {
   uint8_t *p = packet;
   uint16_t len=0;
@@ -650,7 +650,20 @@ uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
   if(qos > 0) { 
     len += 2; // qos packet id
   }
-  len += bLen; // payload length
+
+  // payload length
+  if (len + bLen <= maxPacketLen) {
+    len += bLen;
+  } 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.
+    const bool extraByteNeeded = maxPacketLen % 128 != 0;
+    bLen = maxPacketLen - (len + maxPacketLen / 128 + (extraByteNeeded ? 1 : 0));
+    len = maxPacketLen;
+  }
 
   // Now you can start generating the packet!
   p[0] = MQTT_CTRL_PUBLISH << 4 | qos << 1;
@@ -683,8 +696,9 @@ uint16_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
 
   memmove(p, data, bLen);
   p+= bLen;
-  len = p - packet;
-  DEBUG_PRINTLN(F("MQTT publish packet:"));
+  len = (p - 1) - packet;
+  DEBUG_PRINT(F("MQTT publish packet (length: "));
+  DEBUG_PRINT(len); DEBUG_PRINTLN(F("):"));
   DEBUG_PRINTBUFFER(buffer, len);
   return len;
 }

Adafruit_MQTT.h

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