diff --git a/lib/mosquitto_internal.h b/lib/mosquitto_internal.h
index 666a80c0..da9d8422 100644
--- a/lib/mosquitto_internal.h
+++ b/lib/mosquitto_internal.h
@@ -109,6 +109,7 @@ enum mosquitto_client_state {
 	mosq_cs_expiring = 15,
 	mosq_cs_connecting = 16,
 	mosq_cs_duplicate = 17, /* client that has been taken over by another with the same id */
+	mosq_cs_disconnect_with_will = 18,
 };
 
 enum mosquitto__protocol {
diff --git a/src/handle_connect.c b/src/handle_connect.c
index 208ce67f..8749977a 100644
--- a/src/handle_connect.c
+++ b/src/handle_connect.c
@@ -796,7 +796,7 @@ handle_connect_error:
 int handle__disconnect(struct mosquitto_db *db, struct mosquitto *context)
 {
 	int rc;
-	uint8_t reason_code;
+	uint8_t reason_code = 0;
 	mosquitto_property *properties = NULL;
 
 	if(!context){
@@ -833,7 +833,11 @@ int handle__disconnect(struct mosquitto_db *db, struct mosquitto *context)
 			return MOSQ_ERR_PROTOCOL;
 		}
 	}
-	context->state = mosq_cs_disconnecting;
+	if(reason_code == MQTT_RC_DISCONNECT_WITH_WILL_MSG){
+		context->state = mosq_cs_disconnect_with_will;
+	}else{
+		context->state = mosq_cs_disconnecting;
+	}
 	do_disconnect(db, context);
 	return MOSQ_ERR_SUCCESS;
 }
diff --git a/test/Makefile b/test/Makefile
index 9b3849bd..aeb69f07 100644
--- a/test/Makefile
+++ b/test/Makefile
@@ -12,6 +12,7 @@ test :
 ptest :
 	$(MAKE) -C broker ptest
 	$(MAKE) -C lib ptest
+	$(MAKE) -C unit test
 
 clean : 
 	$(MAKE) -C lib clean
diff --git a/test/broker/07-will-disconnect-with-will.py b/test/broker/07-will-disconnect-with-will.py
new file mode 100755
index 00000000..80e998a2
--- /dev/null
+++ b/test/broker/07-will-disconnect-with-will.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+
+# Test whether a client will is transmitted when a client disconnects with DISCONNECT with will.
+# MQTT 5
+
+from mosq_test_helper import *
+
+def do_test():
+    rc = 1
+    keepalive = 60
+
+    mid = 1
+    connect1_packet = mosq_test.gen_connect("will-qos0-test", keepalive=keepalive, proto_ver=5)
+    connack1_packet = mosq_test.gen_connack(rc=0, proto_ver=5)
+
+    connect2_packet = mosq_test.gen_connect("will-helper", keepalive=keepalive, proto_ver=5, will_topic="will/test", will_payload="will delay", will_qos=2)
+    connack2_packet = mosq_test.gen_connack(rc=0, proto_ver=5)
+    disconnect_packet = mosq_test.gen_disconnect(reason_code=4, proto_ver=5)
+
+    subscribe_packet = mosq_test.gen_subscribe(mid, "will/test", 0, proto_ver=5)
+    suback_packet = mosq_test.gen_suback(mid, 0, proto_ver=5)
+
+    publish_packet = mosq_test.gen_publish("will/test", qos=0, payload="will delay", proto_ver=5)
+
+    port = mosq_test.get_port()
+    broker = mosq_test.start_broker(filename=os.path.basename(__file__), port=port)
+
+    try:
+        sock1 = mosq_test.do_client_connect(connect1_packet, connack1_packet, timeout=30, port=port)
+        mosq_test.do_send_receive(sock1, subscribe_packet, suback_packet, "suback")
+
+        sock2 = mosq_test.do_client_connect(connect2_packet, connack2_packet, timeout=30, port=port)
+        sock2.send(disconnect_packet)
+
+        if mosq_test.expect_packet(sock1, "publish", publish_packet):
+            rc = 0
+
+        sock2.close()
+        sock1.close()
+    finally:
+        broker.terminate()
+        broker.wait()
+        (stdo, stde) = broker.communicate()
+        if rc:
+            print(stde)
+            exit(rc)
+
+do_test()
diff --git a/test/broker/Makefile b/test/broker/Makefile
index b78969b8..d8155437 100644
--- a/test/broker/Makefile
+++ b/test/broker/Makefile
@@ -131,6 +131,7 @@ endif
 	./07-will-delay.py
 	./07-will-delay-recover.py
 	./07-will-delay-reconnect.py
+	./07-will-disconnect-with-will.py
 
 08 :
 ifeq ($(WITH_TLS),yes)
diff --git a/test/broker/test.py b/test/broker/test.py
index 607a0fad..710276f8 100755
--- a/test/broker/test.py
+++ b/test/broker/test.py
@@ -107,6 +107,7 @@ tests = [
     (1, './07-will-delay.py'),
     (1, './07-will-delay-recover.py'),
     (1, './07-will-delay-reconnect.py'),
+    (1, './07-will-disconnect-with-will.py'),
 
     (2, './08-ssl-connect-no-auth.py'),
     (2, './08-ssl-connect-no-auth-wrong-ca.py'),