From 40d347936c279d96508dc2d5d7fb555fec368a7f Mon Sep 17 00:00:00 2001
From: TD-er <gijs.noorlander@gmail.com>
Date: Sat, 20 Feb 2021 14:52:18 +0100
Subject: [PATCH] [MQTT] Fix rejecting QoS=0 messages with 0 or 1 byte length

---
 lib/pubsubclient/src/PubSubClient.cpp | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/lib/pubsubclient/src/PubSubClient.cpp b/lib/pubsubclient/src/PubSubClient.cpp
index a775d252f..288bb2009 100644
--- a/lib/pubsubclient/src/PubSubClient.cpp
+++ b/lib/pubsubclient/src/PubSubClient.cpp
@@ -330,11 +330,12 @@ bool PubSubClient::loop_read() {
         case MQTTPUBLISH: 
         {
             if (callback) {
+                const bool msgId_present = (buffer[0]&0x06) == MQTTQOS1;
                 const uint16_t tl_offset = llen+1;
                 const uint16_t tl = (buffer[tl_offset]<<8)+buffer[tl_offset+1]; /* topic length in bytes */
                 const uint16_t topic_offset = tl_offset+2;
                 const uint16_t msgId_offset = topic_offset+tl;
-                const uint16_t payload_offset = msgId_offset+2;
+                const uint16_t payload_offset = msgId_present ? msgId_offset+2 : msgId_offset;
                 if (payload_offset >= MQTT_MAX_PACKET_SIZE) return false;
                 if (len < payload_offset) return false;
                 memmove(buffer+topic_offset-1,buffer+topic_offset,tl); /* move topic inside buffer 1 byte to front */
@@ -342,7 +343,7 @@ bool PubSubClient::loop_read() {
                 char *topic = (char*) buffer+topic_offset-1;
                 uint8_t *payload;
                 // msgId only present for QOS>0
-                if ((buffer[0]&0x06) == MQTTQOS1) {
+                if (msgId_present) {
                     const uint16_t msgId = (buffer[msgId_offset]<<8)+buffer[msgId_offset+1];
                     payload = buffer+payload_offset;
                     callback(topic,payload,len-payload_offset);
@@ -356,9 +357,9 @@ bool PubSubClient::loop_read() {
                         }
                     }
                 } else {
-                    // No msgId, thus payload starts 2 bytes earlier.
-                    payload = buffer+payload_offset-2;
-                    callback(topic,payload,len-(payload_offset-2));
+                    // No msgId
+                    payload = buffer+payload_offset;
+                    callback(topic,payload,len-payload_offset);
                 }
             }
             break;