diff --git a/lib/mqtt_msg.c b/lib/mqtt_msg.c
index 5931375..589cb46 100644
--- a/lib/mqtt_msg.c
+++ b/lib/mqtt_msg.c
@@ -49,7 +49,7 @@ struct __attribute((__packed__)) mqtt_connect_variable_header
 {
     uint8_t lengthMsb;
     uint8_t lengthLsb;
-#if defined(CONFIG_MQTT_PROTOCOL_311)
+#if defined(MQTT_PROTOCOL_311)
     uint8_t magic[4];
 #else
     uint8_t magic[6];
diff --git a/mqtt_client.c b/mqtt_client.c
index 281bef5..c4f4cb2 100644
--- a/mqtt_client.c
+++ b/mqtt_client.c
@@ -125,14 +125,13 @@ static esp_err_t esp_mqtt_set_config(esp_mqtt_client_handle_t client, const esp_
         client->connect_info.will_topic = strdup(config->lwt_topic);
     }
 
-    if (config->lwt_msg[0]) {
+    if (config->lwt_msg_len) {
+        client->connect_info.will_message = malloc(config->lwt_msg_len);
+        mem_assert(client->connect_info.will_message);
+        memcpy(client->connect_info.will_message, config->lwt_msg, config->lwt_msg_len);
+    } else if (config->lwt_msg[0]) {
         client->connect_info.will_message = strdup(config->lwt_msg);
-        if (config->lwt_msg_len > 0) {
-            client->connect_info.will_length = config->lwt_msg_len;
-        }
-        else {
-    	    client->connect_info.will_length = strlen(config->lwt_msg);
-        }
+        client->connect_info.will_length = strlen(config->lwt_msg);
     }
 
     client->connect_info.will_qos = config->lwt_qos;