commit 8fc7687447a2714a98d43ed885c87ab56476172f
Author: Deomid "rojer" Ryabkov <rojer@rojer.me>
Date:   Fri Oct 6 23:32:49 2023 +0200

    apps/mqtt: New incoming message parser
    
     * Supporting TCP segmentation
     * Zero copy in most cases

diff --git a/src/apps/mqtt/mqtt.c b/src/apps/mqtt/mqtt.c
index 33c89add..9f3c00a9 100644
--- a/src/apps/mqtt/mqtt.c
+++ b/src/apps/mqtt/mqtt.c
@@ -552,6 +552,12 @@ mqtt_close(mqtt_client_t *client, mqtt_connection_status_t reason)
     client->conn = NULL;
   }
 
+  /* Free any buffered input packets */
+  if (client->in_pkt != NULL) {
+    pbuf_free(client->in_pkt);
+    client->in_pkt = NULL;
+  }
+
   /* Remove all pending requests */
   mqtt_clear_requests(&client->pend_req_queue);
   /* Stop cyclic timer */
@@ -634,13 +640,15 @@ static err_t
 pub_ack_rec_rel_response(mqtt_client_t *client, u8_t msg, u16_t pkt_id, u8_t qos)
 {
   err_t err = ERR_OK;
+  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("%s: Sending %s with pkt_id: %d\n",
+                                 "pub_ack_rec_rel_response", mqtt_msg_type_to_str(msg), pkt_id));
   if (mqtt_output_check_space(&client->output, 2)) {
     mqtt_output_append_fixed_header(&client->output, msg, 0, qos, 0, 2);
     mqtt_output_append_u16(&client->output, pkt_id);
     mqtt_output_send(&client->output, client->conn);
   } else {
-    LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("pub_ack_rec_rel_response: OOM creating response: %s with pkt_id: %d\n",
-                                   mqtt_msg_type_to_str(msg), pkt_id));
+    LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("%s: OOM creating response: %s with pkt_id: %d\n",
+                                   "pub_ack_rec_rel_response", mqtt_msg_type_to_str(msg), pkt_id));
     err = ERR_MEM;
   }
   return err;
@@ -659,274 +667,291 @@ mqtt_incoming_suback(struct mqtt_request_t *r, u8_t result)
   }
 }
 
-
 /**
- * Complete MQTT message received or buffer full
- * @param client MQTT client
- * @param fixed_hdr_len length of fixed header
- * @param length length received part
- * @param remaining_length Remaining length of complete message
+ * Parse the fixed header of an incoming MQTT message
+ * @param msg Incoming message parsing state
+ * @param p Incoming packet(s)
+ * @return Number of bytes consumed or < 0 if an error occurs
  */
-static mqtt_connection_status_t
-mqtt_message_received(mqtt_client_t *client, u8_t fixed_hdr_len, u16_t length, u32_t remaining_length,
-                      u8_t *var_hdr_payload)
+static int
+mqtt_parse_incoming_fixed_header(struct mqtt_msg_state_t *msg, struct pbuf *p)
 {
-  mqtt_connection_status_t res = MQTT_CONNECT_ACCEPTED;
+  int i = 0, done = 0;
+  u8_t msg_type;
 
-  /* Control packet type */
-  u8_t pkt_type = MQTT_CTL_PACKET_TYPE(client->rx_buffer[0]);
-  u16_t pkt_id = 0;
+  LWIP_ASSERT("mqtt_parse_incoming_fixed_header: state != MQTT_MSG_RECV_FIXED_HEADER", msg->state == MQTT_MSG_RECV_FIXED_HEADER);
 
-  LWIP_ASSERT("fixed_hdr_len <= client->msg_idx", fixed_hdr_len <= client->msg_idx);
-  LWIP_ERROR("buffer length mismatch", fixed_hdr_len + length <= MQTT_VAR_HEADER_BUFFER_LEN,
-             return MQTT_CONNECT_DISCONNECTED);
+  if (p == NULL || p->tot_len < 2) return 0;
 
-  if (pkt_type == MQTT_MSG_TYPE_CONNACK) {
-    if (client->conn_state == MQTT_CONNECTING) {
-      if (length < 2) {
-        LWIP_DEBUGF(MQTT_DEBUG_WARN,( "mqtt_message_received: Received short CONNACK message\n"));
-        goto out_disconnect;
+  msg->ctl_byte = pbuf_get_at(p, i++);
+  msg_type = MQTT_CTL_PACKET_TYPE(msg->ctl_byte);
+  msg->rem_len = 0;
+  do {
+    const u32_t b = pbuf_get_at(p, i++);
+    done = !(b & 0x80);
+    msg->rem_len |= (b << (i - 2) * 7);
+  } while (!done && i < p->tot_len);
+  if (!done) return 0;
+  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "%s: Received %s, rem_len %"U32_F"\n",
+                                  "mqtt_parse_incoming_fixed_header", mqtt_msg_type_to_str(msg_type), msg->rem_len));
+  msg->rem_len += i; /* Return value will be deducted from rem_len, account for that. */
+  switch (msg_type) {
+    case MQTT_MSG_TYPE_PUBLISH:
+    case MQTT_MSG_TYPE_CONNACK:
+    case MQTT_MSG_TYPE_PUBACK:
+    case MQTT_MSG_TYPE_PUBREC:
+    case MQTT_MSG_TYPE_PUBREL:
+    case MQTT_MSG_TYPE_PUBCOMP:
+    case MQTT_MSG_TYPE_SUBACK:
+    case MQTT_MSG_TYPE_UNSUBACK:
+      if (msg->rem_len < 2) {
+        LWIP_DEBUGF(MQTT_DEBUG_WARN, ( "%s: Received message type %s is too short\n",
+                                       "mqtt_parse_incoming_fixed_header", mqtt_msg_type_to_str(msg_type)));
+        return -2;
       }
-      /* Get result code from CONNACK */
-      res = (mqtt_connection_status_t)var_hdr_payload[1];
-      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: Connect response code %d\n", res));
-      if (res == MQTT_CONNECT_ACCEPTED) {
-        /* Reset cyclic_tick when changing to connected state */
-        client->cyclic_tick = 0;
+      msg->state = MQTT_MSG_RECV_VAR_HEADER;
+      break;
+    case MQTT_MSG_TYPE_PINGRESP:
+      msg->state = MQTT_MSG_RECV_INIT;
+      break;
+    default:
+      LWIP_DEBUGF(MQTT_DEBUG_WARN, ( "%s: Received unsupported message type: %u\n",
+                                     "mqtt_parse_incoming_fixed_header", msg_type));
+      return -1;
+  }
+  return i;
+}
+
+/**
+ * Parse the variable length header of an incoming MQTT message
+ * @param msg Incoming message parsing state
+ * @param p Incoming packet(s)
+ * @return Number of bytes consumed or < 0 if an error occurs.
+ */
+static int
+mqtt_parse_incoming_var_header(mqtt_client_t *client, struct mqtt_msg_state_t *msg, struct pbuf *p)
+{
+  int res = 2;
+  const u8_t msg_type = MQTT_CTL_PACKET_TYPE(msg->ctl_byte);
+  u16_t v;
+
+  LWIP_ASSERT("mqtt_parse_incoming_fixed_header: state != MQTT_MSG_RECV_VAR_HEADER", msg->state == MQTT_MSG_RECV_VAR_HEADER);
+
+  /* Need at least 2 bytes: connack status, packet id or topic len. */
+  if (p == NULL || p->tot_len < 2) return 0;
+
+  v = (((u16_t) pbuf_get_at(p, 0)) << 8) | pbuf_get_at(p, 1);
+  switch (msg_type) {
+    case MQTT_MSG_TYPE_CONNACK: {
+      const mqtt_connection_status_t conn_res = (mqtt_connection_status_t) (v & 0xff);
+      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "%s: %s res %u\n", "mqtt_parse_incoming_var_header", mqtt_msg_type_to_str(msg_type), conn_res));
+      if (conn_res == MQTT_CONNECT_ACCEPTED) {
         client->conn_state = MQTT_CONNECTED;
-        /* Notify upper layer */
-        if (client->connect_cb != NULL) {
-          client->connect_cb(client, client->connect_arg, res);
-        }
       }
-    } else {
-      LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Received CONNACK in connected state\n"));
+      if (client->connect_cb != NULL) {
+        client->connect_cb(client, client->connect_arg, conn_res);
+      }
+      if (conn_res != MQTT_CONNECT_ACCEPTED) {
+        res = -(100 + conn_res);
+        break;
+      }
+      msg->state = MQTT_MSG_RECV_INIT;
+      break;
     }
-  } else if (pkt_type == MQTT_MSG_TYPE_PINGRESP) {
-    LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "mqtt_message_received: Received PINGRESP from server\n"));
-
-  } else if (pkt_type == MQTT_MSG_TYPE_PUBLISH) {
-    u16_t payload_offset = 0;
-    u16_t payload_length = length;
-    u8_t qos = MQTT_CTL_PACKET_QOS(client->rx_buffer[0]);
-
-    if (client->msg_idx == (u32_t)(fixed_hdr_len + length)) {
-      /* First publish message frame. Should have topic and pkt id*/
-      size_t var_hdr_payload_bufsize = sizeof(client->rx_buffer) - fixed_hdr_len;
-      u8_t *topic;
-      u16_t after_topic;
-      u8_t bkp;
-      u16_t topic_len;
-      u16_t qos_len = (qos ? 2U : 0U);
-      if (length < 2 + qos_len) {
-        LWIP_DEBUGF(MQTT_DEBUG_WARN,( "mqtt_message_received: Received short PUBLISH packet\n"));
-        goto out_disconnect;
+    case MQTT_MSG_TYPE_PUBLISH: {
+      const u16_t topic_len = v;
+      const u8_t qos = MQTT_CTL_PACKET_QOS(msg->ctl_byte);
+      const u16_t var_hdr_len = 2 + topic_len + (qos > 0 ? 2 : 0);
+      char topic_buf[MQTT_VAR_HEADER_BUFFER_LEN];
+      char *topic, backup;
+      u32_t data_len;
+      if (topic_len >= MQTT_VAR_HEADER_BUFFER_LEN) {
+        LWIP_DEBUGF(MQTT_DEBUG_WARN, ( "%s: %s topic is too long (%u)\n",
+                                       "mqtt_parse_incoming_var_header", mqtt_msg_type_to_str(msg_type), topic_len));
+        res = -3;
+        break;
       }
-      topic_len = var_hdr_payload[0];
-      topic_len = (topic_len << 8) + (u16_t)(var_hdr_payload[1]);
-      if ((topic_len > length - (2 + qos_len)) ||
-          (topic_len > var_hdr_payload_bufsize - (2 + qos_len))) {
-        LWIP_DEBUGF(MQTT_DEBUG_WARN,( "mqtt_message_received: Received short PUBLISH packet (topic)\n"));
-        goto out_disconnect;
+      if (qos == 3) {
+        res = -4;
+        break;
       }
-
-      topic = var_hdr_payload + 2;
-      after_topic = 2 + topic_len;
-      /* Check buffer length, add one byte even for QoS 0 so that zero termination will fit */
-      if ((after_topic + (qos ? 2U : 1U)) > var_hdr_payload_bufsize) {
-        LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Receive buffer can not fit topic + pkt_id\n"));
-        goto out_disconnect;
+      if (p->tot_len < var_hdr_len) {
+        res = 0;
+        break;
       }
-
-      /* id for QoS 1 and 2 */
+      topic = (char *) pbuf_get_contiguous(p, topic_buf, sizeof(topic_buf), topic_len, 2);
+      /* It can't fail at this point, we've checked the length above. */
+      LWIP_ASSERT("mqtt_parse_incoming: topic != NULL", topic != NULL);
+      backup = topic[topic_len];
+      topic[topic_len] = '\0';
       if (qos > 0) {
-        if (length < after_topic + 2U) {
-          LWIP_DEBUGF(MQTT_DEBUG_WARN,( "mqtt_message_received: Received short PUBLISH packet (after_topic)\n"));
-          goto out_disconnect;
-        }
-        client->inpub_pkt_id = ((u16_t)var_hdr_payload[after_topic] << 8) + (u16_t)var_hdr_payload[after_topic + 1];
-        after_topic += 2;
-      } else {
-        client->inpub_pkt_id = 0;
+        const u16_t pkt_id_off = var_hdr_len - 2;
+        msg->pkt_id = (((u16_t) pbuf_get_at(p, pkt_id_off)) << 8) | pbuf_get_at(p, pkt_id_off + 1);
       }
-      /* Take backup of byte after topic */
-      bkp = topic[topic_len];
-      /* Zero terminate string */
-      topic[topic_len] = 0;
-      /* Payload data remaining in receive buffer */
-      payload_length = length - after_topic;
-      payload_offset = after_topic;
-
-      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_incoming_publish: Received message with QoS %d at topic: %s, payload length %"U32_F"\n",
-                                     qos, topic, remaining_length + payload_length));
+      data_len = msg->rem_len - var_hdr_len;
+      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "%s: %s topic %s qos %u pkt_id %u data_len %"U32_F"\n",
+                                      "mqtt_parse_incoming_var_header", mqtt_msg_type_to_str(msg_type),
+                                      topic, qos, msg->pkt_id, data_len ));
       if (client->pub_cb != NULL) {
-        client->pub_cb(client->inpub_arg, (const char *)topic, remaining_length + payload_length);
+        client->pub_cb(client->inpub_arg, topic, data_len);
       }
-      /* Restore byte after topic */
-      topic[topic_len] = bkp;
+      topic[topic_len] = backup;
+      msg->state = MQTT_MSG_RECV_PAYLOAD;
+      res = var_hdr_len;
+      break;
     }
-    if (payload_length > 0 || remaining_length == 0) {
-      if (length < (size_t)(payload_offset + payload_length)) {
-        LWIP_DEBUGF(MQTT_DEBUG_WARN,( "mqtt_message_received: Received short packet (payload)\n"));
-        goto out_disconnect;
+    case MQTT_MSG_TYPE_PUBREC:
+      if (pub_ack_rec_rel_response(client, MQTT_MSG_TYPE_PUBREL, v, 1) != ERR_OK) {
+        res = -7;
+        break;
       }
-      if (client->data_cb != NULL) {
-        client->data_cb(client->inpub_arg, var_hdr_payload + payload_offset, payload_length, remaining_length == 0 ? MQTT_DATA_FLAG_LAST : 0);
-      }
-      /* Reply if QoS > 0 */
-      if (remaining_length == 0 && qos > 0) {
-        /* Send PUBACK for QoS 1 or PUBREC for QoS 2 */
-        u8_t resp_msg = (qos == 1) ? MQTT_MSG_TYPE_PUBACK : MQTT_MSG_TYPE_PUBREC;
-        LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_incoming_publish: Sending publish response: %s with pkt_id: %d\n",
-                                       mqtt_msg_type_to_str(resp_msg), client->inpub_pkt_id));
-        pub_ack_rec_rel_response(client, resp_msg, client->inpub_pkt_id, 0);
+      msg->state = MQTT_MSG_RECV_INIT;
+      break;
+    case MQTT_MSG_TYPE_PUBREL:
+      if (pub_ack_rec_rel_response(client, MQTT_MSG_TYPE_PUBCOMP, v, 0) != ERR_OK) {
+        res = -8;
+        break;
       }
-    }
-  } else {
-    if (length < 2) {
-      LWIP_DEBUGF(MQTT_DEBUG_WARN,( "mqtt_message_received: Received short message\n"));
-      goto out_disconnect;
-    }
-    /* Get packet identifier */
-    pkt_id = (u16_t)var_hdr_payload[0] << 8;
-    pkt_id |= (u16_t)var_hdr_payload[1];
-    if (pkt_id == 0) {
-      LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: Got message with illegal packet identifier: 0\n"));
-      goto out_disconnect;
-    }
-    if (pkt_type == MQTT_MSG_TYPE_PUBREC) {
-      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: PUBREC, sending PUBREL with pkt_id: %d\n", pkt_id));
-      pub_ack_rec_rel_response(client, MQTT_MSG_TYPE_PUBREL, pkt_id, 1);
-
-    } else if (pkt_type == MQTT_MSG_TYPE_PUBREL) {
-      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: PUBREL, sending PUBCOMP response with pkt_id: %d\n", pkt_id));
-      pub_ack_rec_rel_response(client, MQTT_MSG_TYPE_PUBCOMP, pkt_id, 0);
-
-    } else if (pkt_type == MQTT_MSG_TYPE_SUBACK || pkt_type == MQTT_MSG_TYPE_UNSUBACK ||
-               pkt_type == MQTT_MSG_TYPE_PUBCOMP || pkt_type == MQTT_MSG_TYPE_PUBACK) {
-      struct mqtt_request_t *r = mqtt_take_request(&client->pend_req_queue, pkt_id);
-      if (r != NULL) {
-        LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_message_received: %s response with id %d\n", mqtt_msg_type_to_str(pkt_type), pkt_id));
-        if (pkt_type == MQTT_MSG_TYPE_SUBACK) {
-          if (length < 3) {
-            LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_message_received: To small SUBACK packet\n"));
-            goto out_disconnect;
-          } else {
-            mqtt_incoming_suback(r, var_hdr_payload[2]);
+      msg->state = MQTT_MSG_RECV_INIT;
+      break;
+    case MQTT_MSG_TYPE_SUBACK:
+    case MQTT_MSG_TYPE_PUBACK:
+    case MQTT_MSG_TYPE_PUBCOMP:
+    case MQTT_MSG_TYPE_UNSUBACK: {
+      msg->pkt_id = v;
+      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "%s: %s pkt_id %u\n",
+                                      "mqtt_parse_incoming_var_header", mqtt_msg_type_to_str(msg_type), v ));
+      if (msg_type == MQTT_MSG_TYPE_SUBACK) {
+        if (msg->rem_len == 3) {
+          msg->state = MQTT_MSG_RECV_PAYLOAD;
+        } else {
+          res = -5;
+        }
+      } else {
+        struct mqtt_request_t *r = mqtt_take_request(&client->pend_req_queue, v);
+        if (r == NULL) {
+          res = -4;
+          break;
+        }
+        if (msg->rem_len == 2) {
+          if (r->cb != NULL) {
+            r->cb(r->arg, ERR_OK);
           }
-        } else if (r->cb != NULL) {
-          r->cb(r->arg, ERR_OK);
+          msg->state = MQTT_MSG_RECV_INIT;
+        } else {
+          res = -6;
         }
         mqtt_delete_request(r);
-      } else {
-        LWIP_DEBUGF(MQTT_DEBUG_WARN, ( "mqtt_message_received: Received %s reply, with wrong pkt_id: %d\n", mqtt_msg_type_to_str(pkt_type), pkt_id));
       }
-    } else {
-      LWIP_DEBUGF(MQTT_DEBUG_WARN, ( "mqtt_message_received: Received unknown message type: %d\n", pkt_type));
-      goto out_disconnect;
+      break;
+    }
+    default: {
+      res = -10;
+      break;
     }
   }
   return res;
-out_disconnect:
-  return MQTT_CONNECT_DISCONNECTED;
 }
 
-
 /**
- * MQTT incoming message parser
- * @param client MQTT client
- * @param p PBUF chain of received data
- * @return Connection status
+ * Parse the payload of an incoming MQTT message
+ * @param msg Incoming message parsing state
+ * @param p Incoming packet(s)
+ * @return Number of bytes consumed or < 0 if an error occurs
  */
-static mqtt_connection_status_t
-mqtt_parse_incoming(mqtt_client_t *client, struct pbuf *p)
+static int
+mqtt_parse_incoming_payload(mqtt_client_t *client, struct mqtt_msg_state_t *msg, struct pbuf *p)
 {
-  u16_t in_offset = 0;
-  u32_t msg_rem_len = 0;
-  u8_t fixed_hdr_len = 0;
-  u8_t b = 0;
-
-  while (p->tot_len > in_offset) {
-    /* We ALWAYS parse the header here first. Even if the header was not
-       included in this segment, we re-parse it here by buffering it in
-       client->rx_buffer. client->msg_idx keeps track of this. */
-    if ((fixed_hdr_len < 2) || ((b & 0x80) != 0)) {
-
-      if (fixed_hdr_len < client->msg_idx) {
-        /* parse header from old pbuf (buffered in client->rx_buffer) */
-        b = client->rx_buffer[fixed_hdr_len];
-      } else {
-        /* parse header from this pbuf and save it in client->rx_buffer in case
-           it comes in segmented */
-        b = pbuf_get_at(p, in_offset++);
-        client->rx_buffer[client->msg_idx++] = b;
+  int res = 0;
+  const u8_t msg_type = MQTT_CTL_PACKET_TYPE(msg->ctl_byte);
+  LWIP_ASSERT("mqtt_parse_incoming_fixed_header: state != MQTT_MSG_RECV_PAYLOAD", msg->state == MQTT_MSG_RECV_PAYLOAD);
+
+  switch (msg_type) {
+    case MQTT_MSG_TYPE_PUBLISH: {
+      u16_t p_len;
+      u8_t *p_data, last;
+      u32_t data_len;
+      if (p == NULL && msg->rem_len > 0) break;
+      p_len = (p ? p->len : 0);
+      p_data = (uint8_t *) (p ? p->payload : NULL);
+      data_len = LWIP_MIN(p_len , msg->rem_len);
+      last = (data_len == msg->rem_len);
+      if (client->data_cb != NULL) {
+        client->data_cb(client->inpub_arg, p_data, data_len, (last ? MQTT_DATA_FLAG_LAST : 0));
       }
-      fixed_hdr_len++;
-
-      if (fixed_hdr_len >= 2) {
-        /* fixed header contains at least 2 bytes but can contain more, depending on
-           'remaining length'. All bytes but the last of this have 0x80 set to
-           indicate more bytes are coming. */
-        msg_rem_len |= (u32_t)(b & 0x7f) << ((fixed_hdr_len - 2) * 7);
-        if ((b & 0x80) == 0) {
-          /* fixed header is done */
-          LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_parse_incoming: Remaining length after fixed header: %"U32_F"\n", msg_rem_len));
-          if (msg_rem_len == 0) {
-            /* Complete message with no extra headers of payload received */
-            mqtt_message_received(client, fixed_hdr_len, 0, 0, NULL);
-            client->msg_idx = 0;
-            fixed_hdr_len = 0;
-          } else {
-            /* Bytes remaining in message (changes remaining length if this is
-               not the first segment of this message) */
-            msg_rem_len = (msg_rem_len + fixed_hdr_len) - client->msg_idx;
-          }
+      if (last) {
+        const u8_t msg_qos = MQTT_CTL_PACKET_QOS(msg->ctl_byte);
+        if (msg_qos > 0) {
+          /* Send PUBACK for QoS 1 or PUBREC for QoS 2 */
+          const u8_t resp_msg = (msg_qos == 1 ? MQTT_MSG_TYPE_PUBACK : MQTT_MSG_TYPE_PUBREC);
+          LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("%s: Sending publish response: %s with pkt_id: %d\n",
+                                         "mqtt_parse_incoming_payload", mqtt_msg_type_to_str(resp_msg), msg->pkt_id));
+          pub_ack_rec_rel_response(client, resp_msg, msg->pkt_id, 0);
         }
+        msg->state = MQTT_MSG_RECV_INIT;
       }
-    } else {
-      /* Fixed header has been parsed, parse variable header */
-      u16_t cpy_len, buffer_space;
-      u8_t *var_hdr_payload;
-      mqtt_connection_status_t res;
-
-      /* Allow to copy the lesser one of available length in input data or bytes remaining in message */
-      cpy_len = (u16_t)LWIP_MIN((u16_t)(p->tot_len - in_offset), msg_rem_len);
-
-      /* Limit to available space in buffer */
-      buffer_space = MQTT_VAR_HEADER_BUFFER_LEN - fixed_hdr_len;
-      if (cpy_len > buffer_space) {
-        cpy_len = buffer_space;
-      }
-      /* Adjust cpy_len to ensure zero-copy operation for remaining parts of current message */
-      if (client->msg_idx >= MQTT_VAR_HEADER_BUFFER_LEN) {
-        if (cpy_len > (p->len - in_offset))
-          cpy_len = p->len - in_offset;
-      }
-      var_hdr_payload = (u8_t*)pbuf_get_contiguous(p, client->rx_buffer + fixed_hdr_len,
-                                                   buffer_space, cpy_len, in_offset);
-
-      /* Advance get and put indexes  */
-      client->msg_idx += cpy_len;
-      in_offset += cpy_len;
-      msg_rem_len -= cpy_len;
-
-      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_parse_incoming: msg_idx: %"U32_F", cpy_len: %"U16_F", remaining %"U32_F"\n", client->msg_idx, cpy_len, msg_rem_len));
-      /* Whole or partial message received */
-      res = mqtt_message_received(client, fixed_hdr_len, cpy_len, msg_rem_len, var_hdr_payload);
-      if (res != MQTT_CONNECT_ACCEPTED) {
-        return res;
-      }
-      if (msg_rem_len == 0) {
-        /* Reset parser state */
-        client->msg_idx = 0;
-        /* msg_tot_len = 0; */
-        fixed_hdr_len = 0;
+      res = data_len;
+      break;
+    }
+    case MQTT_MSG_TYPE_SUBACK: {
+      struct mqtt_request_t *r;
+      if (p == NULL) break;
+      LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "%s: %s pkt_id %u\n", "mqtt_parse_incoming_payload", mqtt_msg_type_to_str(msg_type), msg->pkt_id ));
+      r = mqtt_take_request(&client->pend_req_queue, msg->pkt_id);
+      if (r == NULL) {
+        res = -4;
+        break;
       }
+      mqtt_incoming_suback(r, pbuf_get_at(p, 0));
+      mqtt_delete_request(r);
+      msg->state = MQTT_MSG_RECV_INIT;
+      res = 1;
+      break;
     }
+    default:
+      res = -10;
+      break;
   }
-  return MQTT_CONNECT_ACCEPTED;
+  return res;
 }
 
+/**
+ * Parse an incoming MQTT message
+ * @param msg Incoming message parsing state
+ * @param p Incoming packet(s)
+ * @return Number of bytes consumed or < 0 if an error occurs
+ */
+static int
+mqtt_parse_incoming(mqtt_client_t *client, struct mqtt_msg_state_t *msg, struct pbuf *p)
+{
+  /* Spec: http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html */
+  int res = 0;
+  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ( "%s: State %d tot_len %"U32_F" rem_len %"U32_F,
+                                  "mqtt_parse_incoming", msg->state, (p ? p->tot_len : 0), msg->rem_len ));
+  switch (msg->state) {
+    case MQTT_MSG_RECV_INIT:
+      memset(msg, 0, sizeof(*msg));
+      msg->state = MQTT_MSG_RECV_FIXED_HEADER;
+      /* fallthrough */
+    case MQTT_MSG_RECV_FIXED_HEADER: {
+      res = mqtt_parse_incoming_fixed_header(msg, p);
+      break;
+    }
+    case MQTT_MSG_RECV_VAR_HEADER: {
+      res = mqtt_parse_incoming_var_header(client, msg, p);
+      break;
+    }
+    case MQTT_MSG_RECV_PAYLOAD: {
+      res = mqtt_parse_incoming_payload(client, msg, p);
+      break;
+    }
+    default: {
+      res = -1;
+      break;
+    }
+  }
+  return res;
+}
 
 /**
  * TCP received callback function. @see tcp_recv_fn
@@ -939,39 +964,72 @@ static err_t
 mqtt_tcp_recv_cb(void *arg, struct altcp_pcb *pcb, struct pbuf *p, err_t err)
 {
   mqtt_client_t *client = (mqtt_client_t *)arg;
+  struct mqtt_msg_state_t *msg = &client->in_msg;
   LWIP_ASSERT("mqtt_tcp_recv_cb: client != NULL", client != NULL);
   LWIP_ASSERT("mqtt_tcp_recv_cb: client->conn == pcb", client->conn == pcb);
 
-  if (p == NULL) {
-    LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_tcp_recv_cb: Recv pbuf=NULL, remote has closed connection\n"));
+  if (p == NULL || err != ERR_OK) {
+    LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_tcp_recv_cb: Recv pbuf=NULL, remote has closed connection (%d)\n", err));
     mqtt_close(client, MQTT_CONNECT_DISCONNECTED);
-  } else {
-    mqtt_connection_status_t res;
-    if (err != ERR_OK) {
-      LWIP_DEBUGF(MQTT_DEBUG_WARN, ("mqtt_tcp_recv_cb: Recv err=%d\n", err));
-      pbuf_free(p);
-      return err;
-    }
+    if (p != NULL) pbuf_free(p);
+    return err;
+  }
+
+  /* Tell remote that data has been received */
+  altcp_recved(pcb, p->tot_len);
 
-    /* Tell remote that data has been received */
-    altcp_recved(pcb, p->tot_len);
-    res = mqtt_parse_incoming(client, p);
-    pbuf_free(p);
+  /* Flatten and put it on our incoming queue */
+  while (p != NULL) {
+    struct pbuf *q;
+    if (p->next != NULL) {
+      pbuf_ref(p->next);
+    }
+    q = pbuf_dechain(p);
+    if (client->in_pkt == NULL) {
+      client->in_pkt = p;
+    } else {
+      pbuf_cat(client->in_pkt, p);
+    }
+    p = q;
+  }
 
-    if (res != MQTT_CONNECT_ACCEPTED) {
-      mqtt_close(client, res);
+  while (1) {
+    int res, pres;
+    p = client->in_pkt;
+    res = pres = mqtt_parse_incoming(client, msg, p);
+    if (pres > 0) {
+      if (pres <= (int) msg->rem_len && pres <= p->tot_len) {
+        client->in_pkt = pbuf_free_header(p, pres);
+        msg->rem_len -= res;
+      } else {
+        res = -20;
+      }
     }
-    /* If keep alive functionality is used */
-    if (client->keep_alive != 0) {
-      /* Reset server alive watchdog */
-      client->server_watchdog = 0;
+    if (res >= 0 && msg->state == MQTT_MSG_RECV_INIT && msg->rem_len != 0) {
+      /* Not fully consumed a message - bug! */
+      res = -21;
     }
+    LWIP_DEBUGF((res < 0 ? MQTT_DEBUG_WARN : MQTT_DEBUG_TRACE),
+                ("%s: Res %d pres %d state %d rem_len %"U32_F,
+                 "mqtt_parse_incoming", res, pres, msg->state, msg->rem_len));
+    if (res < 0) {
+      mqtt_close(client, MQTT_CONNECT_DISCONNECTED);
+      break;
+    } else if (res == 0) {
+      /* Need more data */
+      break;
+    }
+  }
 
+  /* If keep alive functionality is used */
+  if (client->keep_alive != 0) {
+    /* Reset server alive watchdog */
+    client->server_watchdog = 0;
   }
+
   return ERR_OK;
 }
 
-
 /**
  * TCP data sent callback function. @see tcp_sent_fn
  * @param arg MQTT client
@@ -1058,7 +1116,7 @@ mqtt_tcp_connect_cb(void *arg, struct altcp_pcb *tpcb, err_t err)
   }
 
   /* Initiate receiver state */
-  client->msg_idx = 0;
+  client->in_msg.state = MQTT_MSG_RECV_INIT;
 
   /* Setup TCP callbacks */
   altcp_recv(tpcb, mqtt_tcp_recv_cb);
@@ -1132,7 +1190,7 @@ mqtt_publish(mqtt_client_t *client, const char *topic, const void *payload, u16_
   LWIP_ERROR("mqtt_publish: total length overflow", (total_len <= 0xFFFF), return ERR_ARG);
   remaining_length = (u16_t)total_len;
 
-  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_publish: Publish with payload length %d to topic \"%s\"\n", payload_length, topic));
+  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_publish: Publish with payload length %d to topic \"%s\", QoS %u\n", payload_length, topic, qos));
 
   r = mqtt_create_request(client->req_list, LWIP_ARRAYSIZE(client->req_list), pkt_id, cb, arg);
   if (r == NULL) {
@@ -1215,7 +1273,8 @@ mqtt_sub_unsub(mqtt_client_t *client, const char *topic, u8_t qos, mqtt_request_
     return ERR_MEM;
   }
 
-  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_sub_unsub: Client (un)subscribe to topic \"%s\", id: %d\n", topic, pkt_id));
+  LWIP_DEBUGF(MQTT_DEBUG_TRACE, ("mqtt_sub_unsub: Client %ssubscribe to topic \"%s\", QoS %u, id: %d\n",
+                                 (sub ? "" : "uns"), topic, qos, pkt_id));
 
   mqtt_output_append_fixed_header(&client->output, sub ? MQTT_MSG_TYPE_SUBSCRIBE : MQTT_MSG_TYPE_UNSUBSCRIBE, 0, 1, 0, remaining_length);
   /* Packet id */
diff --git a/src/include/lwip/apps/mqtt_priv.h b/src/include/lwip/apps/mqtt_priv.h
index b775913c..bd079227 100644
--- a/src/include/lwip/apps/mqtt_priv.h
+++ b/src/include/lwip/apps/mqtt_priv.h
@@ -66,6 +66,22 @@ struct mqtt_ringbuf_t {
   u8_t buf[MQTT_OUTPUT_RINGBUF_SIZE];
 };
 
+typedef enum
+{
+  MQTT_MSG_RECV_INIT = 0,
+  MQTT_MSG_RECV_FIXED_HEADER,
+  MQTT_MSG_RECV_VAR_HEADER,
+  MQTT_MSG_RECV_PAYLOAD
+} mqtt_msg_parse_state_t;
+
+struct mqtt_msg_state_t {
+  mqtt_msg_parse_state_t state;
+  /* Parsed fixed header data */
+  u32_t rem_len;
+  u16_t pkt_id;
+  u8_t ctl_byte;
+};
+
 /** MQTT client */
 struct mqtt_client_s
 {
@@ -91,8 +107,8 @@ struct mqtt_client_s
   mqtt_incoming_data_cb_t data_cb;
   mqtt_incoming_publish_cb_t pub_cb;
   /** Input */
-  u32_t msg_idx;
-  u8_t rx_buffer[MQTT_VAR_HEADER_BUFFER_LEN];
+  struct pbuf *in_pkt;
+  struct mqtt_msg_state_t in_msg;
   /** Output ring-buffer */
   struct mqtt_ringbuf_t output;
 };
