diff --git a/mongoose.c b/mongoose.c
index c9e9becb52537640ee3129c4ad39f802290d1269..c7056262f585c6c15112ffb3fdbdf935a454856d 100644
--- a/mongoose.c
+++ b/mongoose.c
@@ -9856,6 +9856,29 @@ void mg_set_protocol_mqtt(struct mg_connection *nc) {
   nc->proto_data_destructor = mg_mqtt_proto_data_destructor;
 }
 
+static void mg_mqtt_prepend_header(struct mg_connection *nc, uint8_t cmd,
+                                   uint8_t flags, size_t len) {
+  size_t off = nc->send_mbuf.len - len;
+  uint8_t header = cmd << 4 | (uint8_t) flags;
+
+  uint8_t buf[1 + sizeof(size_t)];
+  uint8_t *vlen = &buf[1];
+
+  assert(nc->send_mbuf.len >= len);
+
+  buf[0] = header;
+
+  /* mqtt variable length encoding */
+  do {
+    *vlen = len % 0x80;
+    len /= 0x80;
+    if (len > 0) *vlen |= 0x80;
+    vlen++;
+  } while (len > 0);
+
+  mbuf_insert(&nc->send_mbuf, off, buf, vlen - buf);
+}
+
 void mg_send_mqtt_handshake(struct mg_connection *nc, const char *client_id) {
   static struct mg_send_mqtt_handshake_opts opts;
   mg_send_mqtt_handshake_opt(nc, client_id, opts);
@@ -9863,98 +9886,74 @@ void mg_send_mqtt_handshake(struct mg_connection *nc, const char *client_id) {
 
 void mg_send_mqtt_handshake_opt(struct mg_connection *nc, const char *client_id,
                                 struct mg_send_mqtt_handshake_opts opts) {
-  uint8_t header = MG_MQTT_CMD_CONNECT << 4;
-  uint8_t rem_len;
-  uint16_t keep_alive;
-  uint16_t len;
+  uint16_t hlen, nlen, rem_len = 0;
   struct mg_mqtt_proto_data *pd = (struct mg_mqtt_proto_data *) nc->proto_data;
 
-  /*
-   * 9: version_header(len, magic_string, version_number), 1: flags, 2:
-   * keep-alive timer,
-   * 2: client_identifier_len, n: client_id
-   */
-  rem_len = 9 + 1 + 2 + 2 + (uint8_t) strlen(client_id);
+  mg_send(nc, "\00\04MQTT\04", 7);
+  rem_len += 7;
 
   if (opts.user_name != NULL) {
     opts.flags |= MG_MQTT_HAS_USER_NAME;
-    rem_len += (uint8_t) strlen(opts.user_name) + 2;
   }
   if (opts.password != NULL) {
     opts.flags |= MG_MQTT_HAS_PASSWORD;
-    rem_len += (uint8_t) strlen(opts.password) + 2;
   }
   if (opts.will_topic != NULL && opts.will_message != NULL) {
     opts.flags |= MG_MQTT_HAS_WILL;
-    rem_len += (uint8_t) strlen(opts.will_topic) + 2;
-    rem_len += (uint8_t) strlen(opts.will_message) + 2;
   }
-
-  mg_send(nc, &header, 1);
-  mg_send(nc, &rem_len, 1);
-  mg_send(nc, "\00\06MQIsdp\03", 9);
-  mg_send(nc, &opts.flags, 1);
-
   if (opts.keep_alive == 0) {
     opts.keep_alive = 60;
   }
 
-  keep_alive = htons(opts.keep_alive);
-  mg_send(nc, &keep_alive, 2);
+  mg_send(nc, &opts.flags, 1);
+  rem_len += 1;
+
+  nlen = htons(opts.keep_alive);
+  mg_send(nc, &nlen, 2);
+  rem_len += 2;
 
-  len = htons((uint16_t) strlen(client_id));
-  mg_send(nc, &len, 2);
-  mg_send(nc, client_id, strlen(client_id));
+  hlen = strlen(client_id);
+  nlen = htons((uint16_t) hlen);
+  mg_send(nc, &nlen, 2);
+  mg_send(nc, client_id, hlen);
+  rem_len += 2 + hlen;
 
   if (opts.flags & MG_MQTT_HAS_WILL) {
-    len = htons((uint16_t) strlen(opts.will_topic));
-    mg_send(nc, &len, 2);
-    mg_send(nc, opts.will_topic, strlen(opts.will_topic));
+    hlen = strlen(opts.will_topic);
+    nlen = htons((uint16_t) hlen);
+    mg_send(nc, &nlen, 2);
+    mg_send(nc, opts.will_topic, hlen);
+    rem_len += 2 + hlen;
 
-    len = htons((uint16_t) strlen(opts.will_message));
-    mg_send(nc, &len, 2);
-    mg_send(nc, opts.will_message, strlen(opts.will_message));
+    hlen = strlen(opts.will_message);
+    nlen = htons((uint16_t) nlen);
+    mg_send(nc, &nlen, 2);
+    mg_send(nc, opts.will_message, hlen);
+    rem_len += 2 + hlen;
   }
 
   if (opts.flags & MG_MQTT_HAS_USER_NAME) {
-    len = htons((uint16_t) strlen(opts.user_name));
-    mg_send(nc, &len, 2);
-    mg_send(nc, opts.user_name, strlen(opts.user_name));
+    hlen = strlen(opts.user_name);
+    nlen = htons((uint16_t) hlen);
+    mg_send(nc, &nlen, 2);
+    mg_send(nc, opts.user_name, hlen);
+    rem_len += 2 + hlen;
   }
   if (opts.flags & MG_MQTT_HAS_PASSWORD) {
-    len = htons((uint16_t) strlen(opts.password));
-    mg_send(nc, &len, 2);
-    mg_send(nc, opts.password, strlen(opts.password));
+    hlen = strlen(opts.password);
+    nlen = htons((uint16_t) hlen);
+    mg_send(nc, &nlen, 2);
+    mg_send(nc, opts.password, hlen);
+    rem_len += 2 + hlen;
   }
 
+  mg_mqtt_prepend_header(nc, MG_MQTT_CMD_CONNECT, 0, rem_len);
+
   if (pd != NULL) {
     pd->keep_alive = opts.keep_alive;
   }
 }
 
-static void mg_mqtt_prepend_header(struct mg_connection *nc, uint8_t cmd,
-                                   uint8_t flags, size_t len) {
-  size_t off = nc->send_mbuf.len - len;
-  uint8_t header = cmd << 4 | (uint8_t) flags;
-
-  uint8_t buf[1 + sizeof(size_t)];
-  uint8_t *vlen = &buf[1];
-
-  assert(nc->send_mbuf.len >= len);
-
-  buf[0] = header;
-
-  /* mqtt variable length encoding */
-  do {
-    *vlen = len % 0x80;
-    len /= 0x80;
-    if (len > 0) *vlen |= 0x80;
-    vlen++;
-  } while (len > 0);
-
-  mbuf_insert(&nc->send_mbuf, off, buf, vlen - buf);
-}
-
 void mg_mqtt_publish(struct mg_connection *nc, const char *topic,
                      uint16_t message_id, int flags, const void *data,
                      size_t len) {