diff --git a/include/mqtt.h b/include/mqtt.h
index a54e147..d4bc2da 100644
--- a/include/mqtt.h
+++ b/include/mqtt.h
@@ -6,6 +6,9 @@
 #include "mqtt_msg.h"
 #include "ringbuf.h"
 
+#if defined(CONFIG_MQTT_SECURITY_ON)
+#include "openssl/ssl.h"
+#endif
 
 typedef struct mqtt_client mqtt_client;
 typedef struct mqtt_event_data_t mqtt_event_data_t;
@@ -55,6 +58,7 @@ typedef struct mqtt_settings {
     char password[CONFIG_MQTT_MAX_PASSWORD_LEN];
     char lwt_topic[CONFIG_MQTT_MAX_LWT_TOPIC];
     char lwt_msg[CONFIG_MQTT_MAX_LWT_MSG];
+    uint32_t lwt_msg_len;
     uint32_t lwt_qos;
     uint32_t lwt_retain;
     uint32_t clean_session;
@@ -110,6 +114,7 @@ mqtt_client *mqtt_start(mqtt_settings *mqtt_info);
 void mqtt_stop();
 void mqtt_task(void *pvParameters);
 void mqtt_subscribe(mqtt_client *client, const char *topic, uint8_t qos);
+void mqtt_unsubscribe(mqtt_client *client, const char *topic);
 void mqtt_publish(mqtt_client* client, const char *topic, const char *data, int len, int qos, int retain);
 void mqtt_destroy();
 #endif
diff --git a/include/mqtt_msg.h b/include/mqtt_msg.h
index 44da0cc..758aa51 100644
--- a/include/mqtt_msg.h
+++ b/include/mqtt_msg.h
@@ -93,6 +93,7 @@ typedef struct mqtt_connect_info
   char* will_topic;
   char* will_message;
   int keepalive;
+  int will_length;
   int will_qos;
   int will_retain;
   int clean_session;
diff --git a/mqtt.c b/mqtt.c
index de3180d..9523a88 100644
--- a/mqtt.c
+++ b/mqtt.c
@@ -13,9 +13,6 @@
 #include "lwip/sockets.h"
 #include "lwip/dns.h"
 #include "lwip/netdb.h"
-#if defined(CONFIG_MQTT_SECURITY_ON)
-#include "openssl/ssl.h"
-#endif
 #include "ringbuf.h"
 #include "mqtt.h"
 
@@ -222,7 +219,7 @@ int mqtt_write(mqtt_client *client, const void *buffer, int len, int timeout_ms)
     }
 
 #if defined(CONFIG_MQTT_SECURITY_ON)
-    result = SSL_write(client->ssl, buffer, len)
+    result = SSL_write(client->ssl, buffer, len);
 #else
     result = write(client->socket, buffer, len);
 #endif
@@ -284,13 +281,19 @@ static bool mqtt_connect(mqtt_client *client)
             mqtt_info("Connected");
             return true;
         case CONNECTION_REFUSE_PROTOCOL:
+            mqtt_warn("Connection refused, bad protocol");
+            return false;
         case CONNECTION_REFUSE_SERVER_UNAVAILABLE:
+            mqtt_warn("Connection refused, server unavailable");
+            return false;
         case CONNECTION_REFUSE_BAD_USERNAME:
+            mqtt_warn("Connection refused, bad username");
+            return false;
         case CONNECTION_REFUSE_NOT_AUTHORIZED:
-            mqtt_warn("Connection refuse, reason code: %d", connect_rsp_code);
+            mqtt_warn("Connection refused, not authorized");
             return false;
         default:
-            mqtt_warn("Connection refuse, Unknow reason");
+            mqtt_warn("Connection refused, Unknow reason");
             return false;
     }
     return false;
@@ -532,6 +535,10 @@ mqtt_client *mqtt_start(mqtt_settings *settings)
     }
     memset(client, 0, sizeof(mqtt_client));
 
+    if (settings->lwt_msg_len > CONFIG_MQTT_MAX_LWT_MSG) {
+        mqtt_error("Last will message longer than CONFIG_MQTT_MAX_LWT_MSG!");
+    }
+
     client->settings = settings;
     client->connect_info.client_id = settings->client_id;
     client->connect_info.username = settings->username;
@@ -540,7 +547,7 @@ mqtt_client *mqtt_start(mqtt_settings *settings)
     client->connect_info.will_message = settings->lwt_msg;
     client->connect_info.will_qos = settings->lwt_qos;
     client->connect_info.will_retain = settings->lwt_retain;
-
+    client->connect_info.will_length = settings->lwt_msg_len;
 
     client->keepalive_tick = settings->keepalive / 2;
 
@@ -598,6 +605,16 @@ void mqtt_subscribe(mqtt_client *client, const char *topic, uint8_t qos)
     mqtt_queue(client);
 }
 
+
+void mqtt_unsubscribe(mqtt_client *client, const char *topic)
+{
+	client->mqtt_state.outbound_message = mqtt_msg_unsubscribe(&client->mqtt_state.mqtt_connection,
+	                                          topic,
+	                                          &client->mqtt_state.pending_msg_id);
+	mqtt_info("Queue unsubscribe, topic\"%s\", id: %d", topic, client->mqtt_state.pending_msg_id);
+	mqtt_queue(client);
+}
+
 void mqtt_publish(mqtt_client* client, const char *topic, const char *data, int len, int qos, int retain)
 {
 
diff --git a/mqtt_msg.c b/mqtt_msg.c
index b88ca4e..5be6fa3 100644
--- a/mqtt_msg.c
+++ b/mqtt_msg.c
@@ -328,7 +328,7 @@ mqtt_message_t* mqtt_msg_connect(mqtt_connection_t* connection, mqtt_connect_inf
         if (append_string(connection, info->will_topic, strlen(info->will_topic)) < 0)
             return fail_message(connection);
 
-        if (append_string(connection, info->will_message, strlen(info->will_message)) < 0)
+        if (append_string(connection, info->will_message, info->will_length) < 0)
             return fail_message(connection);
 
         variable_header->flags |= MQTT_CONNECT_FLAG_WILL;