diff --git a/src/MQTTAsyncUtils.c b/src/MQTTAsyncUtils.c
index f0f8ecc5..f57ad051 100644
--- a/src/MQTTAsyncUtils.c
+++ b/src/MQTTAsyncUtils.c
@@ -257,13 +257,15 @@ static int MQTTAsync_persistCommand(MQTTAsync_queuedCommand* qcmd)
 	int chars = 0; /* number of chars from snprintf */
 	int props_allocated = 0;
 	int process = 1;
+	int multiplier = 2; /* default value 2 for MQTTVERSION < 5 */
 
 	FUNC_ENTRY;
 	switch (command->type)
 	{
 		case SUBSCRIBE:
+			multiplier = (aclient->c->MQTTVersion >= MQTTVERSION_5) ? 3 : 2;
 			nbufs = ((aclient->c->MQTTVersion >= MQTTVERSION_5) ? 4 : 3) +
-				(command->details.sub.count * 2);
+				(command->details.sub.count * multiplier);
 
 			if (((lens = (int*)malloc(nbufs * sizeof(int))) == NULL) ||
 					((bufs = malloc(nbufs * sizeof(char *))) == NULL))
@@ -285,12 +287,9 @@ static int MQTTAsync_persistCommand(MQTTAsync_queuedCommand* qcmd)
 				bufs[bufindex] = command->details.sub.topics[i];
 				lens[bufindex++] = (int)strlen(command->details.sub.topics[i]) + 1;
 
-				if (aclient->c->MQTTVersion < MQTTVERSION_5)
-				{
-					bufs[bufindex] = &command->details.sub.qoss[i];
-					lens[bufindex++] = sizeof(command->details.sub.qoss[i]);
-				}
-				else
+				bufs[bufindex] = &command->details.sub.qoss[i];
+				lens[bufindex++] = sizeof(command->details.sub.qoss[i]);
+				if (aclient->c->MQTTVersion >= MQTTVERSION_5)
 				{
 					if (command->details.sub.count == 1)
 					{
@@ -459,7 +458,7 @@ static MQTTAsync_queuedCommand* MQTTAsync_restoreCommand(char* buffer, int bufle
 	switch (command->type)
 	{
 		case SUBSCRIBE:
-			if (qcommand->not_restored == 0)
+			if (qcommand->not_restored == 1)
 				break;
 			if (&ptr[sizeof(int)] > endpos)
 				goto error_exit;
@@ -470,16 +469,17 @@ static MQTTAsync_queuedCommand* MQTTAsync_restoreCommand(char* buffer, int bufle
 			{
 				if ((command->details.sub.topics = (char **)malloc(sizeof(char *) * command->details.sub.count)) == NULL)
 					goto error_exit;
-				if (MQTTVersion < MQTTVERSION_5)
+				if ((command->details.sub.qoss = (int *)malloc(sizeof(int) * command->details.sub.count)) == NULL)
+					goto error_exit;
+
+				if ((MQTTVersion >= MQTTVERSION_5))
 				{
-					if ((command->details.sub.qoss = (int *)malloc(sizeof(int) * command->details.sub.count)) == NULL)
-						goto error_exit;
-				}
-				else if (command->details.sub.count > 1)
-				{
-					command->details.sub.optlist = (MQTTSubscribe_options*)malloc(sizeof(MQTTSubscribe_options) * command->details.sub.count);
-					if (command->details.sub.optlist == NULL)
-						goto error_exit;
+					if (command->details.sub.count > 1)
+					{
+						command->details.sub.optlist = (MQTTSubscribe_options*)malloc(sizeof(MQTTSubscribe_options) * command->details.sub.count);
+						if (command->details.sub.optlist == NULL)
+							goto error_exit;
+					}
 				}
 			}
 
@@ -491,18 +491,15 @@ static MQTTAsync_queuedCommand* MQTTAsync_restoreCommand(char* buffer, int bufle
 
 				if ((command->details.sub.topics[i] = malloc(data_size)) == NULL)
 					goto error_exit;
-
 				strcpy(command->details.sub.topics[i], ptr);
 				ptr += data_size;
 
-				if (MQTTVersion < MQTTVERSION_5)
-				{
-					if (&ptr[sizeof(int)] > endpos)
-						goto error_exit;
-					command->details.sub.qoss[i] = *(int*)ptr;
-					ptr += sizeof(int);
-				}
-				else
+				if (&ptr[sizeof(int)] > endpos)
+					goto error_exit;
+				command->details.sub.qoss[i] = *(int*)ptr;
+				ptr += sizeof(int);
+
+				if (MQTTVersion >= MQTTVERSION_5)
 				{
 					if (&ptr[sizeof(MQTTSubscribe_options)] > endpos)
 						goto error_exit;
@@ -521,7 +518,7 @@ static MQTTAsync_queuedCommand* MQTTAsync_restoreCommand(char* buffer, int bufle
 			break;
 
 		case UNSUBSCRIBE:
-			if (qcommand->not_restored == 0)
+			if (qcommand->not_restored == 1)
 				break;
 
 			if (&ptr[sizeof(int)] > endpos)