From 829827d46fc484fc74876265d23ae4187f058559 Mon Sep 17 00:00:00 2001
From: cpq <valenok@gmail.com>
Date: Tue, 9 Mar 2021 09:54:02 +0000
Subject: [PATCH] Add mg_mqtt_next_unsub

---
 mongoose.c | 23 +++++++++++++++++------
 src/mqtt.c | 19 +++++++++++++++----
 2 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/mongoose.c b/mongoose.c
index a9c3b8d6..195fed9b 100644
--- a/mongoose.c
+++ b/mongoose.c
@@ -1189,7 +1189,7 @@ static size_t get_chunk_length(const char *buf, size_t len, size_t *ll) {
 static void walkchunks(struct mg_connection *c, struct mg_http_message *hm,
                        int reqlen) {
   size_t off = 0, bl, ll;
-  while (off < c->recv.len - reqlen) {
+  while (off + reqlen < c->recv.len) {
     char *buf = (char *) &c->recv.buf[reqlen];
     size_t memo = c->recv.len;
     size_t cl = get_chunk_length(&buf[off], memo - reqlen - off, &ll);
@@ -1202,7 +1202,7 @@ static void walkchunks(struct mg_connection *c, struct mg_http_message *hm,
     if (cl <= 5) {
       // Zero chunk - last one. Prepare body - cut off chunk lengths
       off = bl = 0;
-      while (off < c->recv.len - reqlen) {
+      while (off + reqlen < c->recv.len) {
         char *buf = (char *) &c->recv.buf[reqlen];
         size_t memo = c->recv.len;
         size_t cl = get_chunk_length(&buf[off], memo - reqlen - off, &ll);
@@ -2007,20 +2007,31 @@ int mg_mqtt_parse(const uint8_t *buf, size_t len, struct mg_mqtt_message *m) {
   return MQTT_OK;
 }
 
-int mg_mqtt_next_sub(struct mg_mqtt_message *msg, struct mg_str *topic,
-                     uint8_t *qos, int pos) {
+static int mg_mqtt_next_topic(struct mg_mqtt_message *msg, struct mg_str *topic,
+                              uint8_t *qos, int pos) {
   unsigned char *buf = (unsigned char *) msg->dgram.ptr + pos;
   int new_pos;
   if ((size_t) pos >= msg->dgram.len) return -1;
 
   topic->len = buf[0] << 8 | buf[1];
   topic->ptr = (char *) buf + 2;
-  new_pos = pos + 2 + topic->len + 1;
+  new_pos = pos + 2 + topic->len + (qos == NULL ? 0 : 1);
   if ((size_t) new_pos > msg->dgram.len) return -1;
-  *qos = buf[2 + topic->len];
+  if (qos != NULL) *qos = buf[2 + topic->len];
   return new_pos;
 }
 
+int mg_mqtt_next_sub(struct mg_mqtt_message *msg, struct mg_str *topic,
+                     uint8_t *qos, int pos) {
+  uint8_t tmp;
+  return mg_mqtt_next_topic(msg, topic, qos == NULL ? &tmp : qos, pos);
+}
+
+int mg_mqtt_next_unsub(struct mg_mqtt_message *msg, struct mg_str *topic,
+                       int pos) {
+  return mg_mqtt_next_topic(msg, topic, NULL, pos);
+}
+
 static void mqtt_cb(struct mg_connection *c, int ev, void *ev_data,
                     void *fn_data) {
   if (ev == MG_EV_READ) {
diff --git a/src/mqtt.c b/src/mqtt.c
index a857c197..64c6dc12 100644
--- a/src/mqtt.c
+++ b/src/mqtt.c
@@ -176,20 +176,31 @@ int mg_mqtt_parse(const uint8_t *buf, size_t len, struct mg_mqtt_message *m) {
   return MQTT_OK;
 }
 
-int mg_mqtt_next_sub(struct mg_mqtt_message *msg, struct mg_str *topic,
-                     uint8_t *qos, int pos) {
+static int mg_mqtt_next_topic(struct mg_mqtt_message *msg, struct mg_str *topic,
+                              uint8_t *qos, int pos) {
   unsigned char *buf = (unsigned char *) msg->dgram.ptr + pos;
   int new_pos;
   if ((size_t) pos >= msg->dgram.len) return -1;
 
   topic->len = buf[0] << 8 | buf[1];
   topic->ptr = (char *) buf + 2;
-  new_pos = pos + 2 + topic->len + 1;
+  new_pos = pos + 2 + topic->len + (qos == NULL ? 0 : 1);
   if ((size_t) new_pos > msg->dgram.len) return -1;
-  *qos = buf[2 + topic->len];
+  if (qos != NULL) *qos = buf[2 + topic->len];
   return new_pos;
 }
 
+int mg_mqtt_next_sub(struct mg_mqtt_message *msg, struct mg_str *topic,
+                     uint8_t *qos, int pos) {
+  uint8_t tmp;
+  return mg_mqtt_next_topic(msg, topic, qos == NULL ? &tmp : qos, pos);
+}
+
+int mg_mqtt_next_unsub(struct mg_mqtt_message *msg, struct mg_str *topic,
+                       int pos) {
+  return mg_mqtt_next_topic(msg, topic, NULL, pos);
+}
+
 static void mqtt_cb(struct mg_connection *c, int ev, void *ev_data,
                     void *fn_data) {
   if (ev == MG_EV_READ) {