diff --git a/lib/mosquitto_internal.h b/lib/mosquitto_internal.h
index 7d9a5880..39b0431c 100644
--- a/lib/mosquitto_internal.h
+++ b/lib/mosquitto_internal.h
@@ -195,8 +195,10 @@ struct mosquitto {
 	bool is_dropping;
 	bool is_bridge;
 	struct mosquitto__bridge *bridge;
-	struct mosquitto_client_msg *msgs;
-	struct mosquitto_client_msg *last_msg;
+	struct mosquitto_client_msg *inflight_msgs;
+	struct mosquitto_client_msg *last_inflight_msg;
+	struct mosquitto_client_msg *queued_msgs;
+	struct mosquitto_client_msg *last_queued_msg;
 	int msg_count;
 	int msg_count12;
 	struct mosquitto__acl_user *acl_list;
diff --git a/src/context.c b/src/context.c
index 872b8548..82afa917 100644
--- a/src/context.c
+++ b/src/context.c
@@ -70,8 +70,10 @@ struct mosquitto *context__init(struct mosquitto_db *db, mosq_sock_t sock)
 		}
 	}
 	context->bridge = NULL;
-	context->msgs = NULL;
-	context->last_msg = NULL;
+	context->inflight_msgs = NULL;
+	context->last_inflight_msg = NULL;
+	context->queued_msgs = NULL;
+	context->last_queued_msg = NULL;
 	context->msg_count = 0;
 	context->msg_count12 = 0;
 #ifdef WITH_TLS
@@ -166,15 +168,24 @@ void context__cleanup(struct mosquitto_db *db, struct mosquitto *context, bool d
 		context->will = NULL;
 	}
 	if(do_free || context->clean_session){
-		msg = context->msgs;
+		msg = context->inflight_msgs;
 		while(msg){
 			next = msg->next;
 			db__msg_store_deref(db, &msg->store);
 			mosquitto__free(msg);
 			msg = next;
 		}
-		context->msgs = NULL;
-		context->last_msg = NULL;
+		context->inflight_msgs = NULL;
+		context->last_inflight_msg = NULL;
+		msg = context->queued_msgs;
+		while(msg){
+			next = msg->next;
+			db__msg_store_deref(db, &msg->store);
+			mosquitto__free(msg);
+			msg = next;
+		}
+		context->queued_msgs = NULL;
+		context->last_queued_msg = NULL;
 	}
 	if(do_free){
 		mosquitto__free(context);
diff --git a/src/database.c b/src/database.c
index 104246ec..47e93771 100644
--- a/src/database.c
+++ b/src/database.c
@@ -221,12 +221,12 @@ static void db__message_remove(struct mosquitto_db *db, struct mosquitto *contex
 	if(last){
 		last->next = (*msg)->next;
 		if(!last->next){
-			context->last_msg = last;
+			context->last_inflight_msg = last;
 		}
 	}else{
-		context->msgs = (*msg)->next;
-		if(!context->msgs){
-			context->last_msg = NULL;
+		context->inflight_msgs = (*msg)->next;
+		if(!context->inflight_msgs){
+			context->last_inflight_msg = NULL;
 		}
 	}
 	context->msg_count--;
@@ -237,11 +237,54 @@ static void db__message_remove(struct mosquitto_db *db, struct mosquitto *contex
 	if(last){
 		*msg = last->next;
 	}else{
-		*msg = context->msgs;
+		*msg = context->inflight_msgs;
 	}
-	tail = context->msgs;
-	i = 0;
-	while(tail && tail->state == mosq_ms_queued && i<max_inflight){
+}
+
+void db__message_dequeue_first(struct mosquitto *context)
+{
+	struct mosquitto_client_msg *msg;
+
+	msg = context->queued_msgs;
+	context->queued_msgs = msg->next;
+	if (context->last_queued_msg == msg){
+		context->last_queued_msg = NULL;
+	}
+
+	if (context->last_inflight_msg){
+		context->last_inflight_msg->next = msg;
+		context->last_inflight_msg = msg;
+	}else{
+		context->inflight_msgs = msg;
+		context->last_inflight_msg = msg;
+	}
+	msg->next = NULL;
+}
+
+int db__message_delete(struct mosquitto_db *db, struct mosquitto *context, uint16_t mid, enum mosquitto_msg_direction dir)
+{
+	struct mosquitto_client_msg *tail, *last = NULL;
+	int msg_index = 0;
+	bool deleted = false;
+
+	if(!context) return MOSQ_ERR_INVAL;
+
+	tail = context->inflight_msgs;
+	while(tail){
+		msg_index++;
+		if(tail->mid == mid && tail->direction == dir){
+			msg_index--;
+			db__message_remove(db, context, &tail, last);
+			deleted = true;
+		}else{
+			last = tail;
+			tail = tail->next;
+		}
+	}
+	while (context->queued_msgs && (max_inflight == 0 || msg_index < max_inflight)){
+		msg_index++;
+		tail = context->queued_msgs;
+		tail->timestamp = mosquitto_time();
 		if(tail->direction == mosq_md_out){
 			switch(tail->qos){
 				case 0:
@@ -254,58 +297,14 @@ static void db__message_remove(struct mosquitto_db *db, struct mosquitto *contex
 					tail->state = mosq_ms_publish_qos2;
 					break;
 			}
+			db__message_dequeue_first(context);
 		}else{
 			if(tail->qos == 2){
-				tail->state = mosq_ms_send_pubrec;
+				send__pubrec(context, tail->mid);
+				tail->state = mosq_ms_wait_for_pubrel;
+				db__message_dequeue_first(context);
 			}
 		}
-
-		tail = tail->next;
-	}
-}
-
-int db__message_delete(struct mosquitto_db *db, struct mosquitto *context, uint16_t mid, enum mosquitto_msg_direction dir)
-{
-	struct mosquitto_client_msg *tail, *last = NULL;
-	int msg_index = 0;
-	bool deleted = false;
-
-	if(!context) return MOSQ_ERR_INVAL;
-
-	tail = context->msgs;
-	while(tail){
-		msg_index++;
-		if(tail->state == mosq_ms_queued && msg_index <= max_inflight){
-			tail->timestamp = mosquitto_time();
-			if(tail->direction == mosq_md_out){
-				switch(tail->qos){
-					case 0:
-						tail->state = mosq_ms_publish_qos0;
-						break;
-					case 1:
-						tail->state = mosq_ms_publish_qos1;
-						break;
-					case 2:
-						tail->state = mosq_ms_publish_qos2;
-						break;
-				}
-			}else{
-				if(tail->qos == 2){
-					tail->state = mosq_ms_wait_for_pubrel;
-				}
-			}
-		}
-		if(tail->mid == mid && tail->direction == dir){
-			msg_index--;
-			db__message_remove(db, context, &tail, last);
-			deleted = true;
-		}else{
-			last = tail;
-			tail = tail->next;
-		}
-		if(msg_index > max_inflight && deleted){
-			return MOSQ_ERR_SUCCESS;
-		}
 	}
 
 	return MOSQ_ERR_SUCCESS;
@@ -314,6 +313,7 @@ int db__message_delete(struct mosquitto_db *db, struct mosquitto *context, uint1
 int db__message_insert(struct mosquitto_db *db, struct mosquitto *context, uint16_t mid, enum mosquitto_msg_direction dir, int qos, bool retain, struct mosquitto_msg_store *stored)
 {
 	struct mosquitto_client_msg *msg;
+	struct mosquitto_client_msg **msgs, **last_msg;
 	enum mosquitto_msg_state state = mosq_ms_invalid;
 	int rc = 0;
 	int i;
@@ -422,12 +422,20 @@ int db__message_insert(struct mosquitto_db *db, struct mosquitto *context, uint1
 	msg->dup = false;
 	msg->qos = qos;
 	msg->retain = retain;
-	if(context->last_msg){
-		context->last_msg->next = msg;
-		context->last_msg = msg;
+
+	if (state == mosq_ms_queued){
+		msgs = &(context->queued_msgs);
+		last_msg = &(context->last_queued_msg);
 	}else{
-		context->msgs = msg;
-		context->last_msg = msg;
+		msgs = &(context->inflight_msgs);
+		last_msg = &(context->last_inflight_msg);
+	}
+	if(*last_msg){
+		(*last_msg)->next = msg;
+		(*last_msg) = msg;
+	}else{
+		*msgs = msg;
+		*last_msg = msg;
 	}
 	context->msg_count++;
 	if(qos > 0){
@@ -478,7 +486,7 @@ int db__message_update(struct mosquitto *context, uint16_t mid, enum mosquitto_m
 {
 	struct mosquitto_client_msg *tail;
 
-	tail = context->msgs;
+	tail = context->inflight_msgs;
 	while(tail){
 		if(tail->mid == mid && tail->direction == dir){
 			tail->state = state;
@@ -496,15 +504,25 @@ int db__messages_delete(struct mosquitto_db *db, struct mosquitto *context)
 
 	if(!context) return MOSQ_ERR_INVAL;
 
-	tail = context->msgs;
+	tail = context->inflight_msgs;
 	while(tail){
 		db__msg_store_deref(db, &tail->store);
 		next = tail->next;
 		mosquitto__free(tail);
 		tail = next;
 	}
-	context->msgs = NULL;
-	context->last_msg = NULL;
+	context->inflight_msgs = NULL;
+	context->last_inflight_msg = NULL;
+
+	tail = context->queued_msgs;
+	while(tail){
+		db__msg_store_deref(db, &tail->store);
+		next = tail->next;
+		mosquitto__free(tail);
+		tail = next;
+	}
+	context->queued_msgs = NULL;
+	context->last_queued_msg = NULL;
 	context->msg_count = 0;
 	context->msg_count12 = 0;
 
@@ -616,7 +634,7 @@ int db__message_store_find(struct mosquitto *context, uint16_t mid, struct mosqu
 	if(!context) return MOSQ_ERR_INVAL;
 
 	*stored = NULL;
-	tail = context->msgs;
+	tail = context->inflight_msgs;
 	while(tail){
 		if(tail->store->source_mid == mid && tail->direction == mosq_md_in){
 			*stored = tail->store;
@@ -636,11 +654,11 @@ int db__message_reconnect_reset(struct mosquitto_db *db, struct mosquitto *conte
 	struct mosquitto_client_msg *prev = NULL;
 	int count;
 
-	msg = context->msgs;
+	msg = context->inflight_msgs;
 	context->msg_count = 0;
 	context->msg_count12 = 0;
 	while(msg){
-		context->last_msg = msg;
+		context->last_inflight_msg = msg;
 
 		context->msg_count++;
 		if(msg->qos > 0){
@@ -648,22 +666,20 @@ int db__message_reconnect_reset(struct mosquitto_db *db, struct mosquitto *conte
 		}
 
 		if(msg->direction == mosq_md_out){
-			if(msg->state != mosq_ms_queued){
-				switch(msg->qos){
-					case 0:
-						msg->state = mosq_ms_publish_qos0;
-						break;
-					case 1:
-						msg->state = mosq_ms_publish_qos1;
-						break;
-					case 2:
-						if(msg->state == mosq_ms_wait_for_pubcomp){
-							msg->state = mosq_ms_resend_pubrel;
-						}else{
-							msg->state = mosq_ms_publish_qos2;
-						}
-						break;
-				}
+			switch(msg->qos){
+				case 0:
+					msg->state = mosq_ms_publish_qos0;
+					break;
+				case 1:
+					msg->state = mosq_ms_publish_qos1;
+					break;
+				case 2:
+					if(msg->state == mosq_ms_wait_for_pubcomp){
+						msg->state = mosq_ms_resend_pubrel;
+					}else{
+						msg->state = mosq_ms_publish_qos2;
+					}
+					break;
 			}
 		}else{
 			if(msg->qos != 2){
@@ -684,11 +700,18 @@ int db__message_reconnect_reset(struct mosquitto_db *db, struct mosquitto *conte
 	 * get sent until the client next receives a message - and they
 	 * will be sent out of order.
 	 */
-	if(context->msgs){
-		count = 0;
-		msg = context->msgs;
-		while(msg && (max_inflight == 0 || count < max_inflight)){
-			if(msg->state == mosq_ms_queued){
+	if(context->queued_msgs){
+		count = context->msg_count;
+		msg = context->queued_msgs;
+		while(msg){
+			context->last_queued_msg = msg;
+
+			count++;
+			context->msg_count++;
+			if(msg->qos > 0){
+				context->msg_count12++;
+			}
+			if (max_inflight == 0 || count <= max_inflight){
 				switch(msg->qos){
 					case 0:
 						msg->state = mosq_ms_publish_qos0;
@@ -700,9 +723,11 @@ int db__message_reconnect_reset(struct mosquitto_db *db, struct mosquitto *conte
 						msg->state = mosq_ms_publish_qos2;
 						break;
 				}
+				db__message_dequeue_first(context);
+				msg = context->queued_msgs;
+			} else {
+				msg = msg->next;
 			}
-			msg = msg->next;
-			count++;
 		}
 	}
 
@@ -722,30 +747,9 @@ int db__message_release(struct mosquitto_db *db, struct mosquitto *context, uint
 
 	if(!context) return MOSQ_ERR_INVAL;
 
-	tail = context->msgs;
+	tail = context->inflight_msgs;
 	while(tail){
 		msg_index++;
-		if(tail->state == mosq_ms_queued && msg_index <= max_inflight){
-			tail->timestamp = mosquitto_time();
-			if(tail->direction == mosq_md_out){
-				switch(tail->qos){
-					case 0:
-						tail->state = mosq_ms_publish_qos0;
-						break;
-					case 1:
-						tail->state = mosq_ms_publish_qos1;
-						break;
-					case 2:
-						tail->state = mosq_ms_publish_qos2;
-						break;
-				}
-			}else{
-				if(tail->qos == 2){
-					send__pubrec(context, tail->mid);
-					tail->state = mosq_ms_wait_for_pubrel;
-				}
-			}
-		}
 		if(tail->mid == mid && tail->direction == dir){
 			qos = tail->store->qos;
 			topic = tail->store->topic;
@@ -766,8 +770,31 @@ int db__message_release(struct mosquitto_db *db, struct mosquitto *context, uint
 			last = tail;
 			tail = tail->next;
 		}
-		if(msg_index > max_inflight && deleted){
-			return MOSQ_ERR_SUCCESS;
+	}
+
+	while(context->queued_msgs && (max_inflight == 0 || msg_index < max_inflight)){
+		msg_index++;
+		tail = context->queued_msgs;
+		tail->timestamp = mosquitto_time();
+		if(tail->direction == mosq_md_out){
+			switch(tail->qos){
+				case 0:
+					tail->state = mosq_ms_publish_qos0;
+					break;
+				case 1:
+					tail->state = mosq_ms_publish_qos1;
+					break;
+				case 2:
+					tail->state = mosq_ms_publish_qos2;
+					break;
+			}
+			db__message_dequeue_first(context);
+		}else{
+			if(tail->qos == 2){
+				send__pubrec(context, tail->mid);
+				tail->state = mosq_ms_wait_for_pubrel;
+				db__message_dequeue_first(context);
+			}
 		}
 	}
 	if(deleted){
@@ -799,103 +826,119 @@ int db__message_write(struct mosquitto_db *db, struct mosquitto *context)
 		return MOSQ_ERR_SUCCESS;
 	}
 
-	tail = context->msgs;
+	tail = context->inflight_msgs;
 	while(tail){
-		if(tail->direction == mosq_md_in){
-			msg_count++;
-		}
-		if(tail->state != mosq_ms_queued){
-			mid = tail->mid;
-			retries = tail->dup;
-			retain = tail->retain;
-			topic = tail->store->topic;
-			qos = tail->qos;
-			payloadlen = tail->store->payloadlen;
-			payload = UHPA_ACCESS_PAYLOAD(tail->store);
+		msg_count++;
+		mid = tail->mid;
+		retries = tail->dup;
+		retain = tail->retain;
+		topic = tail->store->topic;
+		qos = tail->qos;
+		payloadlen = tail->store->payloadlen;
+		payload = UHPA_ACCESS_PAYLOAD(tail->store);
 
-			switch(tail->state){
-				case mosq_ms_publish_qos0:
-					rc = send__publish(context, mid, topic, payloadlen, payload, qos, retain, retries);
-					if(!rc){
-						db__message_remove(db, context, &tail, last);
-					}else{
-						return rc;
-					}
-					break;
-
-				case mosq_ms_publish_qos1:
-					rc = send__publish(context, mid, topic, payloadlen, payload, qos, retain, retries);
-					if(!rc){
-						tail->timestamp = mosquitto_time();
-						tail->dup = 1; /* Any retry attempts are a duplicate. */
-						tail->state = mosq_ms_wait_for_puback;
-					}else{
-						return rc;
-					}
-					last = tail;
-					tail = tail->next;
-					break;
-
-				case mosq_ms_publish_qos2:
-					rc = send__publish(context, mid, topic, payloadlen, payload, qos, retain, retries);
-					if(!rc){
-						tail->timestamp = mosquitto_time();
-						tail->dup = 1; /* Any retry attempts are a duplicate. */
-						tail->state = mosq_ms_wait_for_pubrec;
-					}else{
-						return rc;
-					}
-					last = tail;
-					tail = tail->next;
-					break;
-				
-				case mosq_ms_send_pubrec:
-					rc = send__pubrec(context, mid);
-					if(!rc){
-						tail->state = mosq_ms_wait_for_pubrel;
-					}else{
-						return rc;
-					}
-					last = tail;
-					tail = tail->next;
-					break;
-
-				case mosq_ms_resend_pubrel:
-					rc = send__pubrel(context, mid);
-					if(!rc){
-						tail->state = mosq_ms_wait_for_pubcomp;
-					}else{
-						return rc;
-					}
-					last = tail;
-					tail = tail->next;
-					break;
-
-				case mosq_ms_resend_pubcomp:
-					rc = send__pubcomp(context, mid);
-					if(!rc){
-						tail->state = mosq_ms_wait_for_pubrel;
-					}else{
-						return rc;
-					}
-					last = tail;
-					tail = tail->next;
-					break;
-
-				default:
-					last = tail;
-					tail = tail->next;
-					break;
-			}
-		}else{
-			/* state == mosq_ms_queued */
-			if(tail->direction == mosq_md_in && (max_inflight == 0 || msg_count < max_inflight)){
-				if(tail->qos == 2){
-					tail->state = mosq_ms_send_pubrec;
+		switch(tail->state){
+			case mosq_ms_publish_qos0:
+				rc = send__publish(context, mid, topic, payloadlen, payload, qos, retain, retries);
+				if(!rc){
+					db__message_remove(db, context, &tail, last);
+				}else{
+					return rc;
+				}
+				break;
+
+			case mosq_ms_publish_qos1:
+				rc = send__publish(context, mid, topic, payloadlen, payload, qos, retain, retries);
+				if(!rc){
+					tail->timestamp = mosquitto_time();
+					tail->dup = 1; /* Any retry attempts are a duplicate. */
+					tail->state = mosq_ms_wait_for_puback;
+				}else{
+					return rc;
 				}
-			}else{
 				last = tail;
 				tail = tail->next;
+				break;
+
+			case mosq_ms_publish_qos2:
+				rc = send__publish(context, mid, topic, payloadlen, payload, qos, retain, retries);
+				if(!rc){
+					tail->timestamp = mosquitto_time();
+					tail->dup = 1; /* Any retry attempts are a duplicate. */
+					tail->state = mosq_ms_wait_for_pubrec;
+				}else{
+					return rc;
+				}
+				last = tail;
+				tail = tail->next;
+				break;
+
+			case mosq_ms_send_pubrec:
+				rc = send__pubrec(context, mid);
+				if(!rc){
+					tail->state = mosq_ms_wait_for_pubrel;
+				}else{
+					return rc;
+				}
+				last = tail;
+				tail = tail->next;
+				break;
+
+			case mosq_ms_resend_pubrel:
+				rc = send__pubrel(context, mid);
+				if(!rc){
+					tail->state = mosq_ms_wait_for_pubcomp;
+				}else{
+					return rc;
+				}
+				last = tail;
+				tail = tail->next;
+				break;
+
+			case mosq_ms_resend_pubcomp:
+				rc = send__pubcomp(context, mid);
+				if(!rc){
+					tail->state = mosq_ms_wait_for_pubrel;
+				}else{
+					return rc;
+				}
+				last = tail;
+				tail = tail->next;
+				break;
+
+			default:
+				last = tail;
+				tail = tail->next;
+				break;
+		}
+	}
+
+	while(context->queued_msgs && (max_inflight == 0 || msg_count < max_inflight)){
+		msg_count++;
+		tail = context->queued_msgs;
+		if(tail->direction == mosq_md_out){
+			switch(tail->qos){
+				case 0:
+					tail->state = mosq_ms_publish_qos0;
+					break;
+				case 1:
+					tail->state = mosq_ms_publish_qos1;
+					break;
+				case 2:
+					tail->state = mosq_ms_publish_qos2;
+					break;
+			}
+			db__message_dequeue_first(context);
+		}else{
+			if(tail->qos == 2){
+				tail->state = mosq_ms_send_pubrec;
+				db__message_dequeue_first(context);
+				rc = send__pubrec(context, mid);
+				if(!rc){
+					tail->state = mosq_ms_wait_for_pubrel;
+				}else{
+					return rc;
+				}
 			}
 		}
 	}
diff --git a/src/handle_connect.c b/src/handle_connect.c
index 33891b57..f28cf014 100644
--- a/src/handle_connect.c
+++ b/src/handle_connect.c
@@ -85,9 +85,11 @@ int handle__connect(struct mosquitto_db *db, struct mosquitto *context)
 	uint8_t will, will_retain, will_qos, clean_session;
 	uint8_t username_flag, password_flag;
 	char *username = NULL, *password = NULL;
+	bool process_queued_msgs = false;
 	int rc;
 	struct mosquitto__acl_user *acl_tail;
 	struct mosquitto_client_msg *msg_tail, *msg_prev;
+	struct mosquitto_client_msg **msgs;
 	struct mosquitto *found_context;
 	int slen;
 	struct mosquitto__subleaf *leaf;
@@ -442,9 +444,11 @@ int handle__connect(struct mosquitto_db *db, struct mosquitto *context)
 		context->clean_session = clean_session;
 
 		if(context->clean_session == false && found_context->clean_session == false){
-			if(found_context->msgs){
-				context->msgs = found_context->msgs;
-				found_context->msgs = NULL;
+			if(found_context->inflight_msgs || found_context->queued_msgs){
+				context->inflight_msgs = found_context->inflight_msgs;
+				context->queued_msgs = found_context->queued_msgs;
+				found_context->inflight_msgs = NULL;
+				found_context->queued_msgs = NULL;
 				db__message_reconnect_reset(db, context);
 			}
 			context->subs = found_context->subs;
@@ -532,7 +536,8 @@ int handle__connect(struct mosquitto_db *db, struct mosquitto *context)
 
 	/* Remove any queued messages that are no longer allowed through ACL,
 	 * assuming a possible change of username. */
-	msg_tail = context->msgs;
+	msgs = &(context->inflight_msgs);
+	msg_tail = *msgs;
 	msg_prev = NULL;
 	while(msg_tail){
 		if(msg_tail->direction == mosq_md_out){
@@ -543,10 +548,11 @@ int handle__connect(struct mosquitto_db *db, struct mosquitto *context)
 					mosquitto__free(msg_tail);
 					msg_tail = msg_prev->next;
 				}else{
-					context->msgs = context->msgs->next;
+					*msgs = (*msgs)->next;
 					mosquitto__free(msg_tail);
-					msg_tail = context->msgs;
+					msg_tail = (*msgs);
 				}
+				// XXX: why it does not update last_msg if msg_tail was the last message ?
 			}else{
 				msg_prev = msg_tail;
 				msg_tail = msg_tail->next;
@@ -555,6 +561,12 @@ int handle__connect(struct mosquitto_db *db, struct mosquitto *context)
 			msg_prev = msg_tail;
 			msg_tail = msg_tail->next;
 		}
+		if (msg_tail == NULL && !process_queued_msgs){
+			msgs = &(context->queued_msgs);
+			msg_tail = *msgs;
+			msg_prev = NULL;
+			process_queued_msgs = true;
+		}
 	}
 
 	HASH_ADD_KEYPTR(hh_id, db->contexts_by_id, context->id, strlen(context->id), context);
diff --git a/src/mosquitto_broker.h b/src/mosquitto_broker.h
index a8e6202f..6cba0212 100644
--- a/src/mosquitto_broker.h
+++ b/src/mosquitto_broker.h
@@ -499,6 +499,7 @@ int db__message_insert(struct mosquitto_db *db, struct mosquitto *context, uint1
 int db__message_release(struct mosquitto_db *db, struct mosquitto *context, uint16_t mid, enum mosquitto_msg_direction dir);
 int db__message_update(struct mosquitto *context, uint16_t mid, enum mosquitto_msg_direction dir, enum mosquitto_msg_state state);
 int db__message_write(struct mosquitto_db *db, struct mosquitto *context);
+void db__message_dequeue_first(struct mosquitto *context);
 int db__messages_delete(struct mosquitto_db *db, struct mosquitto *context);
 int db__messages_easy_queue(struct mosquitto_db *db, struct mosquitto *context, const char *topic, int qos, uint32_t payloadlen, const void *payload, int retain);
 int db__message_store(struct mosquitto_db *db, const char *source, uint16_t source_mid, char *topic, int qos, uint32_t payloadlen, mosquitto__payload_uhpa *payload, int retain, struct mosquitto_msg_store **stored, dbid_t store_id);
diff --git a/src/persist.c b/src/persist.c
index d252b786..19b0e596 100644
--- a/src/persist.c
+++ b/src/persist.c
@@ -71,13 +71,14 @@ static int persist__client_messages_write(struct mosquitto_db *db, FILE *db_fptr
 	dbid_t i64temp;
 	uint16_t i16temp, slen;
 	uint8_t i8temp;
+	bool process_queued_message = false;
 	struct mosquitto_client_msg *cmsg;
 
 	assert(db);
 	assert(db_fptr);
 	assert(context);
 
-	cmsg = context->msgs;
+	cmsg = context->inflight_msgs;
 	while(cmsg){
 		slen = strlen(context->id);
 
@@ -115,6 +116,10 @@ static int persist__client_messages_write(struct mosquitto_db *db, FILE *db_fptr
 		write_e(db_fptr, &i8temp, sizeof(uint8_t));
 
 		cmsg = cmsg->next;
+		if (cmsg == NULL && !process_queued_message){
+			cmsg = context->queued_msgs;
+			process_queued_message = true;
+		}
 	}
 
 	return MOSQ_ERR_SUCCESS;
@@ -408,6 +413,7 @@ error:
 static int persist__client_msg_restore(struct mosquitto_db *db, const char *client_id, uint16_t mid, uint8_t qos, uint8_t retain, uint8_t direction, uint8_t state, uint8_t dup, uint64_t store_id)
 {
 	struct mosquitto_client_msg *cmsg;
+	struct mosquitto_client_msg **msgs, **last_msg;
 	struct mosquitto_msg_store_load *load;
 	struct mosquitto *context;
 
@@ -442,12 +448,20 @@ static int persist__client_msg_restore(struct mosquitto_db *db, const char *clie
 		log__printf(NULL, MOSQ_LOG_ERR, "Error restoring persistent database, message store corrupt.");
 		return 1;
 	}
-	if(context->msgs){
-		context->last_msg->next = cmsg;
+
+	if (state == mosq_ms_queued){
+		msgs = &(context->queued_msgs);
+		last_msg = &(context->last_queued_msg);
 	}else{
-		context->msgs = cmsg;
+		msgs = &(context->inflight_msgs);
+		last_msg = &(context->last_inflight_msg);
 	}
-	context->last_msg = cmsg;
+	if(*msgs){
+		(*last_msg)->next = cmsg;
+	}else{
+		*msgs = cmsg;
+	}
+	*last_msg = cmsg;
 
 	return MOSQ_ERR_SUCCESS;
 }