Fixed various memory leaks reported by valgrind. Also fixed some bugs in DNBD3 image WWN retrieval handling. Finally, did huge code refactoring.

3 files changed, 1589 insertions, 1572 deletions

diff --git a/Doxyfile b/Doxyfile
index 75de69e..ad61abb 100644
--- a/Doxyfile
+++ b/Doxyfile
@@ -51,5 +51,6 @@ GENERATE_XML           = NO
 # Misc
 EXTRACT_ALL            = YES
 EXTRACT_PRIVATE        = NO
+EXTRACT_INLINE         = YES
 EXTRACT_STATIC         = YES
 QUIET                  = NO
diff --git a/src/server/iscsi.c b/src/server/iscsi.c
index a65009b..05c8876 100644
--- a/src/server/iscsi.c
+++ b/src/server/iscsi.c
@@ -30,6 +30,7 @@
 #include <strings.h>
 #include <sys/socket.h>
 #include <sys/types.h>
+#include <dnbd3/config.h>
 #include <dnbd3/shared/log.h>
 #include <dnbd3/shared/sockhelper.h>
 #include <dnbd3/types.h>
@@ -500,230 +501,6 @@ void iscsi_strcpy_pad(char *dst, const char *src, const size_t size, const int p
 }
 
 /**
- * @brief Initializes a doubly linked list for usage.
- *
- * This function sets the head of the list to
- * the pointer of the list's tail, the tail
- * itself to NULL and the predecessor to the
- * pointer of the list's head.
- *
- * @param[in] list Pointer to idoubly linked list to
- * initialize. May NOT be NULL, so be careful.
- * */
-void iscsi_list_create(iscsi_list *list)
-{
-	list->head = (iscsi_node *) &list->tail;
-	list->tail = NULL;
-	list->pred = (iscsi_node *) &list->head;
-}
-
-/**
- * @brief Adds a node at the head of a doubly linked list.
- *
- * This function sets the head of the list to
- * the node and adjusts the list and node
- * pointers accordingly.
- *
- * @param[in] list Pointer to doubly linked list to add to
- * the head. May NOT be NULL, so be careful.
- * @param[in] node Pointer to node to add to the head of
- * the list. NULL is NOT allowed here, take
- * caution.
- */
-void iscsi_list_push(iscsi_list *list, iscsi_node *node)
-{
-	iscsi_node *head = list->head;
-
-	list->head = node;
-	head->pred = node;
-
-	node->succ = head;
-	node->pred = (iscsi_node *) &list->head;
-}
-
-/**
- * @brief Adds a node at the tail of a doubly linked list.
- *
- * This function sets the tail of the list to
- * the node and adjusts the list and node
- * pointers accordingly.
- *
- * @param[in] list Pointer to doubly linked list to add to
- * the tail. May NOT be NULL, so be careful.
- * @param[in] node Pointer to node to add to the tail of
- * the list. NULL is NOT allowed here, take
- * caution.
- */
-void iscsi_list_enqueue(iscsi_list *list, iscsi_node *node)
-{
-	iscsi_node *tail = list->pred;
-
-	list->pred = node;
-	tail->succ = node;
-
-	node->succ = (iscsi_node *) &list->tail;
-	node->pred = tail;
-}
-
-/**
- * @brief Inserts a node into a doubly linked list before an already existing node.
- *
- * This function sets the successor of the
- * new node to the successor of the
- * existing predecessor node and the
- * predecessor of the new node to the
- * the existing predecessor node itself
- * and adjusts the list pointers
- * accordingly.
- *
- * @param[in] list Pointer to doubly linked list to insert the
- * node into. May NOT be NULL, so be careful.
- * @param[in] node Pointer to node to be inserted into the
- * list. NULL is NOT allowed here, take
- * caution.
- * @param[in] pred Pointer to node which should be the
- * previous node of the new inserted node.
- * May be NULL in which case the new node
- * is inserted at the head of the list.
- */
-void iscsi_list_insert(iscsi_list *list, iscsi_node *node, iscsi_node *pred)
-{
-	if ( pred == NULL ) {
-		iscsi_node *head = list->head;
-
-		list->head = node;
-		head->pred = node;
-
-		node->succ = head;
-		node->pred = (iscsi_node *) &list->head;
-
-		return;
-	}
-
-	iscsi_node *tail = pred->succ;
-
-	if ( tail == NULL ) {
-		tail       = pred->pred;
-
-		node->succ = pred;
-		node->pred = tail;
-
-		pred->pred = node;
-		tail->succ = node;
-
-		return;
-	}
-
-	node->succ = tail;
-	node->pred = pred;
-
-	tail->pred = node;
-	pred->succ = node;
-}
-
-/**
- * @brief Removes the node from the head of a doubly linked list.
- *
- * This function sets the head of the list to
- * its successor and adjusts the list and
- * node pointers accordingly.
- *
- * @param[in] list Pointer to doubly linked list to remove the
- * head from. May NOT be NULL, so be careful.
- */
-void iscsi_list_pop(iscsi_list *list)
-{
-	iscsi_node *head = list->head;
-	iscsi_node *node = head->succ;
-
-	if ( node == NULL )
-		return;
-
-	list->head = node;
-
-	node->pred = (iscsi_node *) &list->head;
-}
-
-/**
- * @brief Removes the node from the tail of a doubly linked list.
- *
- * This function sets the tail of the list to
- * its predecessor and adjusts the list and
- * node pointers accordingly.
- *
- * @param[in] list Pointer to doubly linked list to remove the
- * tail from. May NOT be NULL, so be careful.
- */
-void iscsi_list_dequeue(iscsi_list *list)
-{
-	iscsi_node *tail = list->pred;
-	iscsi_node *node = tail->pred;
-
-	if ( node == NULL )
-		return;
-
-	list->pred = node;
-
-	node->succ = (iscsi_node *) &list->tail;
-}
-
-/**
- * @brief Removes a specified node from a doubly linked list.
- *
- * This function sets the successor of the
- * node's predecessor and the predecessor
- * of the node's successor by adjusting
- * the list and node pointers accordingly.
- *
- * @param[in] node Pointer to node to be removed from
- * the list. May NOT be NULL, so
- * be careful.
- */
-void iscsi_list_remove(iscsi_node *node)
-{
-	iscsi_node *succ = node->succ;
-	iscsi_node *pred = node->pred;
-
-	pred->succ = succ;
-	succ->pred = pred;
-}
-
-/**
- * @brief Checks whether a doubly linked list is empty.
- *
- * Whenever this function returns false,
- * iscsi_list_peek will return a pointer
- * to the first node in the list.
- *
- * @param[in] list Pointer to doubly linked list to check if
- * empty. May NOT be NULL, so be careful.
- * @retval true The doubly linked list is empty.
- * @retval false The doubly linked list contains nodes.
- */
-bool iscsi_list_empty(const iscsi_list *list)
-{
-	return (list->head->succ == NULL);
-}
-
-/**
- * @brief Gets the node from the head of a doubly linked list.
- *
- * This function returns NULL if the list is
- * empty.
- *
- * @param[in] list Pointer to doubly linked list to get the
- * head from. May NOT be NULL, so be careful.
- * @return Pointer to doubly linked list node of the
- * head or NULL if the list is empty.
- */
-iscsi_node *iscsi_list_peek(const iscsi_list *list)
-{
-	iscsi_node *head = list->head;
-
-	return (head->succ != NULL) ? head : NULL;
-}
-
-/**
  * @brief Creates an empty hash map with either specified or default capacity.
  *
  * Creates a ultra hardcore speed optimized empty
@@ -781,12 +558,11 @@ iscsi_hashmap *iscsi_hashmap_create(const uint capacity)
 		return NULL;
 	}
 
+	iscsi_list_create( &map->list );
+
+	map->last_insert_id = 0ULL;
 	map->cap_load       = (uint) ((map->capacity * 3U) >> 2U); // 75% of capacity
 	map->count          = 0U;
-	map->removed_count  = 0U;
-	map->first          = NULL;
-	map->last           = (iscsi_hashmap_bucket *) &map->first;
-	map->last_insert_id = 0ULL;
 
 	return map;
 }
@@ -806,149 +582,13 @@ iscsi_hashmap *iscsi_hashmap_create(const uint capacity)
 void iscsi_hashmap_destroy(iscsi_hashmap *map)
 {
 	if ( map != NULL ) {
-		if ( map->buckets != NULL )
+		if ( map->buckets != NULL ) {
 			free( map->buckets );
 
-		free( map );
-	}
-}
-
-/**
- * @brief Puts an old bucket into a resized hash map.
- *
- * Puts an old bucket into a resized hash map.
- *
- * @param[in] map Pointer to resized hash map, may NOT be NULL, so
- * be careful.
- * @param[in] old_entry The old bucket to be put into the resized
- * hash map.
- * @return New bucket where the bucket has been put into.
- */
-static iscsi_hashmap_bucket *iscsi_hashmap_resize_entry(iscsi_hashmap *map, const iscsi_hashmap_bucket *old_entry)
-{
-	uint32_t index = (old_entry->hash & (map->capacity - 1U));
-
-	for ( ;; ) {
-		iscsi_hashmap_bucket *entry = &map->buckets[index];
-
-		if ( entry->key == NULL ) {
-			*entry = *old_entry;
-
-			return entry;
-		}
-
-		index = ((index + 1) & (map->capacity - 1U));
-	}
-}
-
-/**
- * @brief Resizes a hash map by doubling its bucket capacity and purges any removed buckets.
- *
- * Resizes a hash map by doubling its bucket capacity.
- * if any buckets have been removed, they are finally
- * purged. The old bucket list is freed after the
- * resize operation has been finished.
- *
- * @param[in] map Pointer to hash map to resize. This may NOT be
- * NULL, so be careful.
- * @retval -1 An error occured during resize.
- * @retval 0 Hash map has been resized successfully.
- */
-static int iscsi_hashmap_resize(iscsi_hashmap *map)
-{
-	const uint old_capacity           = map->capacity;
-	iscsi_hashmap_bucket *old_buckets = map->buckets;
-
-	map->capacity <<= ISCSI_HASHMAP_RESIZE_SHIFT;
-
-	map->buckets = (iscsi_hashmap_bucket *) calloc( map->capacity, sizeof(struct iscsi_hashmap_bucket) );
-
-	if ( map->buckets == NULL ) {
-		map->capacity = old_capacity;
-		map->buckets  = old_buckets;
-
-		return -1;
-	}
-
-	map->cap_load      = (uint) ((map->capacity * 3U) >> 2U); // 75% of capacity
-	map->last          = (iscsi_hashmap_bucket *) &map->first;
-	map->count        -= map->removed_count;
-	map->removed_count = 0U;
-
-	do {
-		iscsi_hashmap_bucket *current = map->last->next;
-
-		if ( current->key == NULL ) {
-			map->last->next = current->next;
-
-			continue;
+			map->buckets = NULL;
 		}
 
-		map->last->next = iscsi_hashmap_resize_entry(map, map->last->next);
-		map->last       = map->last->next;
-	} while ( map->last->next != NULL );
-
-	free( old_buckets );
-
-	return 0;
-}
-
-/**
- * @brief Calculates the hash code of data with a specified length.
- *
- * Calculates the hash code of data with a specified
- * length.
- *
- * @param[in] data Pointer to data to be hashed, NULL is NOT
- * an allowed here, so be careful. Data needs 8 byte alignment
- * and needs to be zero padded.
- * @param[in] len Number of bytes of hash data, must be larger
- * than 0 and is rounded up to the nearest 8 byte integer prior
- * calculating the hash code, so be careful.
- * @return Hash code of data.
- */
-static inline uint32_t iscsi_hashmap_hash_data(const uint8_t *data, const size_t len)
-{
-	const uint64_t *hash_data = (const uint64_t *) data;
-	size_t num_blocks = iscsi_align(len, ISCSI_HASHMAP_KEY_ALIGN) >> ISCSI_HASHMAP_KEY_ALIGN_SHIFT;
-	uint64_t hash = ISCSI_HASHMAP_HASH_INITIAL;
-
-	do {
-		hash ^= *hash_data++;
-		hash *= ISCSI_HASHMAP_HASH_MUL;
-	} while ( --num_blocks > 0UL );
-
-	return (uint32_t) (hash ^ hash >> 32ULL);
-}
-
-/**
- * @brief Finds a bucket by key of a specified hash map by key, key size and hash code.
- *
- * Finds a bucket by key of a specified hash map by
- * key, key size and hash code. This function may
- * only be called if the bucket is guaranteed to
- * be found, otherwise this function hangs, so be
- * careful.
- *
- * @param[in] map Pointer to hash map where the key to be
- * searched for is located, may NOT be NULL, so be careful.
- * @param[in] key Pointer to key. NULL is invalid, so be
- * careful.
- * @param[in] key_size Number of bytes for the key.
- * @param[in] hash Hash of the key to be searched for.
- * @return Pointer to found bucket.
- */
-static iscsi_hashmap_bucket *iscsi_hashmap_find_entry(iscsi_hashmap *map, const uint8_t *key, size_t key_size, uint32_t hash)
-{
-	uint32_t index = (hash & (map->capacity - 1U));
-
-	for ( ;; ) {
-		iscsi_hashmap_bucket *entry = &map->buckets[index];
-
-		if ( ((entry->key == NULL) && (entry->value == NULL)) || ((entry->key != NULL) && (entry->key_size == key_size) && (entry->hash == hash) && (memcmp( entry->key, key, key_size ) == 0)) )
-			return entry;
-
-		index = ((index + 1UL) & (map->capacity - 1U));
+		free( map );
 	}
 }
 
@@ -970,8 +610,8 @@ static iscsi_hashmap_bucket *iscsi_hashmap_find_entry(iscsi_hashmap *map, const
  */
 uint8_t *iscsi_hashmap_key_create(const uint8_t *data, const size_t len)
 {
-	const size_t key_size = iscsi_align(len, ISCSI_HASHMAP_KEY_ALIGN);
-	uint8_t *key = (uint8_t *) malloc( key_size );
+	const size_t key_size = ISCSI_ALIGN(len, ISCSI_HASHMAP_KEY_ALIGN);
+	uint8_t *key          = (uint8_t *) malloc( key_size );
 
 	if ( key == NULL ) {
 		logadd( LOG_ERROR, "iscsi_hashmap_key_create: Out of memory while allocating iSCSI hash map key" );
@@ -1100,7 +740,176 @@ int iscsi_hashmap_key_destroy_value_callback(uint8_t *key, const size_t key_size
 }
 
 /**
- * @brief Assigns key / value pair to hash map at the tail of linked list without making copies.
+ * @brief Compares two hash keys with equal length match.
+ *
+ * This function is optimized to compare
+ * 8 bytes at once and requires number
+ * of blocks specified in QWORDs. Both
+ * keys must be equal in size of a
+ * QWORD alignment.
+ *
+ * @param[in] buf Pointer to key buffer of which key
+ * to compare. May NOT be NULL, so be
+ * careful.
+ * @param[in] key Pointer to key to compare with.
+ * NULL is NOT allowed here, take
+ * caution.
+ * @param[in] num_blocks Number of blocks in QWORDs (8 bytes)
+ * to be compared.
+ */
+static inline bool iscsi_hashmap_key_eq(const uint64_t *buf, const uint64_t *key, size_t num_blocks)
+{
+	do {
+		if ( *buf++ != *key++ )
+			return false;
+	} while ( --num_blocks > 0UL );
+
+	return true;
+}
+
+/**
+ * @brief Finds a bucket by key of a specified hash map by key, key size and hash code.
+ *
+ * Finds a bucket by key of a specified hash map by
+ * key, key size and hash code. This function may
+ * only be called if the bucket is guaranteed to
+ * be found, otherwise this function hangs, so be
+ * careful.
+ *
+ * @param[in] map Pointer to hash map where the key to be
+ * searched for is located, may NOT be NULL, so be careful.
+ * @param[in] key Pointer to key. NULL is invalid, so be
+ * careful.
+ * @param[in] key_size Number of bytes for the key.
+ * @param[in] hash Hash of the key to be searched for.
+ * @return Pointer to found bucket.
+ */
+static iscsi_hashmap_bucket *iscsi_hashmap_find_entry(iscsi_hashmap *map, const uint8_t *key, size_t key_size, uint32_t hash)
+{
+	const size_t num_blocks = ISCSI_ALIGN(key_size, ISCSI_HASHMAP_KEY_ALIGN) >> ISCSI_HASHMAP_KEY_ALIGN_SHIFT;
+	uint32_t index          = (hash & (map->capacity - 1U));
+
+	for ( ;; ) {
+		iscsi_hashmap_bucket *entry = &map->buckets[index];
+
+		if ( ((entry->key == NULL) && (entry->value == NULL)) || ((entry->key != NULL) && (entry->key_size == key_size) && (entry->hash == hash) && iscsi_hashmap_key_eq( (uint64_t *) entry->key, (uint64_t *) key, num_blocks )) )
+			return entry;
+
+		index = ((index + 1UL) & (map->capacity - 1U));
+	}
+}
+
+/**
+ * @brief Calculates the hash code of data with a specified length.
+ *
+ * Calculates the hash code of data with a specified
+ * length.
+ *
+ * @param[in] data Pointer to data to be hashed, NULL is NOT
+ * an allowed here, so be careful. Data needs 8 byte alignment
+ * and needs to be zero padded.
+ * @param[in] len Number of bytes of hash data, must be larger
+ * than 0 and is rounded up to the nearest 8 byte integer prior
+ * calculating the hash code, so be careful.
+ * @return Hash code of data.
+ */
+static inline uint32_t iscsi_hashmap_key_hash_data(const uint8_t *data, const size_t len)
+{
+	const uint64_t *hash_data = (const uint64_t *) data;
+	size_t num_blocks         = ISCSI_ALIGN(len, ISCSI_HASHMAP_KEY_ALIGN) >> ISCSI_HASHMAP_KEY_ALIGN_SHIFT;
+	uint64_t hash             = ISCSI_HASHMAP_HASH_INITIAL;
+
+	do {
+		hash ^= *hash_data++;
+		hash *= ISCSI_HASHMAP_HASH_MUL;
+	} while ( --num_blocks > 0UL );
+
+	return (uint32_t) (hash ^ hash >> 32ULL);
+}
+
+/**
+ * @brief Puts an old bucket into a resized hash map.
+ *
+ * Puts an old bucket into a resized hash map.
+ *
+ * @param[in] map Pointer to resized hash map, may NOT be NULL, so
+ * be careful.
+ * @param[in] old_entry The old bucket to be put into the resized
+ * hash map.
+ * @return New bucket where the bucket has been put into.
+ */
+static iscsi_hashmap_bucket *iscsi_hashmap_resize_entry(iscsi_hashmap *map, const iscsi_hashmap_bucket *old_entry)
+{
+	uint32_t index = (old_entry->hash & (map->capacity - 1U));
+
+	for ( ;; ) {
+		iscsi_hashmap_bucket *entry = &map->buckets[index];
+
+		if ( entry->key == NULL ) {
+			entry->key       = old_entry->key;
+			entry->key_size  = old_entry->key_size;
+			entry->hash      = old_entry->hash;
+			entry->value     = old_entry->value;
+
+			return entry;
+		}
+
+		index = ((index + 1) & (map->capacity - 1U));
+	}
+}
+
+/**
+ * @brief Resizes a hash map by doubling its bucket capacity.
+ *
+ * Resizes a hash map by doubling its bucket capacity The
+ * old bucket list is freed after the
+ * resize operation has been finished.
+ *
+ * @param[in] map Pointer to hash map to resize. This may NOT be
+ * NULL, so be careful.
+ * @retval -1 An error occured during resize.
+ * @retval 0 Hash map has been resized successfully.
+ */
+static int iscsi_hashmap_resize(iscsi_hashmap *map)
+{
+	const uint old_capacity           = map->capacity;
+	iscsi_hashmap_bucket *old_buckets = map->buckets;
+	iscsi_list old_list               = {map->list.head, map->list.tail, map->list.pred};
+
+	map->capacity <<= ISCSI_HASHMAP_RESIZE_SHIFT;
+
+	map->buckets = (iscsi_hashmap_bucket *) calloc( map->capacity, sizeof(struct iscsi_hashmap_bucket) );
+
+	if ( map->buckets == NULL ) {
+		map->capacity = old_capacity;
+		map->buckets  = old_buckets;
+
+		return -1;
+	}
+
+	map->cap_load = (uint) ((map->capacity * 3U) >> 2U); // 75% of capacity
+
+	iscsi_list_clear( &map->list );
+
+	iscsi_hashmap_bucket *current;
+	iscsi_hashmap_bucket *tmp;
+
+	iscsi_list_foreach_safe_node ( &old_list, current, tmp ) {
+		if ( current->key == NULL )
+			continue;
+
+		current = iscsi_hashmap_resize_entry( map, current );
+
+		iscsi_list_enqueue( &map->list, &current->node );
+	}
+
+	free( old_buckets );
+
+	return 0;
+}
+
+/**
+ * @brief Assigns key / value pair to hash map at the tail of doubly linked list without making copies.
  *
  * Adds a key / value pair to a specified hash map
  * bucket list, if it doesn't exist already. The
@@ -1131,13 +940,11 @@ int iscsi_hashmap_put(iscsi_hashmap *map, uint8_t *key, const size_t key_size, u
 	if ( ((map->count + 1U) > map->cap_load) && (iscsi_hashmap_resize( map ) < 0) )
 		return -1;
 
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	if ( entry->key == NULL ) {
-		map->last->next = entry;
-		map->last       = entry;
-		entry->next     = NULL;
+		iscsi_list_enqueue( &map->list, &entry->node );
 
 		map->count++;
 
@@ -1152,7 +959,7 @@ int iscsi_hashmap_put(iscsi_hashmap *map, uint8_t *key, const size_t key_size, u
 }
 
 /**
- * @brief Assigns key / value pair to hash map without making copies.
+ * @brief Assigns key / value pair to hash map at the tail of doubly linked list without making copies.
  *
  * Adds a key / value pair if it doesn't exist
  * using the value of `*out_in_val`. If the pair
@@ -1186,20 +993,18 @@ int iscsi_hashmap_get_put(iscsi_hashmap *map, uint8_t *key, const size_t key_siz
 	if ( ((map->count + 1U) > map->cap_load) && (iscsi_hashmap_resize( map ) < 0) )
 		return -1;
 
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	if ( entry->key == NULL ) {
-		map->last->next = entry;
-		map->last       = entry;
-		entry->next     = NULL;
-
-		map->count++;
+		iscsi_list_enqueue( &map->list, &entry->node );
 
-		entry->value    = *out_in_value;
 		entry->key      = key;
 		entry->key_size = key_size;
 		entry->hash     = hash;
+		entry->value    = *out_in_value;
+
+		map->count++;
 
 		return 0;
 	}
@@ -1252,21 +1057,19 @@ int iscsi_hashmap_put_free(iscsi_hashmap *map, uint8_t *key, const size_t key_si
 	if ( ((map->count + 1U) > map->cap_load) && (iscsi_hashmap_resize( map ) < 0) )
 		return -1;
 
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	if ( entry->key == NULL ) {
-		map->last->next = entry;
-		map->last       = entry;
-		entry->next     = NULL;
-
-		map->count++;
+		iscsi_list_enqueue( &map->list, &entry->node );
 
 		entry->key      = key;
 		entry->key_size = key_size;
 		entry->hash     = hash;
 		entry->value    = value;
 
+		map->count++;
+
 		return 0;
 	}
 
@@ -1294,7 +1097,7 @@ int iscsi_hashmap_put_free(iscsi_hashmap *map, uint8_t *key, const size_t key_si
  */
 bool iscsi_hashmap_contains(iscsi_hashmap *map, const uint8_t *key, const size_t key_size)
 {
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	return (entry->key != NULL);
@@ -1326,7 +1129,7 @@ bool iscsi_hashmap_contains(iscsi_hashmap *map, const uint8_t *key, const size_t
  */
 int iscsi_hashmap_get(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, uint8_t **out_value)
 {
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	*out_value = entry->value;
@@ -1335,12 +1138,11 @@ int iscsi_hashmap_get(iscsi_hashmap *map, const uint8_t *key, const size_t key_s
 }
 
 /**
- * @brief Marks an element for removal by setting key and value both to NULL.
+ * @brief Removes an element both from the doubly linked list and by setting key and value both to NULL.
  *
  * Removes an element from the bucket list of the
- * hash map. Buckets are marked as removed by
- * setting their key and value to NULL. The actual
- * removal will be done upon next resize operation.
+ * hash map. Removing sets the buckets key and
+ * value to NULL.
  * If the specified key already has been removed,
  * this function will do nothing.
  *
@@ -1352,30 +1154,30 @@ int iscsi_hashmap_get(iscsi_hashmap *map, const uint8_t *key, const size_t key_s
  */
 void iscsi_hashmap_remove(iscsi_hashmap *map, const uint8_t *key, const size_t key_size)
 {
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	if ( entry->key != NULL ) {
+		iscsi_list_remove( &entry->node );
+
+		map->count--;
+
 		entry->key   = NULL;
 		entry->value = NULL;
-
-		map->removed_count++;
 	}
 }
 
 /**
- * @brief Marks an element for removal by setting key and value both to NULL, but invokes a callback function before actual marking for removal.
+ * @brief Removes an element both from the doubly linked list and by setting key and value both to NULL and but invokes a callback function before actual removal.
  *
  * Removes an element from the bucket list of the
  * hash map.\n
- * Buckets are marked as removed by setting their
- * key and value to NULL. The actual removal will
- * be done upon next resize operation. A callback
- * function is invoked if the key to be removed
- * is found in the bucket list and allows, e.g.
- * to free any resources associated with the key.
- * If the key is not found, this function will do
- * nothing.
+ * Removing sets the buckets key and
+ * value to NULL. A callback function is invoked
+ * if the key to be removed is found in the
+ * bucket list and allows, e.g. to free any
+ * resources associated with the key. If the key
+ * is not found, this function will do nothing.
  *
  * @param[in] map Pointer to the hash map to remove from
  * and may NOT be NULL, so take caution.
@@ -1394,46 +1196,45 @@ void iscsi_hashmap_remove(iscsi_hashmap *map, const uint8_t *key, const size_t k
  */
 void iscsi_hashmap_remove_free(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, iscsi_hashmap_callback callback, uint8_t *user_data)
 {
-	const uint32_t hash = iscsi_hashmap_hash_data( key, key_size );
+	const uint32_t hash         = iscsi_hashmap_key_hash_data( key, key_size );
 	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
 
 	if ( entry->key != NULL ) {
+		iscsi_list_remove( &entry->node );
+
+		map->count--;
+
 		callback( entry->key, entry->key_size, entry->value, user_data );
 
 		entry->key   = NULL;
 		entry->value = NULL;
-
-		map->removed_count++;
 	}
 }
 
 /**
- * @brief Retrieves the number of elements of the hash map, ignoring elements marked for removal.
+ * @brief Retrieves the number of elements of the hash map.
  *
  * Returns the number of elements stored in the
- * specified hash map. Elements marked for
- * removal are not included.
+ * specified hash map.
  *
  * @param[in] map Pointer to the hash map to count the
  * number of elements, may NOT be NULL, so
  * take caution.
  * @return Number of elements currently in use by the
- * hash map. Buckets marked for removal are
- * not counted.
+ * hash map.
  */
 uint iscsi_hashmap_size(const iscsi_hashmap *map)
 {
-	return (map->count - map->removed_count);
+	return map->count;
 }
 
 /**
- * @brief Iterator with callback function invoked on each element which has not been removed.
+ * @brief Iterator with callback function invoked on each element.
  *
  * An iterator through the elements of a
  * specified hash map which uses a callback
- * function for each element not marked for
- * removal, which also can abort the iteration,
- * if necessary.\n
+ * function for each element, which also
+ * can abort the iteration, if necessary.\n
  * It is safe to remove the current iterating
  * element in the callback function from the
  * hash map.
@@ -1441,8 +1242,7 @@ uint iscsi_hashmap_size(const iscsi_hashmap *map)
  * @param[in] map Pointer to the hash map to iterate
  * through, may NOT be NULL, so take caution.
  * @param[in] callback Callback function to be
- * invoked for each element not marked for
- * removal in the hash map. If the return
+ * invoked for each element. If the return
  * value of the callback function is below
  * zero, the iteration will stop.
  * @param[in,out] user_data Pointer to user specific data
@@ -1454,508 +1254,24 @@ uint iscsi_hashmap_size(const iscsi_hashmap *map)
  */
 int iscsi_hashmap_iterate(iscsi_hashmap *map, iscsi_hashmap_callback callback, uint8_t *user_data)
 {
-	iscsi_hashmap_bucket *current = map->first;
+	iscsi_hashmap_bucket *current;
+	iscsi_hashmap_bucket *tmp;
 	int err = 0;
 
-	while ( current != NULL ) {
-		if ( current->key != NULL ) {
-			err = callback( current->key, current->key_size, current->value, user_data );
+	iscsi_list_foreach_safe_node ( &map->list, current, tmp ) {
+		if ( current->key == NULL )
+			continue;
 
-			if ( err < 0 )
-				break;
-		}
+		err = callback( current->key, current->key_size, current->value, user_data );
 
-		current = current->next;
+		if ( err < 0 )
+			break;
 	}
 
 	return err;
 }
 
 /**
- * @brief Allocate and initialize an iSCSI BHS packet.
- *
- * Allocates an iSCSI packet data Basic Header Segment (BHS)
- * and zero fills the structure.
- *
- * @return a pointer to BHS structure with all fields
- * initialized or NULL if the allocation failed.
- */
-iscsi_bhs_packet *iscsi_create_packet()
-{
-	iscsi_bhs_packet *bhs_pkt = (iscsi_bhs_packet *) malloc( sizeof(struct iscsi_bhs_packet) );
-
-	if ( bhs_pkt == NULL ) {
-		logadd( LOG_ERROR, "iscsi_create_packet: Out of memory while allocating BHS iSCSI packet data" );
-
-		return bhs_pkt;
-	}
-
-	bhs_pkt->opcode           = 0U; // Initialize everything to zero
-	bhs_pkt->opcode_fields[0] = 0U;
-	bhs_pkt->opcode_fields[1] = 0U;
-	bhs_pkt->opcode_fields[2] = 0U;
-	bhs_pkt->total_ahs_len    = 0U;
-	bhs_pkt->ds_len[0]        = 0U;
-	bhs_pkt->ds_len[1]        = 0U;
-	bhs_pkt->ds_len[2]        = 0U;
-	bhs_pkt->lun_opcode.lun   = 0ULL;
-	bhs_pkt->init_task_tag    = 0UL;
-
-	memset( bhs_pkt->opcode_spec_fields, 0, sizeof(bhs_pkt->opcode_spec_fields) );
-
-	return bhs_pkt;
-}
-
-/**
- * @brief Free resources allocated by iscsi_create_packet.
- *
- * Deallocates all aquired resources by iscsi_create_packet.
- *
- * @param[in] packet_data Pointer to packet data to deallocate. If this is
- * NULL, this function does nothing.
- */
-void iscsi_destroy_packet(iscsi_bhs_packet *packet_data)
-{
-	if ( packet_data != NULL )
-		free( packet_data );
-}
-
-/**
- * @brief Allocate and initialize an iSCSI AHS packet and append to existing data stream.
- *
- * Constructs and appends an Additional Header Segment (AHS) to already allocated
- * packet data. There is no guarantee that the pointer stays the same. Any references
- * to the old structure need to be updated!\n
- * This function currently throws away any data beyond AHS.
- *
- * @param[in] packet_data Pointer to packet data to append to. If NULL, a Basic
- * Header Segment (BHS) will be created and initialized before adding a first
- * AHS.
- * @param[in] ahs_len Length of AHS packet data to be appended.
- * @return New pointer to BHS structure with additional AHS attached or NULL in case
- * of an reallocation error or total AHS length exceeds 255 DWORD's.
- */
-iscsi_bhs_packet *iscsi_append_ahs_packet(iscsi_bhs_packet *packet_data, const uint32_t ahs_len)
-{
-	if ( packet_data == NULL ) {
-		packet_data = iscsi_create_packet();
-
-		if ( packet_data == NULL )
-			return packet_data;
-	}
-
-	const uint32_t old_pkt_size = (const uint32_t) sizeof(struct iscsi_bhs_packet) + (packet_data->total_ahs_len << 2UL);
-	const uint32_t new_pkt_size = (uint32_t) (old_pkt_size + iscsi_align(ahs_len, ISCSI_ALIGN_SIZE));
-
-	if ( new_pkt_size > (sizeof(struct iscsi_bhs_packet) + ISCSI_MAX_AHS_SIZE) ) {
-		logadd( LOG_ERROR, "iscsi_append_ahs_packet: Total numer of AHS packet size exceeds 255 DWORDs" );
-
-		return NULL;
-	}
-
-	packet_data = (iscsi_bhs_packet *) realloc( packet_data, new_pkt_size );
-
-	if ( packet_data == NULL ) {
-		logadd( LOG_ERROR, "iscsi_append_ahs_packet: Out of memory while allocating iSCSI AHS packet data for appending" );
-
-		return packet_data;
-	}
-
-	iscsi_ahs_packet *ahs_pkt = (iscsi_ahs_packet *) ((uint8_t *) packet_data + old_pkt_size);
-	ahs_pkt->len      = iscsi_get_be16((uint16_t) ahs_len);
-	ahs_pkt->type     = 0;
-	ahs_pkt->specific = 0;
-	memset( ahs_pkt->data, 0, (new_pkt_size - old_pkt_size) - offsetof(struct iscsi_ahs_packet, data) );
-	packet_data->total_ahs_len += (uint8_t) ((ahs_len + (ISCSI_ALIGN_SIZE - 1)) >> 2UL);
-
-	return packet_data;
-}
-
-/**
- * @brief Counts number of AHS packets in an iSCSI data packet stream.
- *
- * Gets the total number of AHS packets.
- *
- * @param[in] packet_data Pointer to packet data of which the
- * number of AHS packets should be counted.
- * @return The number of AHS packets or zero in case none exist or
- * -1 in case of error.
- */
-int iscsi_get_ahs_packets(const iscsi_bhs_packet *packet_data)
-{
-	if ( packet_data == NULL )
-		return -1;
-	else if ( packet_data->total_ahs_len == 0U )
-		return 0;
-
-	iscsi_ahs_packet *ahs_pkt = (iscsi_ahs_packet *) ((iscsi_bhs_packet *) packet_data + 1); // First AHS packet
-	int count = 0;
-	uint32_t ahs_len = ((uint32_t) packet_data->total_ahs_len << 2UL);
-
-	while ( (int32_t) ahs_len > 0L ) {
-		uint32_t len = iscsi_get_be16(ahs_pkt->len) + offsetof(struct iscsi_ahs_packet, data); // Total length of current AHS packet
-
-		len      = iscsi_align(len, ISCSI_ALIGN_SIZE);
-		ahs_len -= len;
-		ahs_pkt  = (iscsi_ahs_packet *) (((uint8_t *) ahs_pkt) + (len - offsetof(struct iscsi_ahs_packet, data))); // Advance pointer to next AHS packet
-		count++;
-	}
-
-	return count;
-}
-
-/**
- * @brief Retrieves the pointer to an specific AHS packet by index.
- *
- * Gets the pointer of an AHS packet by specified index.
- *
- * @param[in] packet_data Pointer to packet data of which the
- * AHS packet should be retrieved.
- * @param[in] index Zero-based index number of AHS packet to
- * be received.
- * @return The pointer to the AHS packet at specified index on
- * success or NULL in case of an error or if the specific index
- * is out of range.
- */
-iscsi_ahs_packet *iscsi_get_ahs_packet(const iscsi_bhs_packet *packet_data, const int index)
-{
-	if ( packet_data == NULL || (packet_data->total_ahs_len == 0U) )
-		return NULL;
-
-	iscsi_ahs_packet *ahs_pkt = (iscsi_ahs_packet *) ((iscsi_bhs_packet *) packet_data + 1); // First AHS packet
-	int count = index;
-	uint32_t ahs_len = ((uint32_t) packet_data->total_ahs_len << 2UL);
-
-	while ( (int32_t) ahs_len > 0L ) {
-		if ( count-- < 0 )
-			return ahs_pkt;
-
-		uint32_t len = iscsi_get_be16(ahs_pkt->len) + offsetof(struct iscsi_ahs_packet, data); // Total length of current AHS packet
-
-		len      = iscsi_align(len, ISCSI_ALIGN_SIZE);
-		ahs_len -= len;
-		ahs_pkt  = (iscsi_ahs_packet *) (((uint8_t *) ahs_pkt) + (len - offsetof(struct iscsi_ahs_packet, data))); // Advance pointer to next AHS packet
-	}
-
-	logadd( LOG_ERROR, "iscsi_get_ahs_packet: Specified index for AHS packet does not exist" );
-
-	return NULL;
-}
-
-/**
- * @brief Allocate and initialize an iSCSI header digest (CRC32C) and appends it to existing data stream.
- *
- * Constructs and appends an header digest (CRC32C) to already allocated
- * packet data. There is no guarantee that the pointer stays the same.
- * Any references to the old structure need to be updated!\n
- * This function currently throws away any data beyond AHS.
- *
- * @param[in] packet_data Pointer to packet data to append to. If NULL, a Basic
- * Header Segment (BHS) will be created and initialized before adding the
- * header digest.
- * @param[in] header_digest_size Length of header digest. Currently, only
- * 0, in which case the header digest will be removed, or 4 for CRC32C
- * are allowed.
- * @return New pointer to BHS structure with additional header digest attached
- * or NULL in case of an reallocation error or header digest is neither 0 nor 4.
- */
-iscsi_bhs_packet *iscsi_append_header_digest_packet(iscsi_bhs_packet *packet_data, const int header_digest_size)
-{
-	if ( packet_data == NULL ) {
-		packet_data = iscsi_create_packet();
-
-		if ( packet_data == NULL )
-			return packet_data;
-	}
-
-	if ( (header_digest_size != 0) || (header_digest_size != ISCSI_DIGEST_SIZE) ) {
-		logadd( LOG_ERROR, "iscsi_append_header_digest_packet: Header digest size MUST be either 0 or 4 bytes" );
-
-		return NULL;
-	}
-
-	const uint32_t old_pkt_size = (const uint32_t) sizeof(struct iscsi_bhs_packet) + (packet_data->total_ahs_len << 2UL);
-	const uint32_t new_pkt_size = old_pkt_size + header_digest_size;
-
-	packet_data = (iscsi_bhs_packet *) realloc( packet_data, new_pkt_size );
-
-	if ( packet_data == NULL ) {
-		logadd( LOG_ERROR, "iscsi_append_header_digest_packet: Out of memory while allocating iSCSI header digest packet data for appending" );
-
-		return packet_data;
-	}
-
-	memset( (((uint8_t *) packet_data) + old_pkt_size), 0, header_digest_size );
-
-	return packet_data;
-}
-
-/**
- * @brief Allocate and initialize an iSCSI DS packet and append to existing data stream.
- *
- * Constructs and appends DataSegment (DS) to already allocated packet data.\n
- * There is no guarantee that the pointer stays the same. Any references
- * to the old structure need to be updated!\n
- * This function currently erases an already available DataSegment and
- * throws away any data beyond DS.
- *
- * @param[in] packet_data Pointer to BHS packet data to append to. If NULL, a Basic
- * Header Segment (BHS) will be created and initialized before adding the DataSegment.
- * @param[in] header_digest_size Length of optional header digest (0 or 4 for now) to
- * add.
- * @param[in] ds_len Length of DataSegment packet data to be appended. May
- * not exceed 16MiB - 1 (16777215 bytes).
- * @param[in] data_digest_size Length of optional data digest (0 or 4 for now) to
- * add.
- * @return New pointer to BHS structure with additional DataSegment attached or
- * NULL in case of an reallocation error, either header or data digest size does not
- * confirm to the iSCSI standard or DS length exceeds 16777215 bytes.
- */
-iscsi_bhs_packet *iscsi_append_ds_packet(iscsi_bhs_packet *packet_data, const int header_digest_size, const uint32_t ds_len, const int data_digest_size)
-{
-	if ( ((header_digest_size != 0) && header_digest_size != ISCSI_DIGEST_SIZE) || ((data_digest_size != 0) && data_digest_size != ISCSI_DIGEST_SIZE) || (ds_len >= 16777216UL) )
-		return NULL;
-
-	if ( packet_data == NULL ) {
-		packet_data = iscsi_create_packet();
-
-		if ( packet_data == NULL )
-			return packet_data;
-	}
-
-	const uint32_t old_pkt_size = (const uint32_t) sizeof(struct iscsi_bhs_packet) + ((uint32_t) packet_data->total_ahs_len << 2UL) + header_digest_size;
-	const uint32_t new_pkt_size = (uint32_t) (old_pkt_size + iscsi_align(ds_len, ISCSI_ALIGN_SIZE) + data_digest_size);
-
-	packet_data = (iscsi_bhs_packet *) realloc( packet_data, new_pkt_size );
-
-	if ( packet_data == NULL ) {
-		logadd( LOG_ERROR, "iscsi_append_ds_packet: Out of memory while allocating iSCSI DS packet data for appending" );
-
-		return packet_data;
-	}
-
-	iscsi_put_be24( (uint8_t *) &packet_data->ds_len, ds_len );
-	memset( ((uint8_t *) packet_data) + old_pkt_size, 0, (new_pkt_size - old_pkt_size) );
-
-	return packet_data;
-}
-
-/// CRC32C lookup table. Created with a polynomial reflect value of 0x82F63B78.
-static const uint32_t crc32c_lut[] = {
-	0x00000000, 0xF26B8303, 0xE13B70F7, 0x1350F3F4, 0xC79A971F, 0x35F1141C, 0x26A1E7E8, 0xD4CA64EB,
-	0x8AD958CF, 0x78B2DBCC, 0x6BE22838, 0x9989AB3B, 0x4D43CFD0, 0xBF284CD3, 0xAC78BF27, 0x5E133C24,
-	0x105EC76F, 0xE235446C, 0xF165B798, 0x030E349B, 0xD7C45070, 0x25AFD373, 0x36FF2087, 0xC494A384,
-	0x9A879FA0, 0x68EC1CA3, 0x7BBCEF57, 0x89D76C54, 0x5D1D08BF, 0xAF768BBC, 0xBC267848, 0x4E4DFB4B,
-	0x20BD8EDE, 0xD2D60DDD, 0xC186FE29, 0x33ED7D2A, 0xE72719C1, 0x154C9AC2, 0x061C6936, 0xF477EA35,
-	0xAA64D611, 0x580F5512, 0x4B5FA6E6, 0xB93425E5, 0x6DFE410E, 0x9F95C20D, 0x8CC531F9, 0x7EAEB2FA,
-	0x30E349B1, 0xC288CAB2, 0xD1D83946, 0x23B3BA45, 0xF779DEAE, 0x05125DAD, 0x1642AE59, 0xE4292D5A,
-	0xBA3A117E, 0x4851927D, 0x5B016189, 0xA96AE28A, 0x7DA08661, 0x8FCB0562, 0x9C9BF696, 0x6EF07595,
-	0x417B1DBC, 0xB3109EBF, 0xA0406D4B, 0x522BEE48, 0x86E18AA3, 0x748A09A0, 0x67DAFA54, 0x95B17957,
-	0xCBA24573, 0x39C9C670, 0x2A993584, 0xD8F2B687, 0x0C38D26C, 0xFE53516F, 0xED03A29B, 0x1F682198,
-	0x5125DAD3, 0xA34E59D0, 0xB01EAA24, 0x42752927, 0x96BF4DCC, 0x64D4CECF, 0x77843D3B, 0x85EFBE38,
-	0xDBFC821C, 0x2997011F, 0x3AC7F2EB, 0xC8AC71E8, 0x1C661503, 0xEE0D9600, 0xFD5D65F4, 0x0F36E6F7,
-	0x61C69362, 0x93AD1061, 0x80FDE395, 0x72966096, 0xA65C047D, 0x5437877E, 0x4767748A, 0xB50CF789,
-	0xEB1FCBAD, 0x197448AE, 0x0A24BB5A, 0xF84F3859, 0x2C855CB2, 0xDEEEDFB1, 0xCDBE2C45, 0x3FD5AF46,
-	0x7198540D, 0x83F3D70E, 0x90A324FA, 0x62C8A7F9, 0xB602C312, 0x44694011, 0x5739B3E5, 0xA55230E6,
-	0xFB410CC2, 0x092A8FC1, 0x1A7A7C35, 0xE811FF36, 0x3CDB9BDD, 0xCEB018DE, 0xDDE0EB2A, 0x2F8B6829,
-	0x82F63B78, 0x709DB87B, 0x63CD4B8F, 0x91A6C88C, 0x456CAC67, 0xB7072F64, 0xA457DC90, 0x563C5F93,
-	0x082F63B7, 0xFA44E0B4, 0xE9141340, 0x1B7F9043, 0xCFB5F4A8, 0x3DDE77AB, 0x2E8E845F, 0xDCE5075C,
-	0x92A8FC17, 0x60C37F14, 0x73938CE0, 0x81F80FE3, 0x55326B08, 0xA759E80B, 0xB4091BFF, 0x466298FC,
-	0x1871A4D8, 0xEA1A27DB, 0xF94AD42F, 0x0B21572C, 0xDFEB33C7, 0x2D80B0C4, 0x3ED04330, 0xCCBBC033,
-	0xA24BB5A6, 0x502036A5, 0x4370C551, 0xB11B4652, 0x65D122B9, 0x97BAA1BA, 0x84EA524E, 0x7681D14D,
-	0x2892ED69, 0xDAF96E6A, 0xC9A99D9E, 0x3BC21E9D, 0xEF087A76, 0x1D63F975, 0x0E330A81, 0xFC588982,
-	0xB21572C9, 0x407EF1CA, 0x532E023E, 0xA145813D, 0x758FE5D6, 0x87E466D5, 0x94B49521, 0x66DF1622,
-	0x38CC2A06, 0xCAA7A905, 0xD9F75AF1, 0x2B9CD9F2, 0xFF56BD19, 0x0D3D3E1A, 0x1E6DCDEE, 0xEC064EED,
-	0xC38D26C4, 0x31E6A5C7, 0x22B65633, 0xD0DDD530, 0x0417B1DB, 0xF67C32D8, 0xE52CC12C, 0x1747422F,
-	0x49547E0B, 0xBB3FFD08, 0xA86F0EFC, 0x5A048DFF, 0x8ECEE914, 0x7CA56A17, 0x6FF599E3, 0x9D9E1AE0,
-	0xD3D3E1AB, 0x21B862A8, 0x32E8915C, 0xC083125F, 0x144976B4, 0xE622F5B7, 0xF5720643, 0x07198540,
-	0x590AB964, 0xAB613A67, 0xB831C993, 0x4A5A4A90, 0x9E902E7B, 0x6CFBAD78, 0x7FAB5E8C, 0x8DC0DD8F,
-	0xE330A81A, 0x115B2B19, 0x020BD8ED, 0xF0605BEE, 0x24AA3F05, 0xD6C1BC06, 0xC5914FF2, 0x37FACCF1,
-	0x69E9F0D5, 0x9B8273D6, 0x88D28022, 0x7AB90321, 0xAE7367CA, 0x5C18E4C9, 0x4F48173D, 0xBD23943E,
-	0xF36E6F75, 0x0105EC76, 0x12551F82, 0xE03E9C81, 0x34F4F86A, 0xC69F7B69, 0xD5CF889D, 0x27A40B9E,
-	0x79B737BA, 0x8BDCB4B9, 0x988C474D, 0x6AE7C44E, 0xBE2DA0A5, 0x4C4623A6, 0x5F16D052, 0xAD7D5351};
-
-/**
- * @brief Calculates digest (CRC32C).
- *
- * Calculates CRC32C with 0x82F63B78 polynomial reflect according to iSCSI specs.\n
- * TODO: Implement optimized SSE4.2 and ARM versions
- *
- * @param[in] data Pointer to data to calculate CRC32C for.
- * @param[in] len Length of data to be calculated. Must be
- * divisable by 4 which is guaranteed by iSCSI standard.
- * @param[in] crc32c Previous CRC32C in case of multiple passes.
- * @return CRC32C value. THis function cannot fail.
- */
-static inline uint32_t iscsi_crc32c_update(const uint8_t *data, const uint len, uint32_t crc32c)
-{
-	for ( uint i = 0; i < len; i += 4 ) {
-		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i]) & 0xFF];
-		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i + 1]) & 0xFF];
-		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i + 2]) & 0xFF];
-		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i + 3]) & 0xFF];
-	}
-
-	return crc32c;
-}
-
-/**
- * @brief Calculate and store iSCSI header digest (CRC32C).
- *
- * Calculates header digest (CRC32C) with 0x82F63B78 polynomial reflect
- * according to iSCSI specs and stores the result in the iSCSI packet
- * data. This function cannot fail.
- *
- * @param[in] packet_data Pointer to ISCSI BHS packet to calculate CRC32C for.
- */
-void iscsi_calc_header_digest(const iscsi_bhs_packet *packet_data)
-{
-	const uint32_t len = sizeof(struct iscsi_bhs_packet) + ((const uint32_t) packet_data->total_ahs_len << 2UL);
-	uint8_t *hdr_digest = ((uint8_t *) packet_data) + len;
-	const uint32_t crc32c = iscsi_crc32c_update( (const uint8_t *) packet_data, iscsi_align(len, ISCSI_DIGEST_SIZE), ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
-
-	iscsi_put_be32( hdr_digest, crc32c );
-}
-
-/**
- * @brief Validates a stored iSCSI header digest (CRC32C) with actual header data.
- *
- * Verifies header digest (CRC32C) with 0x82F63B78 polynomial reflect
- * according to iSCSI specs. This function cannot fail.
- *
- * @param[in] packet_data Pointer to ISCSI BHS packet to validate CRC32C for.
- * @return true if CRC32C matches the stored value, false otherwise.
- */
-int iscsi_validate_header_digest(const iscsi_bhs_packet *packet_data)
-{
-	const uint32_t len = sizeof(struct iscsi_bhs_packet) + ((const uint32_t) packet_data->total_ahs_len << 2UL);
-	const uint8_t *hdr_digest = ((uint8_t *) packet_data) + len;
-	const uint32_t pkt_crc32c = *(uint32_t *) hdr_digest;
-	const uint32_t crc32c = iscsi_crc32c_update( (const uint8_t *) packet_data, len, ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
-
-	return iscsi_get_be32(pkt_crc32c) == crc32c;
-}
-
-/**
- * @brief Calculate iSCSI data digest (CRC32C).
- *
- * Calculates data digest (CRC32) with 0x82F63B78 polynomial reflect
- * of a whole DataSegment (CRC32C) according to the iSCSI specs.\n
- * The resulting CRC32C will be stored in the iSCSI packet.
- *
- * @param[in] packet_data Pointer to ISCSI DS packet to calculate CRC32C for.
- * @param[in] header_digest_size Length of optional header digest (0 or 4 for now) in
- * order to calculate correct DataSegment index. The header digest size IS NOT checked
- * for conforming to iSCSI specs, so be careful.
- */
-void iscsi_calc_data_digest(const iscsi_bhs_packet *packet_data, const int header_digest_size)
-{
-	const uint32_t ds_idx = (const uint32_t) sizeof(struct iscsi_bhs_packet) + ((const uint32_t) packet_data->total_ahs_len << 2UL) + header_digest_size;
-	const uint8_t *data = ((uint8_t *) packet_data) + ds_idx;
-	const uint32_t ds_len = iscsi_get_be24(packet_data->ds_len);
-	const uint32_t len = iscsi_align(ds_len, ISCSI_DIGEST_SIZE);
-	uint8_t *data_digest = ((uint8_t *) packet_data) + ds_idx + len;
-	const uint32_t crc32c = iscsi_crc32c_update( data, len, ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
-
-	iscsi_put_be32( data_digest, crc32c );
-}
-
-/**
- * @brief Validates a stored iSCSI data digest (CRC32C) with actual DataSegment.
- *
- * Verifies data digest (CRC32C) with 0x82F63B78 polynomial reflect
- * according to iSCSI specs. This function cannot fail.
- *
- * @param[in] packet_data Pointer to ISCSI BHS packet to calculate CRC32C for.
- * @param[in] header_digest_size Length of optional header digest (0 or 4 for now) in
- * order to calculate correct DataSegment index. The header digest size IS NOT checked
- * for conforming to iSCSI specs, so be careful.
- * @return true if CRC32C matches the stored value, false otherwise.
- */
-int iscsi_validate_data_digest(const iscsi_bhs_packet *packet_data, const int header_digest_size)
-{
-	const uint32_t ds_idx = (const uint32_t) sizeof(struct iscsi_bhs_packet) + ((const uint32_t) packet_data->total_ahs_len << 2UL) + header_digest_size;
-	const uint8_t *data = ((uint8_t *) packet_data) + ds_idx;
-	const uint32_t ds_len = iscsi_get_be24(packet_data->ds_len);
-	const uint32_t len = iscsi_align(ds_len, ISCSI_DIGEST_SIZE);
-	const uint8_t *data_digest = data + len;
-	const uint32_t pkt_crc32c = *(uint32_t *) data_digest;
-	const uint32_t crc32c = iscsi_crc32c_update( (const uint8_t *) data, len, ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
-
-	return iscsi_get_be32(pkt_crc32c) == crc32c;
-}
-
-/**
- * @brief Validates a single text key / value pair according to iSCSI specs.
- *
- * Validates an iSCSI protocol key and value pair for compliance
- * with the iSCSI specs.
- *
- * @param[in] packet_data Pointer to key / value pair to be
- * validated. NULL is an illegal value, so be careful.
- * @param[in] len Length of the remaining packet data.
- * @return Number of bytes used by the key / vair pair or
- * a negative value in case of an error. This can be used for
- * incrementing the offset to the next key / value pair.
- */
-static int iscsi_validate_text_key_value_pair(const uint8_t *packet_data, const uint32_t len)
-{
-	const uint key_val_len = (uint) strnlen( (char *) packet_data, len );
-	const uint8_t *key_end = memchr( packet_data, '=', key_val_len );
-
-	if ( key_end == NULL )
-		return ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS; // Missing separator '=' for key / value pair -> invalid iSCSI packet data
-
-	const uint key_len = (uint) (key_end - packet_data);
-
-	if ( key_len == 0U )
-		return ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS; // Zero length is not allowed -> invalid iSCSI packet data
-
-	if ( key_len > ISCSI_TEXT_KEY_MAX_LEN )
-		return ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS;
-
-	const uint val_len = (uint) strnlen( (char *) (key_end + 1UL), (key_val_len - key_len - 1U) );
-	const uint max_len = (memcmp( packet_data, "CHAP_C=", (key_len + 1U) ) == 0) || (memcmp( packet_data, "CHAP_R=", (key_len + 1U) ) == 0) ? ISCSI_TEXT_VALUE_MAX_LEN : ISCSI_TEXT_VALUE_MAX_SIMPLE_LEN;
-
-	if ( val_len > max_len )
-		return ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS; // Value exceeds maximum length -> invalid iSCSI packet data
-
-	return (int) (key_len + 1U + val_len + 1U); // Number of bytes for processed key / value pair (+1 for '=' and NUL terminator)
-}
-
-/**
- * @brief Validates all text key / value pairs according to iSCSI specs.
- *
- * Validates all iSCSI protocol key and value pairs for
- * compliance with the iSCSI specs.
- *
- * @param[in] packet_data Pointer to first key and value pair to
- * be validated. NULL is an illegal value here, so be careful.
- * @param[in] len Length of the remaining packet data.
- * @return 0 if validation for each text key and value pair was
- * successful, a negative error code in case iSCSI specs
- * are violated.
- */
-static int iscsi_validate_key_value_pairs(const uint8_t *packet_data, uint len)
-{
-	if ( len == 0U )
-		return ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS; // Zero length is not allowed -> invalid iSCSI packet data
-
-	int offset = 0;
-
-	while ( ((uint) offset < len) && (packet_data[offset] != '\0') ) {
-		const int rc = iscsi_validate_text_key_value_pair( (packet_data + offset), (len - offset) );
-
-		if ( rc < ISCSI_VALIDATE_PACKET_RESULT_OK )
-			return rc;
-
-		offset += rc;
-	}
-
-	return (iscsi_align(offset, ISCSI_ALIGN_SIZE) != iscsi_align(len, ISCSI_ALIGN_SIZE)) ? ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS : ISCSI_VALIDATE_PACKET_RESULT_OK;
-}
-
-/**
  * @brief Extracts a single text key / value pairs out of an iSCSI packet into a hash map.
  *
  * Parses and extracts a specific key and value pair out of an iSCSI packet
@@ -2023,7 +1339,7 @@ static int iscsi_parse_text_key_value_pair(iscsi_hashmap *key_value_pairs, const
 		return -1;
 	}
 
-	uint8_t *hash_val = (uint8_t *) malloc( iscsi_align(val_len, ISCSI_TEXT_VALUE_ALIGN) );
+	uint8_t *hash_val = (uint8_t *) malloc( ISCSI_ALIGN(val_len, ISCSI_TEXT_VALUE_ALIGN) );
 
 	if ( hash_val == NULL ) {
 		logadd( LOG_ERROR, "iscsi_parse_text_key_value_pair: Out of memory allocating memory for value string" );
@@ -2185,7 +1501,7 @@ static int iscsi_add_key_value_pair(iscsi_hashmap *key_value_pairs, const uint8_
 	}
 
 	const uint val_len = (uint) (strlen( (char *) value ) + 1U);
-	uint8_t *hash_val = (uint8_t *) malloc( iscsi_align(val_len, ISCSI_TEXT_VALUE_ALIGN) );
+	uint8_t *hash_val = (uint8_t *) malloc( ISCSI_ALIGN(val_len, ISCSI_TEXT_VALUE_ALIGN) );
 
 	if ( hash_val == NULL ) {
 		logadd( LOG_ERROR, "iscsi_add_key_value_pair: Out of memory allocating string value" );
@@ -2229,7 +1545,7 @@ static int iscsi_update_key_value_pair(iscsi_hashmap *key_value_pairs, const uin
 	}
 
 	const uint val_len = (uint) (strlen( (char *) value ) + 1U);
-	uint8_t *hash_val = (uint8_t *) malloc( iscsi_align(val_len, ISCSI_TEXT_VALUE_ALIGN) );
+	uint8_t *hash_val = (uint8_t *) malloc( ISCSI_ALIGN(val_len, ISCSI_TEXT_VALUE_ALIGN) );
 
 	if ( hash_val == NULL ) {
 		logadd( LOG_ERROR, "iscsi_update_key_value_pair: Out of memory allocating string value" );
@@ -2287,7 +1603,7 @@ static int iscsi_get_int_key_value_pair(iscsi_hashmap *key_value_pairs, const ui
  */
 static int iscsi_add_int_key_value_pair(iscsi_hashmap *key_value_pairs, const uint8_t *key, const int32_t value)
 {
-	const uint8_t *hash_val = iscsi_sprintf_alloc( "%d", value );
+	const uint8_t *hash_val = iscsi_sprintf_alloc( "%" PRId32, value );
 
 	if ( hash_val == NULL ) {
 		logadd( LOG_ERROR, "iscsi_add_int_key_value_pair: Out of memory allocating integer value." );
@@ -2317,7 +1633,7 @@ static int iscsi_add_int_key_value_pair(iscsi_hashmap *key_value_pairs, const ui
  */
 static int iscsi_update_int_key_value_pair(iscsi_hashmap *key_value_pairs, const uint8_t *key, const int32_t value)
 {
-	const uint8_t *hash_val = iscsi_sprintf_alloc( "%d", value );
+	const uint8_t *hash_val = iscsi_sprintf_alloc( "%" PRId32, value );
 
 	if ( hash_val == NULL ) {
 		logadd( LOG_ERROR, "iscsi_update_int_key_value_pair: Out of memory allocating integer value." );
@@ -2506,11 +1822,6 @@ void iscsi_task_destroy_callback(iscsi_scsi_task *scsi_task)
 		iscsi_task *sub_task;
 		iscsi_task *tmp;
 
-		iscsi_list_foreach_safe_node ( &task->sub_tasks, sub_task, tmp ) {
-			iscsi_list_remove( &sub_task->node );
-			iscsi_task_destroy( sub_task );
-		}
-
 		if ( task->parent != NULL ) {
 			if ( (task->scsi_task.flags & ISCSI_SCSI_TASK_FLAGS_XFER_READ) != 0 )
 				task->conn->scsi_data_in_cnt--;
@@ -2616,13 +1927,9 @@ static void iscsi_task_xfer_complete_process_read_sub_tasks(iscsi_connection *co
 	iscsi_task *tmp;
 
 	iscsi_list_foreach_safe_node ( &primary_task->sub_tasks, sub_task, tmp ) {
-		logadd( LOG_ERROR, "DEBUG iscsi_task_xfer_complete_process_read_sub_tasks: xfer_pos subtask->xfer_pos = %d, subtask->xfer_pos = %d, subtask->pos = %d, subtask->len = %d, scsi->xfer_pos = %d, scsi->xfer_len = %d, scsi->pos = %d, scsi->len = %d", sub_task->des_data_xfer_pos, sub_task->des_data_xfer_len, sub_task->pos, sub_task->len, sub_task->scsi_task.xfer_pos, sub_task->scsi_task.xfer_len, sub_task->scsi_task.pos, sub_task->scsi_task.len );
-
 		if ( primary_task->des_data_xfer_pos != sub_task->scsi_task.pos )
 			break;
 
-		logadd( LOG_ERROR, "DEBUG iscsi_task_xfer_complete_process_read_sub_tasks: RESPONDING: xfer_pos subtask->xfer_pos = %d, subtask->xfer_pos = %d, subtask->pos = %d, subtask->len = %d, scsi->xfer_pos = %d, scsi->xfer_len = %d, scsi->pos = %d, scsi->len = %d", sub_task->des_data_xfer_pos, sub_task->des_data_xfer_len, sub_task->pos, sub_task->len, sub_task->scsi_task.xfer_pos, sub_task->scsi_task.xfer_len, sub_task->scsi_task.pos, sub_task->scsi_task.len );
-
 		iscsi_list_remove( &sub_task->node );
 
 		primary_task->des_data_xfer_pos += sub_task->scsi_task.len;
@@ -2680,33 +1987,22 @@ void iscsi_task_xfer_complete_process_read(iscsi_connection *conn, iscsi_task *t
 
 		iscsi_task_response( conn, task );
 		iscsi_task_destroy( task );
-	} else {
-		if ( task->scsi_task.pos != primary_task->des_data_xfer_pos ) {
-			iscsi_task *sub_task;
-
-			iscsi_list_foreach_node ( &primary_task->sub_tasks, sub_task ) {
-				if ( task->scsi_task.pos < sub_task->scsi_task.pos ) {
-					iscsi_list_insert( &primary_task->sub_tasks, &sub_task->node, task->node.pred );
-
-					return;
-				}
-			}
+	} else if ( task->scsi_task.pos != primary_task->des_data_xfer_pos ) {
+		iscsi_task *sub_task;
 
-			iscsi_list_enqueue( &primary_task->sub_tasks, &task->node );
+		iscsi_list_foreach_node ( &primary_task->sub_tasks, sub_task ) {
+			if ( task->scsi_task.pos < sub_task->scsi_task.pos ) {
+				iscsi_list_insert( &primary_task->sub_tasks, &sub_task->node, task->node.pred );
 
-			iscsi_list_foreach_node ( &primary_task->sub_tasks, sub_task ) {
-				logadd( LOG_ERROR, "DEBUG iscsi_task_xfer_complete_process_read: pos NE xfer_pos subtask->xfer_pos = %d, subtask->xfer_pos = %d, subtask->pos = %d, subtask->len = %d, scsi->xfer_pos = %d, scsi->xfer_len = %d, scsi->pos = %d, scsi->len = %d", sub_task->des_data_xfer_pos, sub_task->des_data_xfer_len, sub_task->pos, sub_task->len, sub_task->scsi_task.xfer_pos, sub_task->scsi_task.xfer_len, sub_task->scsi_task.pos, sub_task->scsi_task.len );
+				return;
 			}
-		} else {
-			iscsi_list_push( &primary_task->sub_tasks, &task->node );
+		}
 
-			iscsi_task *sub_task;
-			iscsi_list_foreach_node ( &primary_task->sub_tasks, sub_task ) {
-				logadd( LOG_ERROR, "DEBUG iscsi_task_xfer_complete_process_read: pos NE xfer_pos subtask->xfer_pos = %d, subtask->xfer_pos = %d, subtask->pos = %d, subtask->len = %d, scsi->xfer_pos = %d, scsi->xfer_len = %d, scsi->pos = %d, scsi->len = %d", sub_task->des_data_xfer_pos, sub_task->des_data_xfer_len, sub_task->pos, sub_task->len, sub_task->scsi_task.xfer_pos, sub_task->scsi_task.xfer_len, sub_task->scsi_task.pos, sub_task->scsi_task.len );
-			}
+		iscsi_list_enqueue( &primary_task->sub_tasks, &task->node );
+	} else {
+		iscsi_list_push( &primary_task->sub_tasks, &task->node );
 
-			iscsi_task_xfer_complete_process_read_sub_tasks( conn, primary_task );
-		}
+		iscsi_task_xfer_complete_process_read_sub_tasks( conn, primary_task );
 	}
 }
 
@@ -2935,7 +2231,7 @@ static void iscsi_connection_pdu_scsi_data_in_complete(uint8_t *user_data)
  */
 static uint32_t iscsi_scsi_data_in_send(iscsi_connection *conn, iscsi_task *task, const uint32_t pos, const uint32_t len, const uint32_t res_cnt, const uint32_t data_sn, const int8_t flags)
 {
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, len, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_scsi_data_in_send: Out of memory while allocating iSCSI SCSI Data In response PDU" );
@@ -2943,38 +2239,14 @@ static uint32_t iscsi_scsi_data_in_send(iscsi_connection *conn, iscsi_task *task
 		return data_sn;
 	}
 
-	iscsi_scsi_data_in_response_packet *scsi_data_in_pkt = (iscsi_scsi_data_in_response_packet *) iscsi_append_ds_packet( response_pdu->bhs_pkt, conn->header_digest, len, conn->data_digest );
-
-	if ( scsi_data_in_pkt == NULL ) {
-		logadd( LOG_ERROR, "iscsi_scsi_data_in_send: Out of memory while allocating iSCSI SCSI Data In packet data" );
-
-		iscsi_connection_pdu_destroy( response_pdu );
-
-		return data_sn;
-	}
-
-	response_pdu->bhs_pkt = (iscsi_bhs_packet *) scsi_data_in_pkt;
-
-	if ( conn->header_digest != 0 ) {
-		response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) scsi_data_in_pkt) + 1);
-		response_pdu->header_digest_size = conn->header_digest;
-	}
-
-	response_pdu->ds_cmd_data = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) scsi_data_in_pkt) + sizeof(struct iscsi_bhs_packet) + conn->header_digest);
-	response_pdu->ds_len      = len;
-
-	if ( conn->data_digest != 0 ) {
-		response_pdu->data_digest      = (iscsi_data_digest *) (((uint8_t *) response_pdu->ds_cmd_data) + iscsi_align(len, ISCSI_ALIGN_SIZE));
-		response_pdu->data_digest_size = conn->data_digest;
-	}
-
-	memcpy( response_pdu->ds_cmd_data, (task->scsi_task.buf + pos), len );
-
 	response_pdu->task = task;
 	task->scsi_task.ref++;
 
-	scsi_data_in_pkt->opcode = ISCSI_OPCODE_SERVER_SCSI_DATA_IN;
-	scsi_data_in_pkt->flags  = (flags & ~(ISCSI_SCSI_DATA_IN_RESPONSE_FLAGS_RES_UNDERFLOW | ISCSI_SCSI_DATA_IN_RESPONSE_FLAGS_RES_OVERFLOW));
+	iscsi_scsi_data_in_response_packet *scsi_data_in_pkt = (iscsi_scsi_data_in_response_packet *) response_pdu->bhs_pkt;
+
+	scsi_data_in_pkt->opcode   = ISCSI_OPCODE_SERVER_SCSI_DATA_IN;
+	scsi_data_in_pkt->flags    = (flags & ~(ISCSI_SCSI_DATA_IN_RESPONSE_FLAGS_RES_UNDERFLOW | ISCSI_SCSI_DATA_IN_RESPONSE_FLAGS_RES_OVERFLOW));
+	scsi_data_in_pkt->reserved = 0U;
 
 	iscsi_task *primary_task = (task->parent != NULL) ? task->parent : task;
 
@@ -2986,13 +2258,20 @@ static uint32_t iscsi_scsi_data_in_send(iscsi_connection *conn, iscsi_task *task
 				conn->session->max_cmd_sn++;
 
 			iscsi_put_be32( (uint8_t *) &scsi_data_in_pkt->res_cnt, res_cnt );
+		} else {
+			scsi_data_in_pkt->res_cnt = 0UL;
 		}
 
 		scsi_data_in_pkt->status = task->scsi_task.status;
 		iscsi_put_be32( (uint8_t *) &scsi_data_in_pkt->stat_sn, conn->stat_sn++ );
+	} else {
+		scsi_data_in_pkt->status  = 0U;
+		scsi_data_in_pkt->stat_sn = 0UL;
+		scsi_data_in_pkt->res_cnt = 0UL;
 	}
 
-	iscsi_put_be24( (uint8_t *) &scsi_data_in_pkt->ds_len, len );
+	iscsi_put_be32( (uint8_t *) &scsi_data_in_pkt->total_ahs_len, len ); // TotalAHSLength is always 0 and DataSegmentLength is 24-bit, so write in one step.
+	scsi_data_in_pkt->lun             = 0ULL;
 	iscsi_put_be32( (uint8_t *) &scsi_data_in_pkt->init_task_tag, task->init_task_tag );
 	scsi_data_in_pkt->target_xfer_tag = 0xFFFFFFFFUL; // Minus one does not require endianess conversion
 	iscsi_put_be32( (uint8_t *) &scsi_data_in_pkt->exp_cmd_sn, conn->session->exp_cmd_sn );
@@ -3002,9 +2281,11 @@ static uint32_t iscsi_scsi_data_in_send(iscsi_connection *conn, iscsi_task *task
 	if ( conn->session->err_recovery_level > 0UL )
 		primary_task->data_sn = data_sn;
 
-	const uint32_t offset = (pos + task->scsi_task.pos);
+	const uint32_t offset = (task->scsi_task.pos + pos);
 	iscsi_put_be32( (uint8_t *) &scsi_data_in_pkt->buf_offset, offset );
 
+	memcpy( response_pdu->ds_cmd_data, (task->scsi_task.buf + pos), len );
+
 	iscsi_connection_pdu_write( conn, response_pdu, iscsi_connection_pdu_scsi_data_in_complete, (uint8_t *) conn );
 
 	return (data_sn + 1UL);
@@ -3117,7 +2398,8 @@ void iscsi_task_response(iscsi_connection *conn, iscsi_task *task)
 			return;
 	}
 
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	const uint32_t ds_len   = (task->scsi_task.sense_data_len != 0U) ? (task->scsi_task.sense_data_len + offsetof(struct iscsi_scsi_ds_cmd_data, sense_data)) : 0UL;
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, ds_len, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_task_response: Out of memory while allocating iSCSI SCSI response PDU" );
@@ -3125,69 +2407,25 @@ void iscsi_task_response(iscsi_connection *conn, iscsi_task *task)
 		return;
 	}
 
-	iscsi_scsi_response_packet *scsi_response_pkt;
-	uint32_t ds_len;
+	iscsi_scsi_response_packet *scsi_response_pkt = (iscsi_scsi_response_packet *) response_pdu->bhs_pkt;
 
 	if ( task->scsi_task.sense_data_len != 0U ) {
-		ds_len = (task->scsi_task.sense_data_len + offsetof(struct iscsi_scsi_ds_cmd_data, sense_data));
-		scsi_response_pkt = (iscsi_scsi_response_packet *) iscsi_append_ds_packet( response_pdu->bhs_pkt, conn->header_digest, ds_len, conn->data_digest );
-
-		if ( scsi_response_pkt == NULL ) {
-			logadd( LOG_ERROR, "iscsi_task_response: Out of memory while allocating iSCSI SCSI response packet data" );
-
-			iscsi_connection_pdu_destroy( response_pdu );
-
-			return;
-		}
-
-		response_pdu->bhs_pkt = (iscsi_bhs_packet *) scsi_response_pkt;
-
-		if ( conn->header_digest != 0 ) {
-			response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) scsi_response_pkt) + 1);
-			response_pdu->header_digest_size = conn->header_digest;
-		}
-
-		response_pdu->ds_cmd_data = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) scsi_response_pkt) + sizeof(struct iscsi_bhs_packet) + conn->header_digest);
-		response_pdu->ds_len      = ds_len;
-
-		if ( conn->data_digest != 0 ) {
-			response_pdu->data_digest      = (iscsi_data_digest *) (((uint8_t *) response_pdu->ds_cmd_data) + iscsi_align(ds_len, ISCSI_ALIGN_SIZE));
-			response_pdu->data_digest_size = conn->data_digest;
-		}
-
 		iscsi_scsi_ds_cmd_data *ds_cmd_data_pkt = response_pdu->ds_cmd_data;
 
 		iscsi_put_be16( (uint8_t *) &ds_cmd_data_pkt->len, task->scsi_task.sense_data_len );
 		memcpy( ds_cmd_data_pkt->sense_data, task->scsi_task.sense_data, task->scsi_task.sense_data_len );
 
-		iscsi_put_be24( (uint8_t *) &scsi_response_pkt->ds_len, ds_len );
+		iscsi_put_be32( (uint8_t *) &scsi_response_pkt->total_ahs_len, ds_len ); // TotalAHSLength is always 0 and DataSegmentLength is 24-bit, so write in one step.
 	} else {
-		ds_len = 0uL;
-
-		if ( conn->header_digest != 0 ) {
-			scsi_response_pkt = (iscsi_scsi_response_packet *) iscsi_append_header_digest_packet( response_pdu->bhs_pkt, conn->header_digest );
-
-			if ( scsi_response_pkt == NULL ) {
-				logadd( LOG_ERROR, "iscsi_task_response: Out of memory while allocating iSCSI SCSI response packet data" );
-
-				iscsi_connection_pdu_destroy( response_pdu );
-
-				return;
-			}
-
-			response_pdu->bhs_pkt            = (iscsi_bhs_packet *) scsi_response_pkt;
-			response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) scsi_response_pkt) + 1);
-			response_pdu->header_digest_size = conn->header_digest;
-		} else {
-			scsi_response_pkt = (iscsi_scsi_response_packet *) response_pdu->bhs_pkt;
-		}
+		*(uint32_t *) &scsi_response_pkt->total_ahs_len = 0UL; // TotalAHSLength and DataSegmentLength are always 0, so write in one step.
 	}
 
 	response_pdu->task = task;
 	task->scsi_task.ref++;
 
-	scsi_response_pkt->opcode = ISCSI_OPCODE_SERVER_SCSI_RESPONSE;
-	scsi_response_pkt->flags  = -0x80;
+	scsi_response_pkt->opcode   = ISCSI_OPCODE_SERVER_SCSI_RESPONSE;
+	scsi_response_pkt->flags    = -0x80;
+	scsi_response_pkt->response = ISCSI_SCSI_RESPONSE_CODE_OK;
 
 	const uint32_t pos = primary_task->scsi_task.xfer_pos;
 
@@ -3202,11 +2440,17 @@ void iscsi_task_response(iscsi_connection *conn, iscsi_task *task)
 
 			scsi_response_pkt->flags |= ISCSI_SCSI_RESPONSE_FLAGS_RES_OVERFLOW;
 			iscsi_put_be32( (uint8_t *) &scsi_response_pkt->res_cnt, res_cnt );
+		} else {
+			scsi_response_pkt->res_cnt = 0UL;
 		}
+	} else {
+		scsi_response_pkt->res_cnt = 0UL;
 	}
 
-	scsi_response_pkt->status = task->scsi_task.status;
+	scsi_response_pkt->status    = task->scsi_task.status;
+	scsi_response_pkt->reserved  = 0ULL;
 	iscsi_put_be32( (uint8_t *) &scsi_response_pkt->init_task_tag, task->init_task_tag );
+	scsi_response_pkt->snack_tag = 0UL;
 	iscsi_put_be32( (uint8_t *) &scsi_response_pkt->stat_sn, conn->stat_sn++ );
 
 	if ( (scsi_cmd_pkt->opcode & ISCSI_OPCODE_FLAGS_IMMEDIATE) == 0 )
@@ -3214,6 +2458,8 @@ void iscsi_task_response(iscsi_connection *conn, iscsi_task *task)
 
 	iscsi_put_be32( (uint8_t *) &scsi_response_pkt->exp_cmd_sn, conn->session->exp_cmd_sn );
 	iscsi_put_be32( (uint8_t *) &scsi_response_pkt->max_cmd_sn, conn->session->max_cmd_sn );
+	scsi_response_pkt->exp_data_sn       = 0UL;
+	scsi_response_pkt->bidi_read_res_cnt = 0UL;
 
 	iscsi_connection_pdu_write( conn, response_pdu, NULL, NULL );
 }
@@ -3308,9 +2554,9 @@ void iscsi_portal_group_destroy(iscsi_portal_group *portal_group)
  * This function allocates host:port of iSCSI portal for use
  * as key and sets the portal group in the portal.
  *
- * @param[in] iSCSI portal group to add portal to. May NOT be NULL,
+ * @param[in] portal_group iSCSI portal group to add portal to. May NOT be NULL,
  * so take caution.
- * @param[in] iSCSI portal to add to portal group. NULL is NOT
+ * @param[in] portal iSCSI portal to add to portal group. NULL is NOT
  * allowed here, so be careful.
  * @retval -1 An error occured during adding the portal,
  * usually caused by memory exhaustion
@@ -3321,8 +2567,11 @@ int iscsi_portal_group_add_portal(iscsi_portal_group *portal_group, iscsi_portal
 {
 	uint8_t *tmp_buf = iscsi_sprintf_alloc( "%s:%s", portal->host, portal->port );
 
-	if ( tmp_buf == NULL )
+	if ( tmp_buf == NULL ) {
+		logadd( LOG_ERROR, "iscsi_portal_group_add_portal: Out of memory allocating temporarily key buffer for iSCSI portal" );
+
 		return -1;
+	}
 
 	const uint key_len = (uint) (strlen( (char *) tmp_buf ) + 1U);
 	uint8_t *key = iscsi_hashmap_key_create( tmp_buf, key_len );
@@ -3351,6 +2600,50 @@ int iscsi_portal_group_add_portal(iscsi_portal_group *portal_group, iscsi_portal
 }
 
 /**
+ * @brief Removes an iSCSI portal from the iSCSI portal group hash map.
+ *
+ * This function deallocates the hash key used
+ * for storing the portal in the portal group
+ * as well.
+ *
+ * @param[in] portal_group iSCSI portal group to remove portal from. May
+ * NOT be NULL, so take caution.
+ * @param[in] portal iSCSI portal to remove from the portal group.
+ * NULL is NOT allowed here, so be careful.
+ */
+void iscsi_portal_group_del_portal(iscsi_portal_group *portal_group, iscsi_portal *portal)
+{
+	uint8_t *tmp_buf = iscsi_sprintf_alloc( "%s:%s", portal->host, portal->port );
+
+	if ( tmp_buf == NULL ) {
+		logadd( LOG_ERROR, "iscsi_portal_group_del_portal: Out of memory allocating temporarily key buffer for iSCSI portal" );
+
+		return;
+	}
+
+	const uint key_len = (uint) (strlen( (char *) tmp_buf ) + 1U);
+	uint8_t *key = iscsi_hashmap_key_create( tmp_buf, key_len );
+
+	free( tmp_buf );
+
+	if ( key == NULL ) {
+		logadd( LOG_ERROR, "iscsi_portal_group_del_portal: Out of memory allocating key for iSCSI portal" );
+
+		return;
+	}
+
+	int rc = iscsi_hashmap_get( portal_group->portals, key, key_len, (uint8_t **) &portal );
+
+	if ( iscsi_hashmap_contains( portal_group->portals, key, key_len ) ) {
+		portal->group = NULL;
+
+		iscsi_hashmap_remove_free( portal_group->portals, key, key_len, iscsi_hashmap_key_destroy_callback, NULL );
+	}
+
+	iscsi_hashmap_key_destroy( key );
+}
+
+/**
  * @brief Allocates and initializes an iSCSI portal structure.
  *
  * This function makes a copy of the passed host / IP address
@@ -3950,8 +3243,9 @@ void iscsi_scsi_lun_task_run(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task)
  * @brief Handles iSCSI SCSI task completition.
  *
  * This function removes the completed task from
- * the iSCSI SCSI LUN task hash map and calls
- * the transfer finished callback function.
+ * the iSCSI SCSI LUN task doubly linked list
+ * and calls the transfer finished callback
+ * function.
  *
  * @param[in] lun Pointer to iSCSI SCSI LUN to remove the task
  * from.
@@ -3968,7 +3262,7 @@ void iscsi_scsi_lun_task_complete(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_tas
 }
 
 /**
- * @brief Appends iSCSI SCSI task to pending tasks hash map and / or runs it directly.
+ * @brief Appends iSCSI SCSI task to pending tasks doubly linked list and / or runs it directly.
  *
  * This function checks whether there are pending
  * task management pending tasks to be executed
@@ -5545,7 +4839,7 @@ static int iscsi_scsi_emu_check_len(iscsi_scsi_task *scsi_task, const uint len,
  * NAA for. NULL is NOT allowed here, so
  * take caution.
  */
-static void iscsi_scsi_emu_naa_ieee_ext_set(uint64_t *buf, const uint8_t *name)
+static inline void iscsi_scsi_emu_naa_ieee_ext_set(uint64_t *buf, const uint8_t *name)
 {
 	const uint64_t wwn = iscsi_target_node_wwn_get( name );
 
@@ -5614,7 +4908,7 @@ int iscsi_scsi_emu_primary_inquiry_callback(uint8_t *key, const size_t key_size,
 		return 0;
 
 	const uint port_name_len = (uint) (strlen( (char *) port->name ) + 1U);
-	const uint len = (uint) (sizeof(struct iscsi_scsi_vpd_scsi_port_design_dec_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_scsi_target_port_design_dec_inquiry_data_packet) + iscsi_align(port_name_len, ISCSI_ALIGN_SIZE));
+	const uint len = (uint) (sizeof(struct iscsi_scsi_vpd_scsi_port_design_dec_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_scsi_target_port_design_dec_inquiry_data_packet) + ISCSI_ALIGN(port_name_len, ISCSI_ALIGN_SIZE));
 
 	port_report_fill->len -= len;
 
@@ -5745,8 +5039,8 @@ static int iscsi_scsi_emu_primary_inquiry(dnbd3_image_t *image, iscsi_scsi_task
 
 				alloc_len  = (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_page_design_desc_ieee_naa_ext_inquiry_data_packet)); // 64-bit IEEE NAA Extended
 				alloc_len += (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_page_design_desc_t10_vendor_id_inquiry_data_packet)); // T10 Vendor ID
-				alloc_len += (uint) (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + iscsi_align(dev_name_len, ISCSI_ALIGN_SIZE)); // SCSI Device Name
-				alloc_len += (uint) (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + iscsi_align(port_name_len, ISCSI_ALIGN_SIZE)); // SCSI Target Port Name
+				alloc_len += (uint) (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + ISCSI_ALIGN(dev_name_len, ISCSI_ALIGN_SIZE)); // SCSI Device Name
+				alloc_len += (uint) (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + ISCSI_ALIGN(port_name_len, ISCSI_ALIGN_SIZE)); // SCSI Target Port Name
 				alloc_len += (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_page_design_desc_rel_target_port_inquiry_data_packet)); // Relative Target Port
 				alloc_len += (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_page_design_desc_target_port_group_inquiry_data_packet)); // Target Port Group
 				alloc_len += (sizeof(struct iscsi_scsi_vpd_page_design_desc_inquiry_data_packet) + sizeof(struct iscsi_scsi_vpd_page_design_desc_logical_unit_group_inquiry_data_packet)); // Logical Unit Group
@@ -6037,7 +5331,7 @@ static int iscsi_scsi_emu_primary_inquiry(dnbd3_image_t *image, iscsi_scsi_task
 
 		char image_rev[sizeof(std_inquiry_data_pkt->product_rev_level) + 1];
 
-		sprintf( image_rev, "%04X", image->rid );
+		sprintf( image_rev, "%04" PRIX16, image->rid );
 		iscsi_strcpy_pad( (char *) std_inquiry_data_pkt->product_rev_level, image_rev, sizeof(std_inquiry_data_pkt->product_rev_level), ' ' );
 
 		uint add_len = (sizeof(struct iscsi_scsi_std_inquiry_data_packet) - sizeof(struct iscsi_scsi_basic_inquiry_data_packet));
@@ -7196,7 +6490,7 @@ int iscsi_port_transport_id_set(iscsi_port *port, const uint8_t *name, const uin
 	}
 
 	const uint name_len = (uint) (strlen( (char *) tmp_buf ) + 1U);
-	const uint len      = iscsi_align(name_len, ISCSI_ALIGN_SIZE);
+	const uint len      = ISCSI_ALIGN(name_len, ISCSI_ALIGN_SIZE);
 
 	if ( (len < 20U) || ((len + offsetof(struct iscsi_transport_id, name)) >= 65536U) ) {
 		logadd( LOG_ERROR, "iscsi_port_transport_id_set: Out of memory allocating SCSI transport ID for iSCSI port" );
@@ -7593,7 +6887,7 @@ int iscsi_target_node_create_callback(uint8_t *key, const size_t key_size, uint8
 {
 	iscsi_target_node *target = (iscsi_target_node *) user_data;
 	iscsi_portal_group *portal_group = (iscsi_portal_group *) value;
-	uint8_t *port_name = iscsi_sprintf_alloc( "%s,t,0x%4.4x", target->device->name, portal_group->tag );
+	uint8_t *port_name = iscsi_sprintf_alloc( "%s,t,0x%4.4" PRIx64, target->device->name, portal_group->tag );
 
 	if ( port_name == NULL )
 		return -1;
@@ -7763,7 +7057,7 @@ void iscsi_target_node_destroy(iscsi_target_node *target)
  * @return The new position of the written data or a
  * negative error code otherwise.
  */
-int iscsi_target_node_send(iscsi_connection *conn, const uint8_t *dst_iqn, const uint8_t *src_iqn, uint8_t *buf, const uint32_t pos, const uint32_t len)
+int32_t iscsi_target_node_send(iscsi_connection *conn, const uint8_t *dst_iqn, const uint8_t *src_iqn, uint8_t *buf, const uint32_t pos, const uint32_t len)
 {
 	// TODO: Implement function.
 
@@ -7882,12 +7176,23 @@ dnbd3_image_t *iscsi_target_node_image_get(uint8_t *iqn)
 	}
 
 	const uint16_t rev   = (uint16_t) ((len > 0U) ? atoi( (char *) image_rev ) : 0);
-	dnbd3_image_t *image = image_getOrLoad( (char *) tmp, rev );
+	dnbd3_image_t *image = image_getOrLoad( (char *) image_name, rev );
 
 	if ( image == NULL ) {
-		const uint64_t wwn = iscsi_target_node_wwn_get( tmp );
+		image = image_getOrLoad( (char *) tmp, rev );
+
+		if ( image == NULL ) {
+			if ( strncasecmp( (char *) image_name, ISCSI_TARGET_NODE_WWN_NAME_PREFIX, ISCSI_STRLEN(ISCSI_TARGET_NODE_WWN_NAME_PREFIX) ) == 0 ) {
+				uint64_t wwn = strtoull( (char *) (image_name + ISCSI_STRLEN(ISCSI_TARGET_NODE_WWN_NAME_PREFIX)), NULL, 16 );
+
+				image = image_getByWwn( wwn, rev, true );
 
-		image = image_getByWwn( wwn, rev, true );
+				if ( image == NULL ) {
+					wwn   = strtoull( (char *) (tmp + ISCSI_STRLEN(ISCSI_TARGET_NODE_WWN_NAME_PREFIX)), NULL, 16 );
+					image = image_getByWwn( wwn, rev, true );
+				}
+			}
+		}
 	}
 
 	if ( len > 0U )
@@ -7923,7 +7228,7 @@ iscsi_target_node *iscsi_target_node_find(uint8_t *target_name)
 		if ( image == NULL )
 			return NULL;
 
-		target_find.target = iscsi_target_node_create( target_name, NULL, 0, 8U, 1U, 0, 0L, 0, 0 );
+		target_find.target = iscsi_target_node_create( target_name, NULL, 0, 8U, 16U, 0, 0L, 0, 0 );
 
 		if ( target_find.target == NULL ) {
 			logadd( LOG_ERROR, "iscsi_target_node_find: Out of memory while allocating iSCSI target node" );
@@ -8265,21 +7570,8 @@ iscsi_connection *iscsi_connection_create(iscsi_portal *portal, const int sock)
 
 	conn->login_response_pdu = NULL;
 
-	conn->pdu_snack = iscsi_hashmap_create( 0U );
-
-	if ( conn->pdu_snack == NULL ) {
-		logadd( LOG_ERROR, "iscsi_create_connection: Out of memory while allocating iSCSI SNACK PDU hash map" );
-
-		iscsi_hashmap_iterate( conn->text_key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
-		iscsi_hashmap_destroy( conn->text_key_value_pairs );
-
-		iscsi_hashmap_iterate( conn->key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
-		iscsi_hashmap_destroy( conn->key_value_pairs );
-		free( conn );
-
-		return NULL;
-	}
-
+	iscsi_list_create( &conn->pdus_write );
+	iscsi_list_create( &conn->pdus_snack );
 	iscsi_list_create( &conn->r2t_tasks_active );
 	iscsi_list_create( &conn->r2t_tasks_queue );
 
@@ -8313,6 +7605,31 @@ iscsi_connection *iscsi_connection_create(iscsi_portal *portal, const int sock)
 	conn->stat_sn                  = 0UL;
 	conn->exp_stat_sn              = 0UL;
 
+	conn->stat_iscsi_opcodes = iscsi_hashmap_create( 256U );
+
+	if ( conn->stat_iscsi_opcodes == NULL ) {
+		logadd( LOG_ERROR, "iscsi_create: Out of memory while initializing iSCSI global vector iSCSI opcode statistics" );
+
+		iscsi_hashmap_iterate( conn->key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
+		iscsi_hashmap_destroy( conn->key_value_pairs );
+		free( conn );
+
+		return NULL;
+	}
+
+	conn->stat_scsi_opcodes = iscsi_hashmap_create( 256U );
+
+	if ( conn->stat_scsi_opcodes == NULL ) {
+		logadd( LOG_ERROR, "iscsi_create: Out of memory while initializing iSCSI global vector iSCSI SCSI opcode statistics" );
+
+		iscsi_hashmap_destroy( conn->stat_iscsi_opcodes );
+		iscsi_hashmap_iterate( conn->key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
+		iscsi_hashmap_destroy( conn->key_value_pairs );
+		free( conn );
+
+		return NULL;
+	}
+
 	return conn;
 }
 
@@ -8341,6 +7658,45 @@ int iscsi_connection_destroy_callback(uint8_t *key, const size_t key_size, uint8
 }
 
 /**
+ * @brief Deallocates all pending iSCSI tasks and PDUs associated with an iSCSI connection.
+ *
+ * This function only removes tasks which are
+ * not enqueued.
+ *
+ * @param[in] conn Pointer to iSCSI connection of which to
+ * deallocate all the tasks and PDUs. May NOT
+ * be NULL, so be careful.
+ */
+static int iscsi_connection_tasks_destroy(iscsi_connection *conn)
+{
+	iscsi_pdu *pdu;
+	iscsi_pdu *tmp_pdu;
+
+	iscsi_list_foreach_safe_node ( &conn->pdus_snack, pdu, tmp_pdu ) {
+		iscsi_list_remove( &pdu->node );
+		iscsi_connection_pdu_destroy( pdu );
+	}
+
+	iscsi_task *task;
+	iscsi_task *tmp_task;
+
+	iscsi_list_foreach_safe_node ( &conn->scsi_data_in_queued_tasks, task, tmp_task ) {
+		if ( (task->flags & ISCSI_TASK_FLAGS_QUEUED) != 0 )
+			continue;
+
+		iscsi_list_remove( &task->node );
+		iscsi_task_destroy( task );
+	}
+
+	iscsi_list_foreach_safe_node ( &conn->pdus_write, pdu, tmp_pdu ) {
+		iscsi_list_remove( &pdu->node );
+		iscsi_connection_pdu_destroy( pdu );
+	}
+
+	return (conn->task_cnt != 0) ? -1 : 0;
+}
+
+/**
  * @brief Deallocates all resources acquired by iscsi_connection_create.
  *
  * Deallocates a data structure of an iSCSI connection
@@ -8356,6 +7712,14 @@ int iscsi_connection_destroy_callback(uint8_t *key, const size_t key_size, uint8
 void iscsi_connection_destroy(iscsi_connection *conn)
 {
 	if ( conn != NULL ) {
+		iscsi_hashmap_iterate( conn->stat_scsi_opcodes, iscsi_hashmap_destroy_value_callback, NULL );
+		iscsi_hashmap_destroy( conn->stat_scsi_opcodes );
+		conn->stat_scsi_opcodes = NULL;
+
+		iscsi_hashmap_iterate( conn->stat_iscsi_opcodes, iscsi_hashmap_destroy_value_callback, NULL );
+		iscsi_hashmap_destroy( conn->stat_iscsi_opcodes );
+		conn->stat_iscsi_opcodes = NULL;
+
 		iscsi_task *task;
 		iscsi_task *tmp;
 
@@ -8369,11 +7733,17 @@ void iscsi_connection_destroy(iscsi_connection *conn)
 			iscsi_task_destroy( task );
 		}
 
-		if ( conn->pdu_snack != NULL ) {
-			iscsi_hashmap_iterate( conn->pdu_snack, iscsi_hashmap_key_destroy_callback, NULL );
-			iscsi_hashmap_destroy( conn->pdu_snack );
+		iscsi_pdu *pdu;
+		iscsi_pdu *tmp_pdu;
 
-			conn->pdu_snack = NULL;
+		iscsi_list_foreach_safe_node ( &conn->pdus_snack, pdu, tmp_pdu ) {
+			iscsi_list_remove( &pdu->node );
+			iscsi_connection_pdu_destroy( pdu );
+		}
+
+		iscsi_list_foreach_safe_node ( &conn->pdus_write, pdu, tmp_pdu ) {
+			iscsi_list_remove( &pdu->node );
+			iscsi_connection_pdu_destroy( pdu );
 		}
 
 		iscsi_list_foreach_safe_node ( &conn->scsi_data_in_queued_tasks, task, tmp ) {
@@ -8616,14 +7986,14 @@ static int32_t iscsi_append_special_key_value_pair_packet(iscsi_connection *conn
 		return pos;
 
 	if ( (key_value_pair->flags & ISCSI_TEXT_KEY_VALUE_PAIR_FLAGS_OVERRIDE_DEFAULT) != 0 ) {
-		if ( (int32_t) (len - pos) < 1L )
+		if ( pos >= len )
 			return -1L;
 
-		pos += (uint32_t) (snprintf( (char *) (buf + pos), (len - pos), "%s=%ld", key, ISCSI_DEFAULT_MAX_RECV_DS_LEN ) + 1);
+		pos += (uint32_t) (snprintf( (char *) (buf + pos), (len - pos), "%s=%" PRId32, key, (uint32_t) ISCSI_DEFAULT_MAX_RECV_DS_LEN ) + 1);
 	}
 
 	if ( (key_value_pair->flags & ISCSI_TEXT_KEY_VALUE_PAIR_FLAGS_USE_OTHER_MAX_VALUE) != 0 ) {
-		if ( (int32_t) (len - pos) < 1L )
+		if ( pos >= len )
 			return -1L;
 
 		uint8_t *first_burst_len_val = NULL;
@@ -8638,15 +8008,14 @@ static int32_t iscsi_append_special_key_value_pair_packet(iscsi_connection *conn
 			first_burst_len = max_burst_len;
 
 			if ( first_burst_len_val != NULL ) {
-				sprintf( (char *) first_burst_len_val, "%d", first_burst_len );
+				sprintf( (char *) first_burst_len_val, "%" PRId32, first_burst_len );
 			}
 		}
 
-		pos += (uint32_t) (snprintf( (char *) (buf + pos), (len - pos), "%s=%d", key, first_burst_len ) + 1);
+		pos += (uint32_t) (snprintf( (char *) (buf + pos), (len - pos), "%s=%" PRId32, key, first_burst_len ) + 1);
 	}
 
 	return pos;
-
 }
 
 /**
@@ -8675,7 +8044,7 @@ static int32_t iscsi_append_special_key_value_pair_packet(iscsi_connection *conn
 static int32_t iscsi_append_key_value_pair_packet(const iscsi_key_value_pair *key_value_pair, const uint8_t *key, const uint8_t *value, uint8_t *buf, uint32_t pos, const uint32_t len)
 {
 	if ( (key_value_pair == NULL) || ((key_value_pair->type != ISCSI_TEXT_KEY_VALUE_PAIR_TYPE_DECLARATIVE) && (key_value_pair->type != ISCSI_TEXT_KEY_VALUE_PAIR_TYPE_NUM_DECLARATIVE)) ) {
-		if ( (int32_t) (len - pos) < 1L )
+		if ( pos >= len )
 			return -1L;
 
 		pos += (uint32_t) (snprintf( (char *) (buf + pos), (len - pos), "%s=%s", key, value ) + 1);
@@ -8778,7 +8147,7 @@ static uint8_t *iscsi_negotiate_key_value_pair_num(const iscsi_key_value_pair *k
 		}
 	}
 
-	sprintf( (char *) old_value, "%d", old_int_val );
+	sprintf( (char *) old_value, "%" PRId32, old_int_val );
 
 	return old_value;
 }
@@ -8984,7 +8353,7 @@ int iscsi_negotiate_key_value_pair_callback(uint8_t *key, const size_t key_size,
 			max_burst_len = (rc < 0) ? ISCSI_SESSION_DEFAULT_MAX_BURST_LEN : (uint32_t) atol( (char *) max_burst_len_val );
 
 			if ( (first_burst_len < ISCSI_MAX_DS_SIZE) && (first_burst_len > max_burst_len) )
-				sprintf( (char *) value, "%d", first_burst_len );
+				sprintf( (char *) value, "%" PRId32, first_burst_len );
 		}
 
 		if ( (key_value_pair->flags & ISCSI_TEXT_KEY_VALUE_PAIR_FLAGS_TARGET_DECLARATIVE) != 0 )
@@ -9163,7 +8532,7 @@ int iscsi_connection_copy_key_value_pairs(iscsi_connection *conn)
  * @return 0 if authentication methods were handled successfully,
  * a negative error code otherwise.
  */
-static int iscsi_connection_auth_key_value_pairs(iscsi_connection *conn, iscsi_hashmap *key_value_pairs, const uint8_t *auth_method, uint8_t *buf, const uint pos, const uint len)
+static int32_t iscsi_connection_auth_key_value_pairs(iscsi_connection *conn, iscsi_hashmap *key_value_pairs, const uint8_t *auth_method, uint8_t *buf, const uint pos, const uint len)
 {
 	// TODO: Implement CHAP and other authentication methods.
 
@@ -9250,22 +8619,30 @@ static int iscsi_connection_update_key_value_pairs(iscsi_connection *conn)
  * to be sent via TCP/IP.
  *
  * @param[in] conn Pointer to ISCSI connection to send the TCP/IP
- * packet with.
+ * packet with. May NOT be NULL, so be
+ * careful.
  * @param[in] login_response_pdu Pointer to login response PDU to
- * be sent via TCP/IP.
+ * be sent via TCP/IP. NULL is NOT
+ * allowed here, take caution.
  * @param[in] key_value_pairs Pointer to hash map of key and value pairs
  * to be used for login response storage.
  * @param[in] callback Pointer to post processing callback function
  * after sending the TCP/IP packet.
+ * @return 0 if the login response has been sent
+ * successfully, a negative error code otherwise.
  */
-static void iscsi_connection_pdu_login_response(iscsi_connection *conn, iscsi_pdu *login_response_pdu, iscsi_hashmap *key_value_pairs, iscsi_connection_xfer_complete_callback callback)
+static int iscsi_connection_pdu_login_response(iscsi_connection *conn, iscsi_pdu *login_response_pdu, iscsi_hashmap *key_value_pairs, iscsi_connection_xfer_complete_callback callback)
 {
-	iscsi_login_response_packet *login_response_pkt = (iscsi_login_response_packet *) login_response_pdu->bhs_pkt;
+	const uint32_t ds_len = login_response_pdu->ds_len;
+
+	login_response_pdu->ds_len = login_response_pdu->len;
+
+	iscsi_login_response_packet *login_response_pkt = (iscsi_login_response_packet *) iscsi_connection_pdu_append( login_response_pdu, login_response_pdu->ahs_len, 0, ds_len, 0 );
 
 	login_response_pkt->version_max    = ISCSI_VERSION_MAX;
 	login_response_pkt->version_active = ISCSI_VERSION_MAX;
 
-	iscsi_put_be24( (uint8_t *) &login_response_pkt->ds_len, login_response_pdu->ds_len );
+	iscsi_put_be32( (uint8_t *) &login_response_pkt->total_ahs_len, ds_len ); // TotalAHSLength is always 0 and DataSegmentLength is 24-bit, so write in one step.
 	iscsi_put_be32( (uint8_t *) &login_response_pkt->stat_sn, conn->stat_sn++ );
 
 	if ( conn->session != NULL ) {
@@ -9279,10 +8656,14 @@ static void iscsi_connection_pdu_login_response(iscsi_connection *conn, iscsi_pd
 	if ( login_response_pkt->status_class != ISCSI_LOGIN_RESPONSE_STATUS_CLASS_SUCCESS )
 		login_response_pkt->flags &= (int8_t) ~(ISCSI_LOGIN_RESPONSE_FLAGS_TRANSIT | ISCSI_LOGIN_RESPONSE_FLAGS_CURRENT_STAGE_MASK | ISCSI_LOGIN_RESPONSE_FLAGS_NEXT_STAGE_MASK );
 
-	iscsi_hashmap_iterate( key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
-	iscsi_hashmap_destroy( key_value_pairs );
-
 	iscsi_connection_pdu_write( conn, login_response_pdu, callback, (uint8_t *) conn );
+
+	if ( key_value_pairs != NULL ) {
+		iscsi_hashmap_iterate( key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
+		iscsi_hashmap_destroy( key_value_pairs );
+	}
+
+	return ISCSI_CONNECT_PDU_READ_OK;
 }
 
 /**
@@ -9341,27 +8722,15 @@ static void iscsi_connection_pdu_login_ok_complete(uint8_t *user_data)
  * @return 0 if initialization was successful, a negative error
  * code otherwise.
  */
-static int iscsi_login_response_init(iscsi_pdu *login_response_pdu, const iscsi_pdu *pdu)
+static int iscsi_connection_pdu_login_response_init(iscsi_pdu *login_response_pdu, const iscsi_pdu *pdu)
 {
 	iscsi_login_req_packet *login_req_pkt = (iscsi_login_req_packet *) pdu->bhs_pkt;
-	iscsi_login_response_packet *bhs_pkt  = (iscsi_login_response_packet *) login_response_pdu->bhs_pkt;
-
-	bhs_pkt->opcode = ISCSI_OPCODE_SERVER_LOGIN_RES;
-
-	iscsi_login_response_packet *login_response_pkt = (iscsi_login_response_packet *) iscsi_append_ds_packet( (iscsi_bhs_packet *) bhs_pkt, pdu->header_digest_size, ISCSI_DEFAULT_RECV_DS_LEN, pdu->data_digest_size );
-
-	if ( login_response_pkt == NULL ) {
-		bhs_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_SERVER_ERR;
-		bhs_pkt->status_detail = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_SERVER_ERR_OUT_OF_RESOURCES;
-
-		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-	}
+	iscsi_login_response_packet *login_response_pkt = (iscsi_login_response_packet *) login_response_pdu->bhs_pkt;
 
-	login_response_pdu->bhs_pkt     = (iscsi_bhs_packet *) login_response_pkt;
-	login_response_pdu->ds_cmd_data = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) login_response_pkt) + sizeof(struct iscsi_bhs_packet) + pdu->header_digest_size);
-	login_response_pdu->len         = ISCSI_DEFAULT_RECV_DS_LEN;
+	login_response_pdu->ds_len = 0UL;
 
-	login_response_pkt->flags |= (int8_t) (login_req_pkt->flags & (ISCSI_LOGIN_REQ_FLAGS_TRANSIT | ISCSI_LOGIN_REQ_FLAGS_CONTINUE | ISCSI_LOGIN_REQ_FLAGS_CURRENT_STAGE_MASK));
+	login_response_pkt->opcode = ISCSI_OPCODE_SERVER_LOGIN_RES;
+	login_response_pkt->flags  = (int8_t) (login_req_pkt->flags & (ISCSI_LOGIN_REQ_FLAGS_TRANSIT | ISCSI_LOGIN_REQ_FLAGS_CONTINUE | ISCSI_LOGIN_REQ_FLAGS_CURRENT_STAGE_MASK));
 
 	if ( (login_response_pkt->flags & ISCSI_LOGIN_RESPONSE_FLAGS_TRANSIT) != 0 )
 		login_response_pkt->flags |= (login_req_pkt->flags & ISCSI_LOGIN_REQ_FLAGS_NEXT_STAGE_MASK);
@@ -9372,10 +8741,16 @@ static int iscsi_login_response_init(iscsi_pdu *login_response_pdu, const iscsi_
 	login_response_pkt->isid.d        = login_req_pkt->isid.d; // Copying over doesn't change endianess.
 	login_response_pkt->tsih          = login_req_pkt->tsih; // Copying over doesn't change endianess.'
 	login_response_pkt->init_task_tag = login_req_pkt->init_task_tag; // Copying over doesn't change endianess.
+	login_response_pkt->reserved      = 0UL;
 	login_response_pdu->cmd_sn        = iscsi_get_be32(login_req_pkt->cmd_sn);
 
-	if ( login_response_pkt->tsih != 0 )
+	if ( login_response_pkt->tsih != 0U )
 		login_response_pkt->stat_sn = login_req_pkt->exp_stat_sn; // Copying over doesn't change endianess.'
+	else
+		login_response_pkt->stat_sn = 0UL;
+
+	login_response_pkt->reserved2 = 0U;
+	login_response_pkt->reserved3 = 0ULL;
 
 	if ( ((login_response_pkt->flags & ISCSI_LOGIN_RESPONSE_FLAGS_TRANSIT) != 0) && ((login_response_pkt->flags & ISCSI_LOGIN_RESPONSE_FLAGS_CONTINUE) != 0) ) {
 		login_response_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_CLIENT_ERR;
@@ -9393,11 +8768,11 @@ static int iscsi_login_response_init(iscsi_pdu *login_response_pdu, const iscsi_
 		login_response_pkt->status_detail = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_CLIENT_ERR_MISC;
 
 		return ISCSI_CONNECT_PDU_READ_ERR_LOGIN_RESPONSE;
-	} else {
-		login_response_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_SUCCESS;
-		login_response_pkt->status_detail = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_SUCCESS;
 	}
 
+	login_response_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_SUCCESS;
+	login_response_pkt->status_detail = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_SUCCESS;
+
 	return ISCSI_CONNECT_PDU_READ_OK;
 }
 
@@ -9630,7 +9005,7 @@ static int iscsi_connection_login_check_target(iscsi_connection *conn, iscsi_pdu
 			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 		}
 
-		login_response_pdu->ds_len = ds_len;
+		login_response_pdu->ds_len        = ds_len;
 		login_response_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_REDIRECT;
 		login_response_pkt->status_detail = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_REDIRECT_TEMP;
 
@@ -9755,16 +9130,29 @@ static int iscsi_connection_login_check_session(iscsi_connection *conn, iscsi_pd
  * @param[in] pdu Pointer to iSCSI login request PDU, may NOT
  * be NULL, so be careful.
  */
-void iscsi_login_response_reject_init(iscsi_pdu *login_response_pdu, const iscsi_pdu *pdu)
+void iscsi_connection_login_response_reject(iscsi_pdu *login_response_pdu, const iscsi_pdu *pdu)
 {
 	iscsi_login_response_packet *login_response_pkt = (iscsi_login_response_packet *) login_response_pdu->bhs_pkt;
 
-	login_response_pkt->opcode         = ISCSI_OPCODE_SERVER_LOGIN_RES;
-	login_response_pkt->version_max    = ISCSI_VERSION_MAX;
-	login_response_pkt->version_active = ISCSI_VERSION_MAX;
-	login_response_pkt->init_task_tag  = ((iscsi_login_req_packet *) pdu->bhs_pkt)->init_task_tag;
-	login_response_pkt->status_class   = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_CLIENT_ERR;
-	login_response_pkt->status_detail  = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_CLIENT_ERR_INVALID_LOGIN_REQ_TYPE;
+	login_response_pkt->opcode                       = ISCSI_OPCODE_SERVER_LOGIN_RES;
+	login_response_pkt->flags                        = 0;
+	login_response_pkt->version_max                  = ISCSI_VERSION_MAX;
+	login_response_pkt->version_active               = ISCSI_VERSION_MAX;
+	*(uint32_t *) &login_response_pkt->total_ahs_len = 0UL; // TotalAHSLength and DataSegmentLength are always 0, so write in one step.
+	login_response_pkt->isid.a                       = 0U;
+	login_response_pkt->isid.b                       = 0U;
+	login_response_pkt->isid.c                       = 0U;
+	login_response_pkt->isid.d                       = 0U;
+	login_response_pkt->tsih                         = 0U;
+	login_response_pkt->init_task_tag                = ((iscsi_login_req_packet *) pdu->bhs_pkt)->init_task_tag;
+	login_response_pkt->reserved                     = 0UL;
+	login_response_pkt->stat_sn                      = 0UL;
+	login_response_pkt->exp_cmd_sn                   = 0UL;
+	login_response_pkt->max_cmd_sn                   = 0UL;
+	login_response_pkt->status_class                 = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_CLIENT_ERR;
+	login_response_pkt->status_detail                = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_CLIENT_ERR_INVALID_LOGIN_REQ_TYPE;
+	login_response_pkt->reserved2                    = 0U;
+	login_response_pkt->reserved3                    = 0ULL;
 }
 
 /**
@@ -9775,12 +9163,24 @@ void iscsi_login_response_reject_init(iscsi_pdu *login_response_pdu, const iscsi
  * filling the data until everything has been read.
  *
  * @param[in] conn Pointer to connection to link the PDU with.
- * If this is NULL the connection has to be linked later.
+ * If this is NULL the connection has to be
+ * linked later.
+ * @param[in] ahs_len Length of AHS packet data to be appended.
+ * @param[in] header_digest_size Length of header digest. Currently,
+ * only 0, in which case the header digest will
+ * be removed, or 4 for CRC32C are allowed.
+ * @param[in] ds_len Length of DataSegment packet data to be appended.
+ * May not exceed 16MiB - 1 (16777215 bytes).
+ * @param[in] data_digest_size Length of optional data digest (0 or
+ * 4 for now) to add.
  * @return Pointer to allocated and zero filled PDU or NULL
  * in case of an error (usually memory exhaustion).
  */
-iscsi_pdu *iscsi_connection_pdu_create(iscsi_connection *conn)
+iscsi_pdu *iscsi_connection_pdu_create(iscsi_connection *conn, const uint ahs_len, const int header_digest_size, const uint32_t ds_len, const int data_digest_size)
 {
+	if ( (ahs_len > ISCSI_MAX_AHS_SIZE) || ((header_digest_size != 0) && (header_digest_size != ISCSI_DIGEST_SIZE)) || ((data_digest_size != 0) && data_digest_size != ISCSI_DIGEST_SIZE) || (ds_len > ISCSI_MAX_DS_SIZE) )
+		return NULL;
+
 	iscsi_pdu *pdu = (iscsi_pdu *) malloc( sizeof(struct iscsi_pdu) );
 
 	if ( pdu == NULL ) {
@@ -9789,18 +9189,25 @@ iscsi_pdu *iscsi_connection_pdu_create(iscsi_connection *conn)
 		return NULL;
 	}
 
-	pdu->bhs_pkt = iscsi_create_packet();
+	const uint32_t pkt_ds_len = ISCSI_ALIGN(ds_len, ISCSI_ALIGN_SIZE);
+	const uint32_t len        = (uint32_t) (sizeof(struct iscsi_bhs_packet) + (uint32_t) ahs_len + header_digest_size + pkt_ds_len + ((pkt_ds_len != 0UL) ? (uint32_t) data_digest_size : 0UL));
+	iscsi_bhs_packet *bhs_pkt = malloc( len );
+
+	if ( bhs_pkt == NULL ) {
+		logadd( LOG_ERROR, "iscsi_pdu_create: Out of memory while allocating iSCSI PDU packet data" );
 
-	if ( pdu->bhs_pkt == NULL ) {
 		free( pdu );
 
 		return NULL;
 	}
 
-	pdu->ahs_pkt                 = NULL;
-	pdu->header_digest           = NULL;
-	pdu->ds_cmd_data             = NULL;
-	pdu->data_digest             = NULL;
+	pdu->node.succ               = NULL;
+	pdu->node.pred               = NULL;
+	pdu->bhs_pkt                 = bhs_pkt;
+	pdu->ahs_pkt                 = (ahs_len != 0U) ? (iscsi_ahs_packet *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) ) : NULL;
+	pdu->header_digest           = (header_digest_size != 0) ? (iscsi_header_digest *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len) : NULL;
+	pdu->ds_cmd_data             = (pkt_ds_len != 0UL) ? (iscsi_scsi_ds_cmd_data *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len + header_digest_size) : NULL;
+	pdu->data_digest             = ((pkt_ds_len != 0uL) && (data_digest_size != 0)) ? (iscsi_data_digest *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len + header_digest_size + ISCSI_ALIGN(pkt_ds_len, ISCSI_ALIGN_SIZE)) : NULL;
 	pdu->task                    = NULL;
 	pdu->conn                    = conn;
 	pdu->xfer_complete_callback  = NULL;
@@ -9809,17 +9216,20 @@ iscsi_pdu *iscsi_connection_pdu_create(iscsi_connection *conn)
 	pdu->ref                     = 1UL;
 	pdu->bhs_pos                 = 0U;
 	pdu->ahs_pos                 = 0U;
-	pdu->ahs_len                 = 0U;
+	pdu->ahs_len                 = ahs_len;
 	pdu->header_digest_pos       = 0U;
-	pdu->header_digest_size      = 0;
-	pdu->ds_len                  = 0UL;
+	pdu->header_digest_size      = header_digest_size;
+	pdu->ds_len                  = pkt_ds_len;
 	pdu->pos                     = 0UL;
-	pdu->len                     = 0UL;
+	pdu->len                     = pkt_ds_len;
 	pdu->data_digest_pos         = 0U;
-	pdu->data_digest_size        = 0;
+	pdu->data_digest_size        = data_digest_size;
 	pdu->task_ref_cnt            = 0U;
 	pdu->cmd_sn                  = 0UL;
 
+	if ( pkt_ds_len != 0UL )
+		memset( (((uint8_t *) pdu->ds_cmd_data) + ds_len), 0, (pkt_ds_len - ds_len) );
+
 	return pdu;
 }
 
@@ -9839,7 +9249,11 @@ void iscsi_connection_pdu_destroy(iscsi_pdu *pdu)
 		if ( pdu->bhs_pkt != NULL ) {
 			free( pdu->bhs_pkt );
 
-			pdu->bhs_pkt = NULL;
+			pdu->bhs_pkt       = NULL;
+			pdu->ahs_pkt       = NULL;
+			pdu->header_digest = NULL;
+			pdu->ds_cmd_data   = NULL;
+			pdu->data_digest   = NULL;
 		}
 
 		free( pdu );
@@ -9847,6 +9261,69 @@ void iscsi_connection_pdu_destroy(iscsi_pdu *pdu)
 }
 
 /**
+ * @brief Appends packet data to an iSCSI PDU structure used by connections.
+ *
+ * This function adjusts the pointers if
+ * the packet data size needs to be
+ * extended.
+ *
+ * @param[in] pdu Pointer to iSCSI PDU where to append
+ * the packet data to. May NOT be NULL, so
+ * be careful.
+ * @param[in] ahs_len Length of AHS packet data to be appended.
+ * @param[in] header_digest_size Length of header digest. Currently,
+ * only 0, in which case the header digest will
+ * be removed, or 4 for CRC32C are allowed.
+ * @param[in] ds_len Length of DataSegment packet data to be appended.
+ * May not exceed 16MiB - 1 (16777215 bytes).
+ * @param[in] data_digest_size Length of optional data digest (0 or
+ * 4 for now) to add.
+ * @return Pointer to allocated and zero filled PDU or NULL
+ * in case of an error (usually memory exhaustion).
+ */
+iscsi_bhs_packet *iscsi_connection_pdu_append(iscsi_pdu *pdu, const uint ahs_len, const int header_digest_size, const uint32_t ds_len, const int data_digest_size)
+{
+	if ( (ahs_len > ISCSI_MAX_AHS_SIZE) || ((header_digest_size != 0) && (header_digest_size != ISCSI_DIGEST_SIZE)) || ((data_digest_size != 0) && data_digest_size != ISCSI_DIGEST_SIZE) || (ds_len > ISCSI_MAX_DS_SIZE) )
+		return NULL;
+
+	if ( (ahs_len != pdu->ahs_len) || (header_digest_size != pdu->header_digest_size) || (ds_len != pdu->ds_len) || (data_digest_size != pdu->data_digest_size) ) {
+		iscsi_bhs_packet *bhs_pkt;
+		const uint32_t pkt_ds_len = ISCSI_ALIGN(ds_len, ISCSI_ALIGN_SIZE);
+		const uint32_t old_len    = (uint32_t) (sizeof(struct iscsi_bhs_packet) + (uint32_t) pdu->ahs_len + pdu->header_digest_size + pdu->ds_len + ((pdu->ds_len != 0UL) ? (uint32_t) pdu->data_digest_size : 0UL));
+		const uint32_t new_len    = (uint32_t) (sizeof(struct iscsi_bhs_packet) + (uint32_t) ahs_len + header_digest_size + pkt_ds_len + ((pkt_ds_len != 0UL) ? (uint32_t) data_digest_size : 0UL));
+
+		if ( new_len > old_len ) {
+			bhs_pkt = realloc( pdu->bhs_pkt, new_len );
+
+			if ( bhs_pkt == NULL ) {
+				logadd( LOG_ERROR, "iscsi_connection_pdu_append: Out of memory while reallocating iSCSI PDU packet data" );
+
+				return NULL;
+			}
+
+			pdu->bhs_pkt = bhs_pkt;
+		} else {
+			bhs_pkt = pdu->bhs_pkt;
+		}
+
+		pdu->ahs_pkt            = (ahs_len != 0U) ? (iscsi_ahs_packet *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) ) : NULL;
+		pdu->header_digest      = (header_digest_size != 0) ? (iscsi_header_digest *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len) : NULL;
+		pdu->ds_cmd_data        = (pkt_ds_len != 0UL) ? (iscsi_scsi_ds_cmd_data *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len + header_digest_size) : NULL;
+		pdu->data_digest        = ((pkt_ds_len != 0UL) && (data_digest_size != 0)) ? (iscsi_data_digest *) (((uint8_t *) bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len + header_digest_size + pkt_ds_len) : NULL;
+		pdu->ahs_len            = ahs_len;
+		pdu->header_digest_size = header_digest_size;
+		pdu->ds_len             = pkt_ds_len;
+		pdu->len                = pkt_ds_len;
+		pdu->data_digest_size   = data_digest_size;
+
+		if ( pkt_ds_len != 0UL )
+			memset( (((uint8_t *) pdu->ds_cmd_data) + ds_len), 0, (pkt_ds_len - ds_len) );
+	}
+
+	return pdu->bhs_pkt;
+}
+
+/**
  * @brief Frees an iSCSI PDU structure used by using connection callback function.
  *
  * This function frees an iSCSI PDU structure.
@@ -9869,7 +9346,289 @@ void iscsi_connection_pdu_free(iscsi_connection *conn, iscsi_pdu *pdu)
 
 	iscsi_connection_pdu_destroy( pdu );
 
-	callback( user_data );
+	if ( callback != NULL )
+		callback( user_data );
+}
+
+/**
+ * @brief Retrieves the pointer to an specific AHS packet from an iSCSI PDU by index.
+ *
+ * Gets the pointer of an AHS packet by specified index.
+ *
+ * @param[in] pdu Pointer to iSCSI PDU of which the
+ * AHS packet should be retrieved. May
+ * NOT be NULL, so be careful.
+ * @param[in] index Zero-based index number of AHS packet to
+ * be received.
+ * @return The pointer to the AHS packet at specified index on
+ * success or NULL in case of an error or if the specific index
+ * is out of range.
+ */
+iscsi_ahs_packet *iscsi_connection_pdu_ahs_packet_get(const iscsi_pdu *pdu, const int index)
+{
+	iscsi_ahs_packet *ahs_pkt = pdu->ahs_pkt; // First AHS packet
+
+	if ( ahs_pkt == NULL )
+		return NULL;
+
+	int count    = index;
+	uint ahs_len = pdu->ahs_len;
+
+	while ( (int) ahs_len > 0 ) {
+		if ( count-- < 0 )
+			return ahs_pkt;
+
+		uint len = iscsi_get_be16(ahs_pkt->len) + offsetof(struct iscsi_ahs_packet, data); // Total length of current AHS packet
+
+		len      = ISCSI_ALIGN(len, ISCSI_ALIGN_SIZE);
+		ahs_len -= len;
+		ahs_pkt  = (iscsi_ahs_packet *) (((uint8_t *) ahs_pkt) + (len - offsetof(struct iscsi_ahs_packet, data))); // Advance pointer to next AHS packet
+	}
+
+	logadd( LOG_ERROR, "iscsi_get_ahs_packet: Specified index for AHS packet does not exist" );
+
+	return NULL;
+}
+
+/**
+ * @brief Counts number of AHS packets of an iSCSI PDU.
+ *
+ * Gets the total number of AHS packets.
+ *
+ * @param[in] pdu Pointer to iscsi PDU of which the
+ * number of AHS packets should be counted.
+ * May NOT be NULL, so be careful.
+ * @return The number of AHS packets or 0 if no AHS
+ * packet data is available.
+ */
+int iscsi_connection_pdu_ahs_packet_count(const iscsi_pdu *pdu)
+{
+	const iscsi_ahs_packet *ahs_pkt = pdu->ahs_pkt; // First AHS packet
+
+	if ( ahs_pkt == NULL )
+		return 0;
+
+	int count    = 0;
+	uint ahs_len = pdu->ahs_len;
+
+	while ( (int) ahs_len > 0 ) {
+		uint len = iscsi_get_be16(ahs_pkt->len) + offsetof(struct iscsi_ahs_packet, data); // Total length of current AHS packet
+
+		len      = ISCSI_ALIGN(len, ISCSI_ALIGN_SIZE);
+		ahs_len -= len;
+		ahs_pkt  = (iscsi_ahs_packet *) (((uint8_t *) ahs_pkt) + (len - offsetof(struct iscsi_ahs_packet, data))); // Advance pointer to next AHS packet
+		count++;
+	}
+
+	return count;
+}
+
+/// CRC32C lookup table. Created with a polynomial reflect value of 0x82F63B78.
+static const uint32_t crc32c_lut[] = {
+	0x00000000, 0xF26B8303, 0xE13B70F7, 0x1350F3F4, 0xC79A971F, 0x35F1141C, 0x26A1E7E8, 0xD4CA64EB,
+	0x8AD958CF, 0x78B2DBCC, 0x6BE22838, 0x9989AB3B, 0x4D43CFD0, 0xBF284CD3, 0xAC78BF27, 0x5E133C24,
+	0x105EC76F, 0xE235446C, 0xF165B798, 0x030E349B, 0xD7C45070, 0x25AFD373, 0x36FF2087, 0xC494A384,
+	0x9A879FA0, 0x68EC1CA3, 0x7BBCEF57, 0x89D76C54, 0x5D1D08BF, 0xAF768BBC, 0xBC267848, 0x4E4DFB4B,
+	0x20BD8EDE, 0xD2D60DDD, 0xC186FE29, 0x33ED7D2A, 0xE72719C1, 0x154C9AC2, 0x061C6936, 0xF477EA35,
+	0xAA64D611, 0x580F5512, 0x4B5FA6E6, 0xB93425E5, 0x6DFE410E, 0x9F95C20D, 0x8CC531F9, 0x7EAEB2FA,
+	0x30E349B1, 0xC288CAB2, 0xD1D83946, 0x23B3BA45, 0xF779DEAE, 0x05125DAD, 0x1642AE59, 0xE4292D5A,
+	0xBA3A117E, 0x4851927D, 0x5B016189, 0xA96AE28A, 0x7DA08661, 0x8FCB0562, 0x9C9BF696, 0x6EF07595,
+	0x417B1DBC, 0xB3109EBF, 0xA0406D4B, 0x522BEE48, 0x86E18AA3, 0x748A09A0, 0x67DAFA54, 0x95B17957,
+	0xCBA24573, 0x39C9C670, 0x2A993584, 0xD8F2B687, 0x0C38D26C, 0xFE53516F, 0xED03A29B, 0x1F682198,
+	0x5125DAD3, 0xA34E59D0, 0xB01EAA24, 0x42752927, 0x96BF4DCC, 0x64D4CECF, 0x77843D3B, 0x85EFBE38,
+	0xDBFC821C, 0x2997011F, 0x3AC7F2EB, 0xC8AC71E8, 0x1C661503, 0xEE0D9600, 0xFD5D65F4, 0x0F36E6F7,
+	0x61C69362, 0x93AD1061, 0x80FDE395, 0x72966096, 0xA65C047D, 0x5437877E, 0x4767748A, 0xB50CF789,
+	0xEB1FCBAD, 0x197448AE, 0x0A24BB5A, 0xF84F3859, 0x2C855CB2, 0xDEEEDFB1, 0xCDBE2C45, 0x3FD5AF46,
+	0x7198540D, 0x83F3D70E, 0x90A324FA, 0x62C8A7F9, 0xB602C312, 0x44694011, 0x5739B3E5, 0xA55230E6,
+	0xFB410CC2, 0x092A8FC1, 0x1A7A7C35, 0xE811FF36, 0x3CDB9BDD, 0xCEB018DE, 0xDDE0EB2A, 0x2F8B6829,
+	0x82F63B78, 0x709DB87B, 0x63CD4B8F, 0x91A6C88C, 0x456CAC67, 0xB7072F64, 0xA457DC90, 0x563C5F93,
+	0x082F63B7, 0xFA44E0B4, 0xE9141340, 0x1B7F9043, 0xCFB5F4A8, 0x3DDE77AB, 0x2E8E845F, 0xDCE5075C,
+	0x92A8FC17, 0x60C37F14, 0x73938CE0, 0x81F80FE3, 0x55326B08, 0xA759E80B, 0xB4091BFF, 0x466298FC,
+	0x1871A4D8, 0xEA1A27DB, 0xF94AD42F, 0x0B21572C, 0xDFEB33C7, 0x2D80B0C4, 0x3ED04330, 0xCCBBC033,
+	0xA24BB5A6, 0x502036A5, 0x4370C551, 0xB11B4652, 0x65D122B9, 0x97BAA1BA, 0x84EA524E, 0x7681D14D,
+	0x2892ED69, 0xDAF96E6A, 0xC9A99D9E, 0x3BC21E9D, 0xEF087A76, 0x1D63F975, 0x0E330A81, 0xFC588982,
+	0xB21572C9, 0x407EF1CA, 0x532E023E, 0xA145813D, 0x758FE5D6, 0x87E466D5, 0x94B49521, 0x66DF1622,
+	0x38CC2A06, 0xCAA7A905, 0xD9F75AF1, 0x2B9CD9F2, 0xFF56BD19, 0x0D3D3E1A, 0x1E6DCDEE, 0xEC064EED,
+	0xC38D26C4, 0x31E6A5C7, 0x22B65633, 0xD0DDD530, 0x0417B1DB, 0xF67C32D8, 0xE52CC12C, 0x1747422F,
+	0x49547E0B, 0xBB3FFD08, 0xA86F0EFC, 0x5A048DFF, 0x8ECEE914, 0x7CA56A17, 0x6FF599E3, 0x9D9E1AE0,
+	0xD3D3E1AB, 0x21B862A8, 0x32E8915C, 0xC083125F, 0x144976B4, 0xE622F5B7, 0xF5720643, 0x07198540,
+	0x590AB964, 0xAB613A67, 0xB831C993, 0x4A5A4A90, 0x9E902E7B, 0x6CFBAD78, 0x7FAB5E8C, 0x8DC0DD8F,
+	0xE330A81A, 0x115B2B19, 0x020BD8ED, 0xF0605BEE, 0x24AA3F05, 0xD6C1BC06, 0xC5914FF2, 0x37FACCF1,
+	0x69E9F0D5, 0x9B8273D6, 0x88D28022, 0x7AB90321, 0xAE7367CA, 0x5C18E4C9, 0x4F48173D, 0xBD23943E,
+	0xF36E6F75, 0x0105EC76, 0x12551F82, 0xE03E9C81, 0x34F4F86A, 0xC69F7B69, 0xD5CF889D, 0x27A40B9E,
+	0x79B737BA, 0x8BDCB4B9, 0x988C474D, 0x6AE7C44E, 0xBE2DA0A5, 0x4C4623A6, 0x5F16D052, 0xAD7D5351};
+
+/**
+ * @brief Calculates digest (CRC32C).
+ *
+ * Calculates CRC32C with 0x82F63B78 polynomial
+ * reflect according to iSCSI specs.\n
+ * TODO: Implement optimized SSE4.2 and ARM versions
+ *
+ * @param[in] data Pointer to data to calculate CRC32C for.
+ * @param[in] len Length of data to be calculated. Must be
+ * divisable by 4 which is guaranteed by iSCSI standard.
+ * @param[in] crc32c Previous CRC32C in case of multiple passes.
+ * @return CRC32C value. THis function cannot fail.
+ */
+static inline uint32_t iscsi_crc32c_update(const uint8_t *data, const uint len, uint32_t crc32c)
+{
+	for ( uint i = 0; i < len; i += 4 ) {
+		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i]) & 0xFF];
+		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i + 1]) & 0xFF];
+		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i + 2]) & 0xFF];
+		crc32c = (crc32c >> 8UL) ^ crc32c_lut[(crc32c ^ data[i + 3]) & 0xFF];
+	}
+
+	return crc32c;
+}
+
+/**
+ * @brief Calculate and store iSCSI header digest (CRC32C).
+ *
+ * Calculates header digest (CRC32C) with
+ * 0x82F63B78 polynomial reflect according
+ * to iSCSI specs and stores the result in
+ * the iSCSI packet data. This function
+ * cannot fail.
+ *
+ * @param[out] header_digest Pointer to iSCSI header digest
+ * packet data to put CRC32C into.
+ * May NOT be NULL, so be careful.
+ * @param[in] packet_data Pointer to ISCSI BHS packet to
+ * calculate CRC32C for. NULL is NOT
+ * allowed here, take caution.
+ * @param[in] ahs_len AHS segment length in bytes.
+ */
+void iscsi_connection_pdu_digest_header_update(iscsi_header_digest *header_digest, const iscsi_bhs_packet *packet_data, const uint ahs_len)
+{
+	const uint32_t crc32c = iscsi_crc32c_update( (uint8_t *) packet_data, (sizeof(struct iscsi_bhs_packet) + ahs_len), ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
+
+	iscsi_put_le32( (uint8_t *) &header_digest->crc32c, crc32c );
+}
+
+/**
+ * @brief Validates a stored iSCSI header digest (CRC32C) with actual header data.
+ *
+ * Verifies header digest (CRC32C) with
+ * 0x82F63B78 polynomial reflect according
+ * to iSCSI specs. This function cannot
+ * fail.
+ *
+ * @param[in] header_digest Pointer to iSCSI header digest
+ * packet data to compare CRC32C with.
+ * May NOT be NULL, so be careful.
+ * @param[in] packet_data Pointer to ISCSI BHS packet to
+ * validate CRC32C for. May NOT be NULL,
+ * so be careful.
+ * @param[in] ahs_len AHS segment length in bytes.
+ * @retval true CRC32C matches the stored value.
+ * @retval false CRC32C does NOT match the stored value.
+ */
+bool iscsi_connection_pdu_digest_header_verify(const iscsi_header_digest *header_digest, const iscsi_bhs_packet *packet_data, const uint ahs_len)
+{
+	const uint32_t crc32c = iscsi_crc32c_update( (uint8_t *) packet_data, (sizeof(struct iscsi_bhs_packet) + ahs_len), ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
+
+	return (iscsi_get_le32(crc32c) == header_digest->crc32c);
+}
+
+/**
+ * @brief Calculate iSCSI data digest (CRC32C).
+ *
+ * Calculates data digest (CRC32) with
+ * 0x82F63B78 polynomial reflect of a
+ * whole DataSegment (CRC32C) according
+ * to the iSCSI specs.\n
+ * The resulting CRC32C will be stored
+ * in the iSCSI packet.
+ *
+ * @param[out] data_digest Pointer to iSCSI data digest
+ * packet data to put CRC32C into.
+ * May NOT be NULL, so be careful.
+ * @param[in] ds_cmd_data Pointer to iSCSI DataSegment packet to
+ * calculate CRC32C for. NULL is NOT
+ * allowed here, take caution.
+ * @param[in] ds_len Data segment length in bytes.
+ */
+void iscsi_connection_pdu_digest_data_update(iscsi_data_digest *data_digest, const iscsi_scsi_ds_cmd_data *ds_cmd_data, const uint32_t ds_len)
+{
+	const uint32_t crc32c = iscsi_crc32c_update( (uint8_t *) ds_cmd_data, ISCSI_ALIGN(ds_len, ISCSI_DIGEST_SIZE), ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
+
+	iscsi_put_le32( (uint8_t *) &data_digest->crc32c, crc32c );
+}
+
+/**
+ * @brief Validates a stored iSCSI data digest (CRC32C) with actual DataSegment.
+ *
+ * Verifies data digest (CRC32C) with
+ * 0x82F63B78 polynomial reflect according
+ * to iSCSI specs. This function cannot
+ * fail.
+ *
+ * @param[out] data_digest Pointer to iSCSI data digest
+ * packet data to compare CRC32C with.
+ * May NOT be NULL, so be careful.
+ * @param[in] ds_cmd_data Pointer to iSCSI DataSegment
+ * packet to calculate CRC32C for. May NOT
+ * be NULL, so be careful.
+ * @param[in] ds_len Data segment length in bytes.
+ * @retval true CRC32C matches the stored value.
+ * @retval false CRC32C does NOT match the stored value.
+ */
+bool iscsi_connection_pdu_digest_data_verify(const iscsi_data_digest *data_digest, const iscsi_scsi_ds_cmd_data *ds_cmd_data, const uint32_t ds_len)
+{
+	const uint32_t crc32c = iscsi_crc32c_update( (uint8_t *) ds_cmd_data, ISCSI_ALIGN(ds_len, ISCSI_DIGEST_SIZE), ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
+
+	return (iscsi_get_le32(crc32c) == data_digest->crc32c);
+}
+
+/**
+ * @brief Checks whether iSCSI PDU cleanup procedure has to be deferred.
+ *
+ * This function checks whether the cleanup
+ * process of a written PDU has to be
+ * deferred to a later stage.
+ *
+ * @param[in] pdu Pointer to iSCSI PDU to be checked for
+ * deferrred cleanup processs.
+ * @retval true The PDUs cleanup stage has to be
+ * deferred to a later stage.
+ * @retval false The PDU can be cleaned up immediately.
+ */
+static inline bool iscsi_connection_pdu_free_is_deferred(const iscsi_pdu *pdu)
+{
+	return ((pdu != NULL) && ((pdu->bhs_pkt->opcode == ISCSI_OPCODE_SERVER_READY_XFER) || (pdu->bhs_pkt->opcode == ISCSI_OPCODE_SERVER_SCSI_DATA_IN)));
+}
+
+/**
+ * @brief Handles iSCSI PDU cleanup after the PDU has been sent via TCP/IP to the client.
+ *
+ * This function checks whether there are PDU
+ * cleanup actions required and either frees
+ * the PDU or adds it to the PDU Sequence
+ * Number Acknowledgement (SNACK) list.
+ *
+ * @param[in] user_data Pointer to iSCSI PDU which completed
+ * the TCP/IP write. May NOT be NULL, so be
+ * careful.
+ */
+static void iscsi_connection_pdu_write_complete(uint8_t *user_data, int err)
+{
+	iscsi_pdu *pdu         = (iscsi_pdu *) user_data;
+	iscsi_connection *conn = pdu->conn;
+
+	if ( conn->state >= ISCSI_CONNECT_STATE_EXITING )
+		return;
+
+	iscsi_list_remove( &pdu->node );
+
+	if ( err != 0 )
+		conn->state = ISCSI_CONNECT_STATE_EXITING;
+
+	if ( ((conn->flags & ISCSI_CONNECT_FLAGS_FULL_FEATURE) != 0) && (conn->session->err_recovery_level > 0UL) && iscsi_connection_pdu_free_is_deferred( pdu ) )
+		iscsi_list_enqueue( &conn->pdus_snack, &pdu->node );
+	else
+		iscsi_connection_pdu_free( conn, pdu );
 }
 
 /**
@@ -9893,27 +9652,40 @@ void iscsi_connection_pdu_free(iscsi_connection *conn, iscsi_pdu *pdu)
  */
 void iscsi_connection_pdu_write(iscsi_connection *conn, iscsi_pdu *pdu, iscsi_connection_xfer_complete_callback callback, uint8_t *user_data)
 {
-	if ( conn->state >= ISCSI_CONNECT_STATE_EXITING )
-		return;
-
 	if ( ISCSI_GET_OPCODE(pdu->bhs_pkt->opcode) != ISCSI_OPCODE_CLIENT_LOGIN_REQ ) {
-		if ( conn->header_digest != 0 )
-			iscsi_calc_header_digest( pdu->bhs_pkt );
+		if ( pdu->header_digest != NULL )
+			iscsi_connection_pdu_digest_header_update( pdu->header_digest, pdu->bhs_pkt, pdu->ahs_len );
 
-		if ( (conn->data_digest != 0) && (pdu->ds_len != 0U) )
-			iscsi_calc_data_digest( pdu->bhs_pkt, conn->header_digest );
+		if ( pdu->data_digest != NULL )
+			iscsi_connection_pdu_digest_data_update( pdu->data_digest, pdu->ds_cmd_data, pdu->ds_len );
 	}
 
-	const uint len = (uint) (sizeof(struct iscsi_bhs_packet) + pdu->ahs_len + conn->header_digest + iscsi_align(pdu->ds_len, ISCSI_ALIGN_SIZE) + conn->data_digest);
+	pdu->xfer_complete_callback  = callback;
+	pdu->xfer_complete_user_data = user_data;
+
+	iscsi_list_enqueue( &conn->pdus_write, &pdu->node );
+
+	if ( conn->state >= ISCSI_CONNECT_STATE_EXITING )
+		return;
 
-	// TODO: Do the writing in a queue.
-	iscsi_connection_write( conn, (uint8_t *) pdu->bhs_pkt, len );
+	const uint len = (uint) (sizeof(struct iscsi_bhs_packet) + pdu->ahs_len + conn->header_digest + ISCSI_ALIGN(pdu->ds_len, ISCSI_ALIGN_SIZE) + conn->data_digest);
 
 	// TODO: Begin remove after I/O async implementation
+	int32_t rc = iscsi_connection_write( conn, (uint8_t *) pdu->bhs_pkt, len );
+
+	uint64_t *exec_data = malloc( 64 );
+
+	exec_data[2] = (uint64_t *) iscsi_connection_pdu_write_complete;
+	exec_data[3] = 2ULL;
+	exec_data[4] = pdu;
+	exec_data[5] = (rc == (int32_t) len) ? 0 : -1;
+
+	iscsi_list_enqueue( &iscsi_globvec->exec_queue, (iscsi_node *) exec_data );
+
 	if ( callback == NULL )
 		return;
 
-	uint64_t *exec_data = malloc( 64 );
+	exec_data = malloc( 64 );
 
 	exec_data[2] = (uint64_t *) callback;
 	exec_data[3] = 1ULL;
@@ -9922,7 +9694,9 @@ void iscsi_connection_pdu_write(iscsi_connection *conn, iscsi_pdu *pdu, iscsi_co
 	iscsi_list_enqueue( &iscsi_globvec->exec_queue, (iscsi_node *) exec_data );
 	// TODO: End remove after I/O async implementation
 
-//	if ( callback != NULL )
+//	iscsi_connection_pdu_write_complete( (uint8_t *) pdu, (rc == (int32_t) len) ? 0 : -1 );
+//
+//	if ( user_data != NULL )
 //		callback( user_data );
 }
 
@@ -9965,37 +9739,7 @@ static inline int iscsi_seq_num_cmp_gt(const uint32_t seq_num, const uint32_t se
 }
 
 /**
- * @brief iSCSI portal destructor callback for hash map.
- *
- * Callback function for deallocation of an iSCSI
- * portal stored in the iSCSI portal group hash map.
- *
- * @param[in] key Pointer to zero padded key. NULL is
- * an invalid pointer here, so be careful.
- * @param[in] key_size Number of bytes for the key.
- * @param[in] value Value of the key, NULL is allowed.
- * @param[in,out] user_data This argument is not used by
- * this function and should be always NULL for now, as
- * there is a possibility for future usage.
- * @return Always returns 0 as this function cannot fail.
- */
-int iscsi_connection_pdu_ack_remove_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data)
-{
-	iscsi_connection *conn                        = (iscsi_connection *) user_data;
-	iscsi_pdu *pdu                                = (iscsi_pdu *) value;
-	iscsi_scsi_response_packet *scsi_response_pkt = (iscsi_scsi_response_packet *) pdu->bhs_pkt;
-	const uint32_t stat_sn                        = iscsi_get_be32(scsi_response_pkt->stat_sn);
-
-	if ( iscsi_seq_num_cmp_lt( stat_sn, conn->exp_stat_sn ) ) {
-		iscsi_hashmap_remove_free( conn->pdu_snack, key, key_size, iscsi_hashmap_key_destroy_callback, NULL );
-		iscsi_connection_pdu_free( conn, pdu );
-	}
-
-	return 0;
-}
-
-/**
- * @brief Removes an acknowledged PDU from SNACK PDU hash map by ExpStatSN.
+ * @brief Removes an acknowledged PDU from SNACK PDU doubly linked list by ExpStatSN.
  *
  * This function is invoked when ExpStatSN becomes
  * invalid.
@@ -10008,7 +9752,18 @@ void iscsi_connection_pdu_ack_remove(iscsi_connection *conn, const uint32_t exp_
 {
 	conn->exp_stat_sn = (exp_stat_sn < conn->stat_sn) ? exp_stat_sn : conn->stat_sn;
 
-	iscsi_hashmap_iterate( conn->pdu_snack, iscsi_connection_pdu_ack_remove_callback, (uint8_t *) conn );
+	iscsi_pdu *pdu;
+	iscsi_pdu *tmp;
+
+	iscsi_list_foreach_safe_node ( &conn->pdus_snack, pdu, tmp ) {
+		iscsi_scsi_response_packet *scsi_response_pkt = (iscsi_scsi_response_packet *) pdu->bhs_pkt;
+		const uint32_t stat_sn                        = iscsi_get_be32(scsi_response_pkt->stat_sn);
+
+		if ( iscsi_seq_num_cmp_lt( stat_sn, conn->exp_stat_sn ) ) {
+			iscsi_list_remove( &pdu->node );
+			iscsi_connection_pdu_free( conn, pdu );
+		}
+	}
 }
 
 /**
@@ -10030,7 +9785,8 @@ static int iscsi_connection_handle_reject(iscsi_connection *conn, iscsi_pdu *pdu
 {
 	pdu->flags |= ISCSI_PDU_FLAGS_REJECTED;
 
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	const uint32_t ds_len   = (uint32_t) sizeof(struct iscsi_bhs_packet) + ((uint32_t) pdu->bhs_pkt->total_ahs_len << 2UL);
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, ds_len, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_handle_reject: Out of memory while allocating iSCSI reject response PDU" );
@@ -10038,48 +9794,29 @@ static int iscsi_connection_handle_reject(iscsi_connection *conn, iscsi_pdu *pdu
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 	}
 
-	const uint32_t ds_len = (uint32_t) sizeof(struct iscsi_bhs_packet) + ((uint32_t) pdu->bhs_pkt->total_ahs_len << 2UL) + conn->header_digest;
-	iscsi_reject_packet *reject_pkt = (iscsi_reject_packet *) iscsi_append_ds_packet( response_pdu->bhs_pkt, conn->header_digest, ds_len, conn->data_digest );
-
-	if ( reject_pkt == NULL ) {
-		logadd( LOG_ERROR, "iscsi_connection_handle_reject: Out of memory while allocating iSCSI reject packet data" );
+	iscsi_reject_packet *reject_pkt = (iscsi_reject_packet *) response_pdu->bhs_pkt;
 
-		iscsi_connection_pdu_destroy( response_pdu );
-
-		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-	}
-
-	response_pdu->bhs_pkt = (iscsi_bhs_packet *) reject_pkt;
-
-	if ( conn->header_digest != 0 ) {
-		response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) reject_pkt) + 1);
-		response_pdu->header_digest_size = conn->header_digest;
-	}
-
-	response_pdu->ds_cmd_data = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) reject_pkt) + sizeof(struct iscsi_bhs_packet) + conn->header_digest);
-	response_pdu->ds_len      = ds_len;
-
-	if ( conn->data_digest != 0 ) {
-		response_pdu->data_digest      = (iscsi_data_digest *) (((uint8_t *) response_pdu->ds_cmd_data) + iscsi_align(ds_len, ISCSI_ALIGN_SIZE));
-		response_pdu->data_digest_size = conn->data_digest;
-	}
-
-	reject_pkt->opcode = ISCSI_OPCODE_SERVER_REJECT;
-	reject_pkt->flags |= -0x80;
-	reject_pkt->reason = (uint8_t) reason_code;
-	iscsi_put_be24( (uint8_t *) &reject_pkt->ds_len, ds_len );
-	reject_pkt->tag    = 0xFFFFFFFFUL; // Minus one does not require endianess conversion
+	reject_pkt->opcode    = ISCSI_OPCODE_SERVER_REJECT;
+	reject_pkt->flags     = -0x80;
+	reject_pkt->reason    = (uint8_t) reason_code;
+	reject_pkt->reserved  = 0U;
+	iscsi_put_be32( (uint8_t *) &reject_pkt->total_ahs_len, ds_len ); // TotalAHSLength is always 0 and DataSegmentLength is 24-bit, so write in one step.
+	reject_pkt->reserved2 = 0ULL;
+	reject_pkt->tag       = 0xFFFFFFFFUL; // Minus one does not require endianess conversion
+	reject_pkt->reserved3 = 0UL;
 	iscsi_put_be32( (uint8_t *) &reject_pkt->stat_sn, conn->stat_sn++ );
 
 	if ( conn->session != NULL ) {
 		iscsi_put_be32( (uint8_t *) &reject_pkt->exp_cmd_sn, conn->session->exp_cmd_sn );
 		iscsi_put_be32( (uint8_t *) &reject_pkt->max_cmd_sn, conn->session->max_cmd_sn );
 	} else {
-		iscsi_put_be32( (uint8_t *) &reject_pkt->exp_cmd_sn, 1 );
-		iscsi_put_be32( (uint8_t *) &reject_pkt->max_cmd_sn, 1 );
+		iscsi_put_be32( (uint8_t *) &reject_pkt->exp_cmd_sn, 1UL );
+		iscsi_put_be32( (uint8_t *) &reject_pkt->max_cmd_sn, 1UL );
 	}
 
-	memcpy( ((uint8_t *) reject_pkt) + sizeof(struct iscsi_bhs_packet), pdu->bhs_pkt, ds_len );
+	reject_pkt->reserved4 = 0ULL;
+
+	memcpy( response_pdu->ds_cmd_data, pdu->bhs_pkt, ds_len );
 
 	iscsi_connection_pdu_write( conn, response_pdu, NULL, NULL );
 
@@ -10128,7 +9865,7 @@ static int iscsi_connection_update_cmd_sn(iscsi_connection *conn, iscsi_pdu *pdu
 	if ( session->err_recovery_level > 0UL )
 		iscsi_connection_pdu_ack_remove( conn, exp_stat_sn );
 
-	if ( ((scsi_cmd_pkt->opcode & ISCSI_OPCODE_FLAGS_IMMEDIATE) == 0) && (opcode != ISCSI_OPCODE_CLIENT_NOP_OUT) )
+	if ( ((scsi_cmd_pkt->opcode & ISCSI_OPCODE_FLAGS_IMMEDIATE) == 0) && (opcode != ISCSI_OPCODE_CLIENT_SCSI_DATA_OUT) )
 		session->exp_cmd_sn++;
 
 	return ISCSI_CONNECT_PDU_READ_OK;
@@ -10161,12 +9898,12 @@ static int iscsi_connection_pdu_header_handle_login_req(iscsi_connection *conn,
 	if ( pdu->ds_len > ISCSI_DEFAULT_RECV_DS_LEN )
 		return iscsi_connection_handle_reject( conn, pdu, ISCSI_REJECT_REASON_PROTOCOL_ERR );
 
-	iscsi_pdu *login_response_pdu = iscsi_connection_pdu_create( conn );
+	iscsi_pdu *login_response_pdu = iscsi_connection_pdu_create( conn, 0U, 0, ISCSI_DEFAULT_RECV_DS_LEN, 0 );
 
 	if ( login_response_pdu == NULL )
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 
-	const int rc = iscsi_login_response_init( login_response_pdu, pdu );
+	const int rc = iscsi_connection_pdu_login_response_init( login_response_pdu, pdu );
 
 	if ( rc < 0 ) {
 		iscsi_connection_pdu_login_response( conn, login_response_pdu, NULL, iscsi_connection_pdu_login_err_complete );
@@ -10232,6 +9969,37 @@ static int iscsi_connection_pdu_header_handle_nop_out(iscsi_connection *conn, is
  */
 static int iscsi_connection_pdu_header_handle_scsi_cmd(iscsi_connection *conn, iscsi_pdu *pdu)
 {
+	iscsi_scsi_cmd_packet *stat_scsi_cmd_pkt = (iscsi_scsi_cmd_packet *) pdu->bhs_pkt;
+	uint64_t stat_opcode = (uint64_t) stat_scsi_cmd_pkt->scsi_cdb.opcode;
+	uint64_t *stat_value = NULL;
+	int stat_rc = iscsi_hashmap_get( conn->stat_scsi_opcodes, (uint8_t *) &stat_opcode, sizeof(stat_opcode), (uint8_t *) &stat_value );
+
+	if ( stat_value == NULL ) {
+		stat_value = malloc( sizeof(uint64_t) );
+
+		if ( stat_value != NULL ) {
+			uint8_t *stat_key = iscsi_hashmap_key_create( (uint8_t *) &stat_opcode, sizeof(stat_opcode) );
+
+			if ( stat_key != NULL ) {
+				*stat_value = 0ULL;
+
+				stat_rc = iscsi_hashmap_put( conn->stat_scsi_opcodes, stat_key, sizeof(stat_opcode), (uint8_t *) stat_value );
+
+				if ( stat_rc < 0 ) {
+					free( stat_key );
+					free( stat_value );
+					stat_value = NULL;
+				}
+			} else {
+				free( stat_value );
+				stat_value = NULL;
+			}
+		}
+	}
+
+	if ( stat_value != NULL )
+		(*stat_value)++;
+
 	if ( conn->session->type != ISCSI_SESSION_TYPE_NORMAL )
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 
@@ -10443,7 +10211,7 @@ iscsi_pdu *iscsi_r2t_find_pdu_bhs(iscsi_connection *conn, iscsi_pdu *pdu)
  */
 int iscsi_r2t_send(iscsi_connection *conn, iscsi_task *task, uint32_t *r2t_sn, const uint32_t pos, const uint32_t len, const uint32_t target_xfer_tag)
 {
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, 0UL, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_r2t_send: Out of memory while allocating iSCSI Ready To Transfer response PDU" );
@@ -10453,24 +10221,10 @@ int iscsi_r2t_send(iscsi_connection *conn, iscsi_task *task, uint32_t *r2t_sn, c
 
 	iscsi_r2t_packet *r2t_pkt = (iscsi_r2t_packet *) response_pdu->bhs_pkt;
 
-	if ( conn->header_digest != 0 ) {
-		r2t_pkt = (iscsi_r2t_packet *) iscsi_append_header_digest_packet( response_pdu->bhs_pkt, conn->header_digest );
-
-		if ( r2t_pkt == NULL ) {
-			logadd( LOG_ERROR, "iscsi_r2t_send: Out of memory while allocating iSCSI Ready To Transfer packet data" );
-
-			iscsi_connection_pdu_destroy( response_pdu );
-
-			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-		}
-
-		response_pdu->bhs_pkt            = (iscsi_bhs_packet *) r2t_pkt;
-		response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) r2t_pkt) + 1);
-		response_pdu->header_digest_size = conn->header_digest;
-	}
-
-	r2t_pkt->opcode = ISCSI_OPCODE_SERVER_READY_XFER;
-	r2t_pkt->flags  = -0x80;
+	r2t_pkt->opcode                       = ISCSI_OPCODE_SERVER_READY_XFER;
+	r2t_pkt->flags                        = -0x80;
+	r2t_pkt->reserved                     = 0U;
+	*(uint32_t *) &r2t_pkt->total_ahs_len = 0UL; // TotalAHSLength and DataSegmentLength are always 0, so write in one step.
 
 	const uint64_t lun = iscsi_scsi_lun_get_from_scsi( task->lun_id );
 
@@ -10480,6 +10234,7 @@ int iscsi_r2t_send(iscsi_connection *conn, iscsi_task *task, uint32_t *r2t_sn, c
 	iscsi_put_be32( (uint8_t *) &r2t_pkt->stat_sn, conn->stat_sn );
 	iscsi_put_be32( (uint8_t *) &r2t_pkt->exp_cmd_sn, conn->session->exp_cmd_sn );
 	iscsi_put_be32( (uint8_t *) &r2t_pkt->max_cmd_sn, conn->session->max_cmd_sn );
+	r2t_pkt->data_sn = 0UL;
 	iscsi_put_be32( (uint8_t *) &r2t_pkt->r2t_sn, (*r2t_sn)++ );
 
 	task->r2t_data_sn = 0UL;
@@ -10496,53 +10251,15 @@ int iscsi_r2t_send(iscsi_connection *conn, iscsi_task *task, uint32_t *r2t_sn, c
 }
 
 /**
- * @brief Finds and removes an iSCSI PDU by Ready To Transfer Sequence Number (R2TSN).
- *
- * Callback function for each element while iterating
- * through the iSCSI SNACK PDU's.
- *
- * @param[in] key Pointer to zero padded key. NULL is
- * an invalid pointer here, so be careful.
- * @param[in] key_size Number of bytes for the key.
- * @param[in] value Value of the key, NULL creates an
- * empty key assignment.
- * @param[in,out] user_data Pointer to a data structure
- * containing the iSCSI PDU, the iSCSI task,
- * the iSCSI SNACK PDU's hash map and the
- * Ready To Transfer Sequence Number (R2TSN)
- * to be searched for and may NOT be NULL, so
- * be careful.
- * @retval -1 The PDU has been found and stored
- * in the result structure, as well as been
- * removed from the SNACK PDU's hash map.
- * Therefore, no further searching is needed.
- * @retval 0 The PDU has not been found yet.
- */
-int iscsi_r2t_remove_pdu_from_snack_list_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data)
-{
-	iscsi_r2t_remove_pdu *pdu_remove = (iscsi_r2t_remove_pdu *) user_data;
-	iscsi_pdu *pdu = (iscsi_pdu *) value;
-	iscsi_r2t_packet *r2t_pkt = (iscsi_r2t_packet *) pdu->bhs_pkt;
-
-	if ( (pdu->task != pdu_remove->task) || (iscsi_get_be32(r2t_pkt->r2t_sn) != pdu_remove->r2t_sn) )
-		return 0;
-
-	pdu_remove->pdu = pdu;
-	iscsi_hashmap_remove_free( pdu_remove->pdu_snack, key, key_size, iscsi_hashmap_key_destroy_callback, NULL );
-
-	return -1;
-}
-
-/**
- * @brief Searches an iSCSI PDU task by Ready To Transfer Sequence Number (R2TSN) and removes it from PDU SNACK hash map.
+ * @brief Searches an iSCSI PDU task by Ready To Transfer Sequence Number (R2TSN) and removes it from PDU SNACK doubly linked list.
  *
  * This function searches for an iSCSI PDU task
  * by iterating through the iSCSI connection
- * Sequence Number Acknowledgement S(NACK)
+ * Sequence Number Acknowledgement (SNACK)
  * and matches the Ready To Transfer Sequence
  * Number (R2TSN).\n
  * If found, the PDU will be removed from the
- * PDU SNACK list.
+ * PDU SNACK doubly linked list.
  *
  * @param[in] conn Pointer to iSCSI connection to
  * search in the Sequence Number
@@ -10560,11 +10277,21 @@ int iscsi_r2t_remove_pdu_from_snack_list_callback(uint8_t *key, const size_t key
  */
 static iscsi_pdu *iscsi_r2t_remove_pdu_from_snack_list(iscsi_connection *conn, iscsi_task *task, const uint32_t r2t_sn)
 {
-	iscsi_r2t_remove_pdu pdu_remove = {NULL, task, conn->pdu_snack, r2t_sn};
+	iscsi_pdu *pdu;
+
+	iscsi_list_foreach_node ( &conn->pdus_snack, pdu ) {
+		if ( pdu->bhs_pkt->opcode == ISCSI_OPCODE_SERVER_READY_XFER ) {
+			iscsi_r2t_packet *r2t_pkt = (iscsi_r2t_packet *) pdu->bhs_pkt;
 
-	iscsi_hashmap_iterate( conn->pdu_snack, iscsi_r2t_remove_pdu_from_snack_list_callback, (uint8_t *) &pdu_remove );
+			if ( (pdu->task == task) && (iscsi_get_be32(r2t_pkt->r2t_sn) == r2t_sn) ) {
+				iscsi_list_remove( &pdu->node );
 
-	return pdu_remove.pdu;
+				return pdu;
+			}
+		}
+	}
+
+	return NULL;
 }
 
 /**
@@ -10743,7 +10470,7 @@ static int iscsi_connection_pdu_header_handle_logout_req(iscsi_connection *conn,
 	if ( (conn->session != NULL) && (conn->session->type == ISCSI_SESSION_TYPE_DISCOVERY) && (logout_req_pkt->reason_code != ISCSI_LOGOUT_REQ_REASON_CODE_CLOSE_SESSION) )
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, 0UL, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_pdu_header_handle_logout_req: Out of memory while allocating iSCSI logout response PDU" );
@@ -10753,22 +10480,6 @@ static int iscsi_connection_pdu_header_handle_logout_req(iscsi_connection *conn,
 
 	iscsi_logout_response_packet *logout_response_pkt = (iscsi_logout_response_packet *) response_pdu->bhs_pkt;
 
-	if ( conn->header_digest != 0 ) {
-		logout_response_pkt = (iscsi_logout_response_packet *) iscsi_append_header_digest_packet( response_pdu->bhs_pkt, conn->header_digest );
-
-		if ( logout_response_pkt == NULL ) {
-			logadd( LOG_ERROR, "iscsi_connection_pdu_header_handle_logout_req: Out of memory while allocating iSCSI logout packet data" );
-
-			iscsi_connection_pdu_destroy( response_pdu );
-
-			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-		}
-
-		response_pdu->bhs_pkt            = (iscsi_bhs_packet *) logout_response_pkt;
-		response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) logout_response_pkt) + 1);
-		response_pdu->header_digest_size = conn->header_digest;
-	}
-
 	logout_response_pkt->opcode = ISCSI_OPCODE_SERVER_LOGOUT_RES;
 	logout_response_pkt->flags  = -0x80;
 
@@ -10782,7 +10493,11 @@ static int iscsi_connection_pdu_header_handle_logout_req(iscsi_connection *conn,
 		logout_response_pkt->response = ISCSI_LOGOUT_RESPONSE_CID_NOT_FOUND;
 	}
 
-	logout_response_pkt->init_task_tag = logout_req_pkt->init_task_tag; // Copying over doesn't change endianess.
+	logout_response_pkt->reserved                     = 0U;
+	*(uint32_t *) &logout_response_pkt->total_ahs_len = 0UL; // TotalAHSLength and DataSegmentLength are always 0, so write in one step.
+	logout_response_pkt->reserved2                    = 0ULL;
+	logout_response_pkt->init_task_tag                = logout_req_pkt->init_task_tag; // Copying over doesn't change endianess.
+	logout_response_pkt->reserved3                    = 0UL;
 	iscsi_put_be32( (uint8_t *) &logout_response_pkt->stat_sn, conn->stat_sn++ );
 
 	if ( conn->session != NULL ) {
@@ -10796,8 +10511,10 @@ static int iscsi_connection_pdu_header_handle_logout_req(iscsi_connection *conn,
 		iscsi_put_be32( (uint8_t *) &logout_response_pkt->max_cmd_sn, pdu->cmd_sn );
 	}
 
+	logout_response_pkt->reserved4   = 0UL;
 	logout_response_pkt->time_wait   = 0U;
 	logout_response_pkt->time_retain = 0U;
+	logout_response_pkt->reserved5   = 0UL;
 
 	iscsi_connection_pdu_write( conn, response_pdu, NULL, NULL );
 
@@ -10852,12 +10569,12 @@ static int iscsi_connection_pdu_header_handle(iscsi_connection *conn, iscsi_pdu
 		return iscsi_connection_pdu_header_handle_login_req( conn, pdu );
 
 	if ( ((conn->flags & ISCSI_CONNECT_FLAGS_FULL_FEATURE) == 0) && (conn->state == ISCSI_CONNECT_STATE_RUNNING) ) {
-		iscsi_pdu *login_response_pdu = iscsi_connection_pdu_create( conn );
+		iscsi_pdu *login_response_pdu = iscsi_connection_pdu_create( conn, 0U, 0, 0UL, 0 );
 
 		if ( login_response_pdu == NULL )
 			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 
-		iscsi_login_response_reject_init( login_response_pdu, pdu );
+		iscsi_connection_login_response_reject( login_response_pdu, pdu );
 		iscsi_connection_pdu_write( conn, login_response_pdu, NULL, NULL );
 
 		return ISCSI_CONNECT_PDU_READ_ERR_LOGIN_RESPONSE;
@@ -10934,7 +10651,7 @@ static int iscsi_connection_pdu_header_handle(iscsi_connection *conn, iscsi_pdu
 static int iscsi_connection_pdu_data_handle_nop_out(iscsi_connection *conn, iscsi_pdu *pdu)
 {
 	iscsi_nop_out_packet *nop_out_pkt = (iscsi_nop_out_packet *) pdu->bhs_pkt;
-	uint32_t ds_len = pdu->ds_len;
+	uint32_t ds_len                   = pdu->ds_len;
 
 	if ( ds_len > conn->max_recv_ds_len )
 		ds_len = conn->max_recv_ds_len;
@@ -10947,7 +10664,7 @@ static int iscsi_connection_pdu_data_handle_nop_out(iscsi_connection *conn, iscs
 	if ( init_task_tag == 0xFFFFFFFFUL )
 		return ISCSI_CONNECT_PDU_READ_OK;
 
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, ds_len, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_pdu_data_handle_nop_out: Out of memory while allocating iSCSI NOP-In response PDU" );
@@ -10955,34 +10672,12 @@ static int iscsi_connection_pdu_data_handle_nop_out(iscsi_connection *conn, iscs
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 	}
 
-	iscsi_nop_in_packet *nop_in_pkt = (iscsi_nop_in_packet *) iscsi_append_ds_packet( response_pdu->bhs_pkt, conn->header_digest, ds_len, conn->data_digest );
-
-	if ( nop_in_pkt == NULL ) {
-		logadd( LOG_ERROR, "iscsi_connection_pdu_data_handle_nop_out: Out of memory while allocating iSCSI NOP-In packet data" );
-
-		iscsi_connection_pdu_destroy( response_pdu );
-
-		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-	}
-
-	response_pdu->bhs_pkt = (iscsi_bhs_packet *) nop_in_pkt;
-
-	if ( conn->header_digest != 0 ) {
-		response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) nop_in_pkt) + 1);
-		response_pdu->header_digest_size = conn->header_digest;
-	}
-
-	response_pdu->ds_cmd_data = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) nop_in_pkt) + sizeof(struct iscsi_bhs_packet) + conn->header_digest);
-	response_pdu->ds_len      = ds_len;
-
-	if ( conn->data_digest != 0 ) {
-		response_pdu->data_digest      = (iscsi_data_digest *) (((uint8_t *) response_pdu->ds_cmd_data) + iscsi_align(ds_len, ISCSI_ALIGN_SIZE));
-		response_pdu->data_digest_size = conn->data_digest;
-	}
+	iscsi_nop_in_packet *nop_in_pkt = (iscsi_nop_in_packet *) response_pdu->bhs_pkt;
 
 	nop_in_pkt->opcode          = ISCSI_OPCODE_SERVER_NOP_IN;
 	nop_in_pkt->flags           = -0x80;
-	iscsi_put_be24( (uint8_t *) &nop_in_pkt->ds_len, ds_len );
+	nop_in_pkt->reserved        = 0U;
+	iscsi_put_be32( (uint8_t *) &nop_in_pkt->total_ahs_len, ds_len ); // TotalAHSLength is always 0 and DataSegmentLength is 24-bit, so write in one step.
 	iscsi_put_be64( (uint8_t *) &nop_in_pkt->lun, lun );
 	nop_in_pkt->target_xfer_tag = 0xFFFFFFFFUL; // Minus one does not require endianess conversion
 	iscsi_put_be32( (uint8_t *) &nop_in_pkt->init_task_tag, init_task_tag );
@@ -10993,6 +10688,11 @@ static int iscsi_connection_pdu_data_handle_nop_out(iscsi_connection *conn, iscs
 
 	iscsi_put_be32( (uint8_t *) &nop_in_pkt->exp_cmd_sn, conn->session->exp_cmd_sn );
 	iscsi_put_be32( (uint8_t *) &nop_in_pkt->max_cmd_sn, conn->session->max_cmd_sn );
+	nop_in_pkt->reserved2 = 0UL;
+	nop_in_pkt->reserved3 = 0ULL;
+
+	if ( ds_len != 0UL )
+		memcpy( response_pdu->ds_cmd_data, pdu->ds_cmd_data, ds_len );
 
 	iscsi_connection_pdu_write( conn, response_pdu, NULL, NULL );
 
@@ -11385,7 +11085,7 @@ static int iscsi_connection_login_set_target_info(iscsi_connection *conn, iscsi_
 			return ISCSI_CONNECT_PDU_READ_ERR_LOGIN_PARAMETER;
 	}
 
-	uint8_t *tmp_buf = iscsi_sprintf_alloc( "%s:%s,%d", conn->portal_host, conn->portal_port, conn->pg_tag );
+	uint8_t *tmp_buf = iscsi_sprintf_alloc( "%s:%s,%" PRIu64, conn->portal_host, conn->portal_port, conn->pg_tag );
 
 	if ( tmp_buf == NULL )
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
@@ -11417,7 +11117,7 @@ static int iscsi_connection_login_set_target_info(iscsi_connection *conn, iscsi_
 			if ( ds_len < 0L )
 				return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 
-			login_response_pdu->ds_len = ds_len;
+			login_response_pdu->len = ds_len;
 		}
 
 		if ( type == ISCSI_SESSION_TYPE_DISCOVERY ) {
@@ -11563,9 +11263,9 @@ static int iscsi_connecction_handle_login_response_csg_bit(iscsi_connection *con
 			if ( strcasecmp( (char *) auth_method, "None" ) == 0 ) {
 				conn->flags |= ISCSI_CONNECT_FLAGS_AUTH;
 			} else {
-				const int ds_len = iscsi_connection_auth_key_value_pairs( conn, key_value_pairs, auth_method, (uint8_t *) login_response_pdu->ds_cmd_data, login_response_pdu->pos, login_response_pdu->len );
+				const int32_t ds_len = iscsi_connection_auth_key_value_pairs( conn, key_value_pairs, auth_method, (uint8_t *) login_response_pdu->ds_cmd_data, login_response_pdu->ds_len, login_response_pdu->len );
 
-				if ( ds_len < 0 ) {
+				if ( ds_len < 0L ) {
 					login_response_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_CLIENT_ERR;
 					login_response_pkt->status_detail = ISCSI_LOGIN_RESPONSE_STATUS_DETAILS_CLIENT_ERR_AUTH_ERR;
 
@@ -11724,7 +11424,7 @@ static int iscsi_connecction_handle_login_response_t_bit(iscsi_connection *conn,
 static int iscsi_connecction_handle_login_response(iscsi_connection *conn, iscsi_pdu *login_response_pdu, iscsi_hashmap *key_value_pairs)
 {
 	iscsi_login_response_packet *login_response_pkt = (iscsi_login_response_packet *) login_response_pdu->bhs_pkt;
-	const int32_t ds_len = iscsi_negotiate_key_value_pairs( conn, key_value_pairs, (uint8_t *) login_response_pdu->ds_cmd_data, login_response_pdu->pos, login_response_pdu->len );
+	const int32_t ds_len = iscsi_negotiate_key_value_pairs( conn, key_value_pairs, (uint8_t *) login_response_pdu->ds_cmd_data, login_response_pdu->ds_len, login_response_pdu->len );
 
 	if ( ds_len < 0L ) {
 		login_response_pkt->status_class  = ISCSI_LOGIN_RESPONSE_STATUS_CLASS_CLIENT_ERR;
@@ -11863,7 +11563,7 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 		conn->text_key_value_pairs = tmp_hashmap;
 	}
 
-	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn );
+	iscsi_pdu *response_pdu = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, conn->max_recv_ds_len, conn->data_digest );
 
 	if ( response_pdu == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_pdu_data_handle_text_req: Out of memory while allocating iSCSI text response PDU" );
@@ -11874,12 +11574,11 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 	}
 
-	const uint32_t ds_len = conn->max_recv_ds_len;
-	iscsi_text_response_packet *text_response_pkt = (iscsi_text_response_packet *) iscsi_append_ds_packet( response_pdu->bhs_pkt, conn->header_digest, ds_len, conn->data_digest );
+	response_pdu->ds_len = 0UL;
 
-	if ( text_response_pkt == NULL ) {
-		logadd( LOG_ERROR, "iscsi_connection_pdu_data_handle_text_req: Out of memory while allocating iSCSI text packet data" );
+	int32_t ds_len = iscsi_negotiate_key_value_pairs( conn, key_value_pairs, (uint8_t *) response_pdu->ds_cmd_data, response_pdu->ds_len, response_pdu->len );
 
+	if ( ds_len < 0L ) {
 		iscsi_hashmap_iterate( key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
 		iscsi_hashmap_destroy( key_value_pairs );
 		iscsi_connection_pdu_destroy( response_pdu );
@@ -11887,32 +11586,10 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 	}
 
-	response_pdu->bhs_pkt = (iscsi_bhs_packet *) text_response_pkt;
-
-	if ( conn->header_digest != 0 ) {
-		response_pdu->header_digest      = (iscsi_header_digest *) (((iscsi_bhs_packet *) text_response_pkt) + 1);
-		response_pdu->header_digest_size = conn->header_digest;
-	}
-
-	response_pdu->ds_cmd_data = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) text_response_pkt) + sizeof(struct iscsi_bhs_packet) + conn->header_digest);
-	response_pdu->ds_len      = ds_len;
-
-	if ( conn->data_digest != 0 ) {
-		response_pdu->data_digest      = (iscsi_data_digest *) (((uint8_t *) response_pdu->ds_cmd_data) + iscsi_align(ds_len, ISCSI_ALIGN_SIZE));
-		response_pdu->data_digest_size = conn->data_digest;
-	}
-
-	response_pdu->pos = iscsi_negotiate_key_value_pairs( conn, key_value_pairs, (uint8_t *) response_pdu->ds_cmd_data, response_pdu->pos, response_pdu->ds_len );
-
-	if ( (int32_t) response_pdu->pos < 0L ) {
-		iscsi_hashmap_iterate( key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
-		iscsi_hashmap_destroy( key_value_pairs );
-		iscsi_connection_pdu_destroy( response_pdu );
-
-		return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-	}
+	iscsi_text_response_packet *text_response_pkt = (iscsi_text_response_packet *) response_pdu->bhs_pkt;
 
 	text_response_pkt->opcode = ISCSI_OPCODE_SERVER_TEXT_RES;
+	text_response_pkt->flags  = 0;
 
 	if ( (text_req_pkt->flags & ISCSI_TEXT_REQ_FLAGS_CONTINUE) != 0 )
 		text_response_pkt->flags |= (int8_t) ISCSI_TEXT_RESPONSE_FLAGS_CONTINUE;
@@ -11920,6 +11597,8 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 	if ( (text_req_pkt->flags & ISCSI_TEXT_REQ_FLAGS_FINAL) != 0 )
 		text_response_pkt->flags |= (int8_t) ISCSI_TEXT_RESPONSE_FLAGS_FINAL;
 
+	text_req_pkt->reserved = 0U;
+
 	uint8_t *send_targets_val;
 	rc = iscsi_get_key_value_pair( key_value_pairs, ISCSI_LOGIN_AUTH_SESSION_TEXT_KEY_SEND_TARGETS, &send_targets_val );
 
@@ -11942,7 +11621,7 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 			if ( send_targets_val[0] == '\0' )
 				send_targets_val = (uint8_t *) "ALL";
 
-			response_pdu->pos = iscsi_target_node_send( conn, send_targets_val, conn->init_name, (uint8_t *) response_pdu->ds_cmd_data, response_pdu->pos, response_pdu->ds_len );
+			ds_len = iscsi_target_node_send( conn, send_targets_val, conn->init_name, (uint8_t *) response_pdu->ds_cmd_data, ds_len, response_pdu->len );
 		} else {
 			if ( send_targets_val[0] == '\0' )
 				send_targets_val = conn->target_port->name;
@@ -11959,9 +11638,9 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 					return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 				}
 
-				response_pdu->pos = iscsi_append_key_value_pair_packet( key_value_pair, ISCSI_LOGIN_AUTH_SESSION_TEXT_KEY_SEND_TARGETS, (uint8_t *) "Reject", (uint8_t *) response_pdu->ds_cmd_data, response_pdu->pos, response_pdu->ds_len );
+				ds_len = iscsi_append_key_value_pair_packet( key_value_pair, ISCSI_LOGIN_AUTH_SESSION_TEXT_KEY_SEND_TARGETS, (uint8_t *) "Reject", (uint8_t *) response_pdu->ds_cmd_data, ds_len, response_pdu->len );
 			} else {
-				response_pdu->pos = iscsi_target_node_send( conn, send_targets_val, conn->init_name, (uint8_t *) response_pdu->ds_cmd_data, response_pdu->pos, response_pdu->ds_len );
+				ds_len = iscsi_target_node_send( conn, send_targets_val, conn->init_name, (uint8_t *) response_pdu->ds_cmd_data, ds_len, response_pdu->len );
 			}
 		}
 
@@ -11971,7 +11650,7 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 		}
 	}
 
-	if ( (int32_t) response_pdu->pos < 0L ) {
+	if ( ds_len < 0L ) {
 		iscsi_hashmap_iterate( key_value_pairs, iscsi_hashmap_key_destroy_value_callback, NULL );
 		iscsi_hashmap_destroy( key_value_pairs );
 		iscsi_connection_pdu_destroy( response_pdu );
@@ -11986,7 +11665,9 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 		conn->text_key_value_pairs = key_value_pairs;
 	}
 
-	iscsi_put_be24( (uint8_t *) &text_response_pkt->ds_len, (uint32_t) response_pdu->pos );
+	text_response_pkt = (iscsi_text_response_packet *) iscsi_connection_pdu_append( response_pdu, response_pdu->ahs_len, conn->header_digest, ds_len, conn->data_digest );
+
+	iscsi_put_be32( (uint8_t *) &text_response_pkt->total_ahs_len, ds_len ); // TotalAHSLength is always 0 and DataSegmentLength is 24-bit, so write in one step.
 	text_response_pkt->lun           = text_req_pkt->lun; // Copying over doesn't change endianess.
 	text_response_pkt->init_task_tag = text_req_pkt->init_task_tag; // Copying over doesn't change endianess.
 
@@ -12005,6 +11686,8 @@ static int iscsi_connection_pdu_data_handle_text_req(iscsi_connection *conn, isc
 
 	iscsi_put_be32( (uint8_t *) &text_response_pkt->exp_cmd_sn, conn->session->exp_cmd_sn );
 	iscsi_put_be32( (uint8_t *) &text_response_pkt->max_cmd_sn, conn->session->max_cmd_sn );
+	text_response_pkt->reserved2[0] = 0ULL;
+	text_response_pkt->reserved2[1] = 0ULL;
 
 	iscsi_connection_pdu_write( conn, response_pdu, iscsi_connection_pdu_text_complete, (uint8_t *) conn );
 
@@ -12115,27 +11798,9 @@ static int iscsi_connection_pdu_data_handle(iscsi_connection *conn, iscsi_pdu *p
 int iscsi_connection_pdu_data_read(iscsi_connection *conn, iscsi_pdu *pdu)
 {
 	const uint32_t ds_len = pdu->ds_len;
-	uint8_t *buf          = (uint8_t *) pdu->ds_cmd_data;
-
-	if ( buf == NULL ) {
-		buf = (uint8_t *) iscsi_append_ds_packet( (iscsi_bhs_packet *) pdu->bhs_pkt, conn->header_digest, ds_len, conn->data_digest );
-
-		if ( buf == NULL )
-			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-
-		pdu->bhs_pkt       = (iscsi_bhs_packet *) buf;
-		pdu->ahs_pkt       = (iscsi_ahs_packet *) (((iscsi_bhs_packet *) pdu->bhs_pkt) + 1);
-		pdu->header_digest = (iscsi_header_digest *) (((uint8_t *) pdu->bhs_pkt) + sizeof(struct iscsi_bhs_packet) + pdu->ahs_len);
-		pdu->ds_cmd_data   = (iscsi_scsi_ds_cmd_data *) (((uint8_t *) pdu->bhs_pkt) + sizeof(struct iscsi_bhs_packet) + pdu->ahs_len + conn->header_digest);
-
-		if ( conn->data_digest != 0 )
-			pdu->data_digest = (iscsi_data_digest *) (((uint8_t *) pdu->bhs_pkt) + sizeof(struct iscsi_bhs_packet) + pdu->ahs_len + conn->header_digest + ds_len);
-
-		pdu->data_digest_size = conn->data_digest;
-	}
 
 	if ( pdu->pos < ds_len ) {
-		const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->ds_cmd_data) + pdu->pos), (pdu->ds_len - pdu->pos) );
+		const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->ds_cmd_data) + pdu->pos), (ds_len - pdu->pos) );
 
 		if ( len < 0L )
 			return len;
@@ -12146,20 +11811,20 @@ int iscsi_connection_pdu_data_read(iscsi_connection *conn, iscsi_pdu *pdu)
 	if ( pdu->pos < ds_len )
 		return ISCSI_CONNECT_PDU_READ_PROCESSED;
 
-	if ( conn->data_digest != 0 ) {
-		if ( pdu->data_digest_pos < (uint) conn->data_digest ) {
-			const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->data_digest) + pdu->data_digest_pos), (conn->data_digest - pdu->data_digest_pos) );
+	if ( pdu->data_digest != NULL ) {
+		if ( (int) pdu->data_digest_pos < pdu->data_digest_size ) {
+			const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->data_digest) + pdu->data_digest_pos), (pdu->data_digest_size - pdu->data_digest_pos) );
 
 			if ( len < 0L )
 				return len;
 
 			pdu->data_digest_pos += len;
 
-			if ( pdu->data_digest_pos < (uint) conn->data_digest )
+			if ( (int) pdu->data_digest_pos < pdu->data_digest_size )
 				return ISCSI_CONNECT_PDU_READ_OK;
 		}
 
-		if  ( iscsi_validate_data_digest( pdu->bhs_pkt, conn->data_digest ) == 0 )
+		if  ( !iscsi_connection_pdu_digest_data_verify( pdu->data_digest, pdu->ds_cmd_data, ds_len ) )
 			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
 	}
 
@@ -12193,7 +11858,7 @@ static int iscsi_connection_pdu_read(iscsi_connection *conn)
 
 		switch ( conn->pdu_recv_state ) {
 			case ISCSI_CONNECT_PDU_RECV_STATE_WAIT_PDU_READY : {
-				conn->pdu_processing = iscsi_connection_pdu_create( conn );
+				conn->pdu_processing = iscsi_connection_pdu_create( conn, 0U, conn->header_digest, 0UL, conn->data_digest );
 
 				if ( conn->pdu_processing == NULL )
 					return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
@@ -12224,24 +11889,46 @@ static int iscsi_connection_pdu_read(iscsi_connection *conn)
 					break;
 				}
 
-				pdu->ds_len  = iscsi_get_be24(pdu->bhs_pkt->ds_len);
-				pdu->ds_len  = iscsi_align(pdu->ds_len, ISCSI_ALIGN_SIZE);
-				pdu->pos     = 0UL;
-				pdu->len     = pdu->ds_len;
+				iscsi_bhs_packet *bhs_pkt = pdu->bhs_pkt;
+				const uint ahs_len        = ((uint) bhs_pkt->total_ahs_len << 2U);
+				const uint32_t ds_len     = iscsi_get_be24(bhs_pkt->ds_len);
 
-				const uint ahs_len = ((uint) pdu->bhs_pkt->total_ahs_len << 2U);
+				bhs_pkt = iscsi_connection_pdu_append( pdu, ahs_len, conn->header_digest, ds_len, conn->data_digest );
 
-				if ( pdu->ahs_pos < ahs_len ) {
-					if ( pdu->ahs_pkt == NULL ) {
-						pdu->ahs_pkt = (iscsi_ahs_packet *) iscsi_append_ahs_packet( pdu->bhs_pkt, (uint32_t) ahs_len );
+				if ( bhs_pkt == NULL )
+					return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
+
+				uint64_t stat_opcode = (uint64_t) ISCSI_GET_OPCODE(bhs_pkt->opcode);
+				uint64_t *stat_value = NULL;
+				int stat_rc = iscsi_hashmap_get( conn->stat_iscsi_opcodes, (uint8_t *) &stat_opcode, sizeof(stat_opcode), (uint8_t *) &stat_value );
+
+				if ( stat_value == NULL ) {
+					stat_value = malloc( sizeof(uint64_t) );
+
+					if ( stat_value != NULL ) {
+						uint8_t *stat_key = iscsi_hashmap_key_create( (uint8_t *) &stat_opcode, sizeof(stat_opcode) );
 
-						if ( pdu->ahs_pkt == NULL )
-							return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
+						if ( stat_key != NULL ) {
+							*stat_value = 0ULL;
 
-						pdu->bhs_pkt = (iscsi_bhs_packet *) pdu->ahs_pkt;
-						pdu->ahs_pkt = (iscsi_ahs_packet *) (((iscsi_bhs_packet *) pdu->bhs_pkt) + 1);
+							stat_rc = iscsi_hashmap_put( conn->stat_iscsi_opcodes, stat_key, sizeof(stat_opcode), (uint8_t *) stat_value );
+
+							if ( stat_rc < 0 ) {
+								free( stat_key );
+								free( stat_value );
+								stat_value = NULL;
+							}
+						} else {
+							free( stat_value );
+							stat_value = NULL;
+						}
 					}
+				}
 
+				if ( stat_value != NULL )
+					(*stat_value)++;
+
+				if ( pdu->ahs_pos < ahs_len ) {
 					const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->ahs_pkt) + pdu->ahs_pos), (ahs_len - pdu->ahs_pos) );
 
 					if ( len < 0L ) {
@@ -12256,20 +11943,9 @@ static int iscsi_connection_pdu_read(iscsi_connection *conn)
 						return ISCSI_CONNECT_PDU_READ_OK;
 				}
 
-				if ( conn->header_digest != 0 ) {
-					if ( pdu->header_digest == NULL ) {
-						pdu->header_digest = (iscsi_header_digest *) iscsi_append_header_digest_packet( pdu->bhs_pkt, ISCSI_DIGEST_SIZE );
-
-						if ( pdu->header_digest == NULL )
-							return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
-
-						pdu->bhs_pkt       = (iscsi_bhs_packet *) pdu->header_digest;
-						pdu->ahs_pkt       = (iscsi_ahs_packet *) (((iscsi_bhs_packet *) pdu->bhs_pkt) + 1);
-						pdu->header_digest = (iscsi_header_digest *) (((uint8_t *) pdu->bhs_pkt) + sizeof(struct iscsi_bhs_packet) + ahs_len);
-					}
-
-					if ( pdu->header_digest_pos < (uint) conn->header_digest ) {
-						const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->header_digest) + pdu->header_digest_pos), (conn->header_digest - pdu->header_digest_pos) );
+				if ( pdu->header_digest != NULL ) {
+					if ( (int) pdu->header_digest_pos < pdu->header_digest_size ) {
+						const int32_t len = iscsi_connection_read( conn, (((uint8_t *) pdu->header_digest) + pdu->header_digest_pos), (pdu->header_digest_size - pdu->header_digest_pos) );
 
 						if ( len < 0L ) {
 							conn->pdu_recv_state = ISCSI_CONNECT_PDU_RECV_STATE_ERR;
@@ -12279,11 +11955,11 @@ static int iscsi_connection_pdu_read(iscsi_connection *conn)
 
 						pdu->header_digest_pos += len;
 
-						if ( pdu->header_digest_pos < (uint) conn->header_digest )
+						if ( (int) pdu->header_digest_pos < pdu->header_digest_size )
 							return ISCSI_CONNECT_PDU_READ_OK;
 					}
 
-					if  ( iscsi_validate_header_digest( pdu->bhs_pkt ) == 0 ) {
+					if  ( !iscsi_connection_pdu_digest_header_verify( pdu->header_digest, bhs_pkt, ahs_len ) ) {
 						conn->pdu_recv_state = ISCSI_CONNECT_PDU_RECV_STATE_ERR;
 
 						break;
@@ -12365,8 +12041,11 @@ int iscsi_connection_pdu_handle(iscsi_connection *conn)
 	for ( i = 0; i < ISCSI_PDU_HANDLE_COUNT; i++ ) {
 		pthread_rwlock_wrlock( &iscsi_globvec_rwlock );
 
-		if ( iscsi_globvec == NULL )
+		if ( iscsi_globvec == NULL ) {
+			pthread_rwlock_unlock( &iscsi_globvec_rwlock );
+
 			return ISCSI_CONNECT_PDU_READ_ERR_FATAL;
+		}
 
 		// TODO: Remove after I/O sync implementation
 		iscsi_list_create( &iscsi_globvec->exec_queue );
@@ -12383,6 +12062,9 @@ int iscsi_connection_pdu_handle(iscsi_connection *conn)
 			if ( exec_data[3] == 1ULL ) {
 				iscsi_scsi_task_xfer_complete_callback callback = (iscsi_scsi_task_xfer_complete_callback) exec_data[2];
 				callback( (uint8_t *) exec_data[4] );
+			} else if ( exec_data[3] == 2ULL ) {
+				iscsi_connection_write_complete_callback callback = (iscsi_connection_write_complete_callback) exec_data[2];
+				callback( (uint8_t *) exec_data[4], (int) exec_data[5] );
 			} else if ( exec_data[3] == 3ULL ) {
 				iscsi_scsi_emu_io_complete_callback callback = (iscsi_scsi_emu_io_complete_callback) exec_data[2];
 				callback( (dnbd3_image_t *) exec_data[4], (uint8_t *) exec_data[5], (bool) exec_data[6] );
@@ -12395,9 +12077,9 @@ int iscsi_connection_pdu_handle(iscsi_connection *conn)
 
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
-		if ( rc == 0 )
+		if ( rc == ISCSI_CONNECT_PDU_READ_OK )
 			break;
-		else if ( rc < 0 )
+		else if ( rc == ISCSI_CONNECT_PDU_READ_ERR_FATAL )
 			return rc;
 
 		if ( (conn->flags & ISCSI_CONNECT_FLAGS_STOPPED) != 0 )
@@ -12426,12 +12108,6 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 	_Static_assert( sizeof(dnbd3_request_t) <= sizeof(struct iscsi_bhs_packet), "DNBD3 request size larger than iSCSI BHS packet data size - Manual intervention required!" );
 	sock_setTimeout( client->sock, 1000L * 3600L ); // TODO: Remove after finishing iSCSI implementation
 
-	host_to_string( &client->host, client->hostName, HOSTNAMELEN );
-	const uint8_t *port = memchr( client->hostName, ':', HOSTNAMELEN );
-
-	if ( port != NULL )
-		port++;
-
 	pthread_rwlock_wrlock( &iscsi_globvec_rwlock );
 
 	if ( iscsi_globvec == NULL )
@@ -12477,11 +12153,34 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 		}
 	}
 
-	uint8_t *tmp_buf = iscsi_sprintf_alloc( "%s:%s", client->hostName, port );
+	host_to_string( &client->host, client->hostName, HOSTNAMELEN );
+
+	const uint8_t *port = memchr( client->hostName, ':', HOSTNAMELEN );
+	const uint host_len = (port != NULL) ? (uint) (port++ - (uint8_t *) client->hostName) : (uint) strlen( client->hostName );
+	uint8_t *host       = malloc( (host_len + 1U) );
+
+	if ( host == NULL ) {
+		logadd( LOG_ERROR, "iscsi_connection_handle: Out of memory while allocating iSCSI portal host name" );
+
+		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
+
+		return;
+	}
+
+	memcpy( host, client->hostName, host_len );
+	host[host_len] = '\0';
+
+	uint8_t *tmp_buf;
+
+	if ( port != NULL )
+		tmp_buf = iscsi_sprintf_alloc( "%s:%s", host, port );
+	else
+		tmp_buf = iscsi_sprintf_alloc( "%s:%u", host, PORT );
 
 	if ( tmp_buf == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_handle: Out of memory while allocating temporarily iSCSI portal name" );
 
+		free( host );
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
 		return;
@@ -12496,6 +12195,7 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 	if ( hash_key == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_handle: Out of memory while allocating temporarily iSCSI portal name hash key" );
 
+		free( host );
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
 		return;
@@ -12505,11 +12205,17 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 	rc = iscsi_hashmap_get( portal_group->portals, (uint8_t *) hash_key, key_len, (uint8_t **) &portal );
 
 	if ( portal == NULL ) {
-		portal = iscsi_portal_create( (uint8_t *) client->hostName, port );
+		if ( port == NULL ) {
+			port = (uint8_t *) strchr( (char *) hash_key, ':' );
+			port++;
+		}
+
+		portal = iscsi_portal_create( host, port );
 
 		if ( portal == NULL ) {
 			logadd( LOG_ERROR, "iscsi_connection_handle: Out of memory while allocating iSCSI portal" );
 
+			free( host );
 			pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
 			return;
@@ -12519,18 +12225,21 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 
 		if ( rc < 0 ) {
 			iscsi_portal_destroy( portal );
-
+			free( host );
 			pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
 			return;
 		}
 	}
 
+	free( host );
+
 	iscsi_connection *conn = iscsi_connection_create( portal, client->sock );
 
 	if ( conn == NULL ) {
 		logadd( LOG_ERROR, "iscsi_connection_handle: Out of memory while allocating iSCSI connection" );
 
+		iscsi_portal_group_del_portal( portal_group, portal );
 		iscsi_portal_destroy( portal );
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
@@ -12543,17 +12252,19 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 		logadd( LOG_ERROR, "iscsi_connection_handle: Out of memory while allocating iSCSI connection" );
 
 		iscsi_connection_destroy( conn );
+		iscsi_portal_group_del_portal( portal_group, portal );
 		iscsi_portal_destroy( portal );
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
 		return;
 	}
 
-	conn->pdu_processing = iscsi_connection_pdu_create( conn );
+	conn->pdu_processing = iscsi_connection_pdu_create( conn, 0U, 0, 0UL, 0 );
 
 	if ( conn->pdu_processing == NULL ) {
 		iscsi_hashmap_key_destroy( (uint8_t *) hash_key );
 		iscsi_connection_destroy( conn );
+		iscsi_portal_group_del_portal( portal_group, portal );
 		iscsi_portal_destroy( portal );
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
@@ -12575,6 +12286,7 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 		iscsi_connection_pdu_destroy( conn->pdu_processing );
 		iscsi_hashmap_key_destroy( (uint8_t *) hash_key );
 		iscsi_connection_destroy( conn );
+		iscsi_portal_group_del_portal( portal_group, portal );
 		iscsi_portal_destroy( portal );
 		pthread_rwlock_unlock( &iscsi_globvec_rwlock );
 
@@ -12585,4 +12297,31 @@ void iscsi_connection_handle(dnbd3_client_t *client, const dnbd3_request_t *requ
 
 	while ( iscsi_connection_pdu_handle( conn ) >= ISCSI_CONNECT_PDU_READ_OK ) {
 	}
+
+	pthread_rwlock_wrlock( &iscsi_globvec_rwlock );
+
+	if ( iscsi_globvec == NULL )
+		return;
+
+	iscsi_hashmap_bucket *stat_bucket;
+
+	iscsi_list_foreach_node ( &conn->stat_iscsi_opcodes->list, stat_bucket ) {
+		uint64_t *stat_opcode = (uint64_t *) stat_bucket->value;
+
+		logadd( LOG_INFO, "iSCSI opcode usage statistics for device %s from initiator %s using port %s and portal %s:%s: Opcode 0x%02" PRIX64 " has been received %" PRIu64 " times until connection drop.", (conn->device != NULL ? conn->device->name : "(null)"), conn->init_name, ((conn->init_port != NULL) ? conn->init_port->name : "(null)"), portal->host, portal->port, *(uint64_t *) stat_bucket->key, *stat_opcode );
+	}
+
+	iscsi_list_foreach_node ( &conn->stat_scsi_opcodes->list, stat_bucket ) {
+		uint64_t *stat_opcode = (uint64_t *) stat_bucket->value;
+
+		logadd( LOG_INFO, "iSCSI SCSI CDB opcode usage statistics for device %s from initiator %s using port %s and portal %s:%s: SCSI CDB opcode 0x%02" PRIX64 " has been received %" PRIu64 " times until connection drop.", (conn->device != NULL ? conn->device->name : "(null)"), conn->init_name, ((conn->init_port != NULL) ? conn->init_port->name : "(null)"), portal->host, portal->port, *(uint64_t *) stat_bucket->key, *stat_opcode );
+	}
+
+	iscsi_connection_destroy( conn );
+
+	iscsi_portal_group_del_portal( portal_group, portal );
+	iscsi_portal_destroy( portal );
+
+	pthread_rwlock_unlock( &iscsi_globvec_rwlock );
+
 }
diff --git a/src/server/iscsi.h b/src/server/iscsi.h
index dac8e8f..6f48e80 100644
--- a/src/server/iscsi.h
+++ b/src/server/iscsi.h
@@ -36,9 +36,9 @@
 extern "C" {
 #endif
 
-#include <inttypes.h>
 #include <limits.h>
 #include <stdbool.h>
+#include <stdint.h>
 #include <stdio.h>
 #include <sys/types.h>
 #include <dnbd3/types.h>
@@ -47,6 +47,26 @@ extern "C" {
 #include "globals.h"
 #include "image.h"
 
+#if defined(__GNUC__) && (defined(__x86_64__) || defined(__i386__))
+     // GCC-compatible compiler, targeting x86/x86-64
+     #include <x86intrin.h>
+#elif defined(__GNUC__) && defined(__ARM_NEON__)
+     // GCC-compatible compiler, targeting ARM with NEON
+     #include <arm_neon.h>
+#elif defined(__GNUC__) && defined(__IWMMXT__)
+     // GCC-compatible compiler, targeting ARM with WMMX
+     #include <mmintrin.h>
+#elif (defined(__GNUC__) || defined(__xlC__)) && (defined(__VEC__) || defined(__ALTIVEC__))
+     // XLC or GCC-compatible compiler, targeting PowerPC with VMX/VSX
+     #include <altivec.h>
+#elif defined(__GNUC__) && defined(__SPE__)
+     // GCC-compatible compiler, targeting PowerPC with SPE
+     #include <spe.h>
+#elif defined(_MSC_VER)
+     // Microsoft C/C++-compatible compiler
+     #include <intrin.h>
+#endif
+
 #if defined(__BIG_ENDIAN__) || (defined(__BYTE_ORDER) && defined(__BIG_ENDIAN) && __BYTE_ORDER == __BIG_ENDIAN) || (defined(__BYTE_ORDER__) && defined(__ORDER_BIG_ENDIAN__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
 #define iscsi_get_be16(x) (x)
 #define iscsi_get_be24(x) (iscsi_get_be32((*(uint32_t *) ((uint8_t *) x - 1))) & 0xFFFFFFUL)
@@ -74,27 +94,55 @@ static inline void iscsi_put_be64(uint8_t *data, const uint64_t value)
 {
 	(*(uint64_t *) data) = value;
 }
-#elif defined(__LITTLE_ENDIAN__) || (defined(__BYTE_ORDER) && defined(__LITTLE_ENDIAN) && __BYTE_ORDER == __LITTLE_ENDIAN) || (defined(__BYTE_ORDER__) && defined(__ORDER_LITTLE_ENDIAN__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) || defined(__i386__) || defined(__i386) || defined(__x86_64)
-#if defined(__GNUC__) && (defined(__x86_64__) || defined(__i386__))
-     // GCC-compatible compiler, targeting x86/x86-64
-     #include <x86intrin.h>
-#elif defined(__GNUC__) && defined(__ARM_NEON__)
-     // GCC-compatible compiler, targeting ARM with NEON
-     #include <arm_neon.h>
-#elif defined(__GNUC__) && defined(__IWMMXT__)
-     // GCC-compatible compiler, targeting ARM with WMMX
-     #include <mmintrin.h>
-#elif (defined(__GNUC__) || defined(__xlC__)) && (defined(__VEC__) || defined(__ALTIVEC__))
-     // XLC or GCC-compatible compiler, targeting PowerPC with VMX/VSX
-     #include <altivec.h>
-#elif defined(__GNUC__) && defined(__SPE__)
-     // GCC-compatible compiler, targeting PowerPC with SPE
-     #include <spe.h>
+
+#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)
+// GCC or CLang
+#define iscsi_get_le16(x) (__builtin_bswap16(x))
+#define iscsi_get_le24(x) (iscsi_get_le32((*(uint32_t *) ((uint8_t *) x - 1))) & 0xFFFFFFUL)
+#define iscsi_get_le32(x) (__builtin_bswap32(x))
+#define iscsi_get_le64(x) (__builtin_bswap64(x))
 #elif defined(_MSC_VER)
-     // Microsoft C/C++-compatible compiler
-     #include <intrin.h>
+// MSVC
+#define iscsi_get_le16(x) (_byteswap_ushort(x))
+#define iscsi_get_le24(x) (iscsi_get_le32((*(uint32_t *) ((uint8_t *) x - 1))) & 0xFFFFFFUL)
+#define iscsi_get_le32(x) (_byteswap_ulong(x))
+#define iscsi_get_le64(x) (_byteswap_uint64(x))
+#elif defined(__INTEL_COMPILER) || defined(__ECC)
+// Intel Compiler
+#define iscsi_get_le16(x) (_bswap16(x))
+#define iscsi_get_le24(x) (iscsi_get_le32((*(uint32_t *) ((uint8_t *) x - 1))) & 0xFFFFFFUL)
+#define iscsi_get_le32(x) (_bswap(x))
+#define iscsi_get_le64(x) (_bswap64(x))
+#else
+// Other compilers (use slow conversion method with bit rotation, bit shift and logcal AND)
+#define iscsi_get_le16(x) ((((uint16_t) (x)) << 8U) | (((uint16_t) (x)) >> 8U))
+#define iscsi_get_le24(x) (iscsi_get_le32((*(uint32_t *) ((uint8_t *) x - 1))) & 0xFFFFFFUL)
+#define iscsi_get_le32(x) ((((uint32_t) (x) & 0xFFUL) << 24UL) | (((uint32_t) (x) & 0xFF00UL) << 8UL) | (((uint32_t) (x) & 0xFF0000UL) >> 8UL) | (((uint32_t) (x) >> 24UL)))
+#define iscsi_get_le64(x) ((uint64_t)((((x) & 0xFFULL) << 56ULL) | (((x) & 0xFF00ULL) << 40ULL) | (((x) & 0xFF0000ull) << 24ULL) | (((x) & 0xFF000000ULL) << 8ULL) | (((x) & 0xFF00000000ULL) >> 8ULL) | (((x) & 0xFF0000000000ULL) >> 24ULL) | (((x) & 0xFF000000000000ULL) >> 40ULL) | (((x) & 0xFF00000000000000ULL) >> 56ULL)))
 #endif
 
+static inline void iscsi_put_le16(uint8_t *data, const uint16_t value)
+{
+	(*(uint16_t *) data) = iscsi_get_le16(value);
+}
+
+static inline void iscsi_put_le24(uint8_t *data, const uint32_t value)
+{
+	data--;
+
+	(*(uint32_t *) data) = ((uint32_t ) *data | (iscsi_get_le32(value) & 0xFFFFFF00UL));
+}
+
+static inline void iscsi_put_le32(uint8_t *data, const uint32_t value)
+{
+	(*(uint32_t *) data) = iscsi_get_le32(value);
+}
+
+static inline void iscsi_put_le64(uint8_t *data, const uint64_t value)
+{
+	(*(uint64_t *) data) = iscsi_get_le64(value);
+}
+#elif defined(__LITTLE_ENDIAN__) || (defined(__BYTE_ORDER) && defined(__LITTLE_ENDIAN) && __BYTE_ORDER == __LITTLE_ENDIAN) || (defined(__BYTE_ORDER__) && defined(__ORDER_LITTLE_ENDIAN__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) || defined(__i386__) || defined(__i386) || defined(__x86_64)
 #if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)
 // GCC or CLang
 #define iscsi_get_be16(x) (__builtin_bswap16(x))
@@ -142,6 +190,33 @@ static inline void iscsi_put_be64(uint8_t *data, const uint64_t value)
 {
 	(*(uint64_t *) data) = iscsi_get_be64(value);
 }
+
+#define iscsi_get_le16(x) (x)
+#define iscsi_get_le24(x) (iscsi_get_le32((*(uint32_t *) ((uint8_t *) x - 1))) & 0xFFFFFFUL)
+#define iscsi_get_le32(x) (x)
+#define iscsi_get_le64(x) (x)
+
+static inline void iscsi_put_le16(uint8_t *data, const uint16_t value)
+{
+	(*(uint16_t *) data) = value;
+}
+
+static inline void iscsi_put_le24(uint8_t *data, const uint32_t value)
+{
+	data--;
+
+	(*(uint32_t *) data) = (((uint32_t ) *data << 24UL) | (value & 0xFFFFFFUL));
+}
+
+static inline void iscsi_put_le32(uint8_t *data, const uint32_t value)
+{
+	(*(uint32_t *) data) = value;
+}
+
+static inline void iscsi_put_le64(uint8_t *data, const uint64_t value)
+{
+	(*(uint64_t *) data) = value;
+}
 #else
 #error "Unknown CPU endianness"
 #endif
@@ -257,7 +332,12 @@ static inline uint32_t iscsi_get_log2_of_pow2(const uint32_t value)
 
 
 /// Aligns value x by rounding up, so it's evenly divisable by n.
-#define iscsi_align(x, n) (((x) + (n) - 1) & ~((n) - 1))
+#define ISCSI_ALIGN(x, n) (((x) + (n) - 1) & ~((n) - 1))
+
+
+/// Determines the length of a zero terminated string at compile time.
+#define ISCSI_STRLEN(x) ((sizeof(x) / sizeof(uint8_t)) - 1)
+
 
 uint8_t *iscsi_vsprintf_append_realloc(char *buf, const char *format, va_list args); // Allocates and appends a buffer and sprintf's it
 uint8_t *iscsi_sprintf_append_realloc(char *buf, const char *format, ...); // Allocates and appends a buffer and sprintf's it
@@ -343,15 +423,246 @@ typedef struct iscsi_list {
 #define iscsi_list_foreach_safe_node(list, entry, tmp) iscsi_list_foreach_safe_field(list, entry, node, tmp)
 
 
-void iscsi_list_create(iscsi_list *list); // Initializes a doubly linked list for usage
-void iscsi_list_push(iscsi_list *list, iscsi_node *node); // Adds a node at the head of a doubly linked list
-void iscsi_list_enqueue(iscsi_list *list, iscsi_node *node); // Adds a node at the tail of a doubly linked list
-void iscsi_list_insert(iscsi_list *list, iscsi_node *node, iscsi_node *pred); // Inserts a node into a doubly linked list before an already existing node
-void iscsi_list_pop(iscsi_list *list); // Removes the node from the head of a doubly linked list
-void iscsi_list_dequeue(iscsi_list *list); // Removes the node from the tail of a doubly linked list
-void iscsi_list_remove(iscsi_node *node); // Removes a specified node from a doubly linked list
-bool iscsi_list_empty(const iscsi_list *list); // Checks whether a doubly linked list is empty
-iscsi_node *iscsi_list_peek(const iscsi_list *list); // Gets the node from the head of a doubly linked list
+/**
+ * @brief Initializes a doubly linked list for usage.
+ *
+ * This function sets the head of the list to
+ * the pointer of the list's tail, the tail
+ * itself to NULL and the predecessor to the
+ * pointer of the list's head.
+ *
+ * @param[in] list Pointer to idoubly linked list to
+ * initialize. May NOT be NULL, so be careful.
+ * */
+static inline void iscsi_list_create(iscsi_list *list)
+{
+	list->head = (iscsi_node *) &list->tail;
+	list->tail = NULL;
+	list->pred = (iscsi_node *) &list->head;
+}
+
+/**
+ * @brief Clears an already initialized doubly linked list.
+ *
+ * This function sets the head of the list to
+ * the pointer of the list's tail and the
+ * predecessor to the pointer of the list's
+ * head.
+ *
+ * @param[in] list Pointer to idoubly linked list to
+ * initialize. May NOT be NULL, so be careful.
+ * */
+static inline void iscsi_list_clear(iscsi_list *list)
+{
+	list->head = (iscsi_node *) &list->tail;
+	list->pred = (iscsi_node *) &list->head;
+}
+
+/**
+ * @brief Adds a node at the head of a doubly linked list.
+ *
+ * This function sets the head of the list to
+ * the node and adjusts the list and node
+ * pointers accordingly.
+ *
+ * @param[in] list Pointer to doubly linked list to add to
+ * the head. May NOT be NULL, so be careful.
+ * @param[in] node Pointer to node to add to the head of
+ * the list. NULL is NOT allowed here, take
+ * caution.
+ */
+static inline void iscsi_list_push(iscsi_list *list, iscsi_node *node)
+{
+	iscsi_node *head = list->head;
+
+	list->head = node;
+	head->pred = node;
+
+	node->succ = head;
+	node->pred = (iscsi_node *) &list->head;
+}
+
+/**
+ * @brief Adds a node at the tail of a doubly linked list.
+ *
+ * This function sets the tail of the list to
+ * the node and adjusts the list and node
+ * pointers accordingly.
+ *
+ * @param[in] list Pointer to doubly linked list to add to
+ * the tail. May NOT be NULL, so be careful.
+ * @param[in] node Pointer to node to add to the tail of
+ * the list. NULL is NOT allowed here, take
+ * caution.
+ */
+static inline void iscsi_list_enqueue(iscsi_list *list, iscsi_node *node)
+{
+	iscsi_node *tail = list->pred;
+
+	list->pred = node;
+	tail->succ = node;
+
+	node->succ = (iscsi_node *) &list->tail;
+	node->pred = tail;
+}
+
+/**
+ * @brief Inserts a node into a doubly linked list before an already existing node.
+ *
+ * This function sets the successor of the
+ * new node to the successor of the
+ * existing predecessor node and the
+ * predecessor of the new node to the
+ * the existing predecessor node itself
+ * and adjusts the list pointers
+ * accordingly.
+ *
+ * @param[in] list Pointer to doubly linked list to insert the
+ * node into. May NOT be NULL, so be careful.
+ * @param[in] node Pointer to node to be inserted into the
+ * list. NULL is NOT allowed here, take
+ * caution.
+ * @param[in] pred Pointer to node which should be the
+ * previous node of the new inserted node.
+ * May be NULL in which case the new node
+ * is inserted at the head of the list.
+ */
+static inline void iscsi_list_insert(iscsi_list *list, iscsi_node *node, iscsi_node *pred)
+{
+	if ( pred == NULL ) {
+		iscsi_node *head = list->head;
+
+		list->head = node;
+		head->pred = node;
+
+		node->succ = head;
+		node->pred = (iscsi_node *) &list->head;
+
+		return;
+	}
+
+	iscsi_node *tail = pred->succ;
+
+	if ( tail == NULL ) {
+		tail       = pred->pred;
+
+		node->succ = pred;
+		node->pred = tail;
+
+		pred->pred = node;
+		tail->succ = node;
+
+		return;
+	}
+
+	node->succ = tail;
+	node->pred = pred;
+
+	tail->pred = node;
+	pred->succ = node;
+}
+
+/**
+ * @brief Removes the node from the head of a doubly linked list.
+ *
+ * This function sets the head of the list to
+ * its successor and adjusts the list and
+ * node pointers accordingly.
+ *
+ * @param[in] list Pointer to doubly linked list to remove the
+ * head from. May NOT be NULL, so be careful.
+ */
+static inline void iscsi_list_pop(iscsi_list *list)
+{
+	iscsi_node *head = list->head;
+	iscsi_node *node = head->succ;
+
+	if ( node == NULL )
+		return;
+
+	list->head = node;
+
+	node->pred = (iscsi_node *) &list->head;
+}
+
+/**
+ * @brief Removes the node from the tail of a doubly linked list.
+ *
+ * This function sets the tail of the list to
+ * its predecessor and adjusts the list and
+ * node pointers accordingly.
+ *
+ * @param[in] list Pointer to doubly linked list to remove the
+ * tail from. May NOT be NULL, so be careful.
+ */
+static inline void iscsi_list_dequeue(iscsi_list *list)
+{
+	iscsi_node *tail = list->pred;
+	iscsi_node *node = tail->pred;
+
+	if ( node == NULL )
+		return;
+
+	list->pred = node;
+
+	node->succ = (iscsi_node *) &list->tail;
+}
+
+/**
+ * @brief Removes a specified node from a doubly linked list.
+ *
+ * This function sets the successor of the
+ * node's predecessor and the predecessor
+ * of the node's successor by adjusting
+ * the list and node pointers accordingly.
+ *
+ * @param[in] node Pointer to node to be removed from
+ * the list. May NOT be NULL, so
+ * be careful.
+ */
+static inline void iscsi_list_remove(iscsi_node *node)
+{
+	iscsi_node *succ = node->succ;
+	iscsi_node *pred = node->pred;
+
+	pred->succ = succ;
+	succ->pred = pred;
+}
+
+/**
+ * @brief Checks whether a doubly linked list is empty.
+ *
+ * Whenever this function returns false,
+ * iscsi_list_peek will return a pointer
+ * to the first node in the list.
+ *
+ * @param[in] list Pointer to doubly linked list to check if
+ * empty. May NOT be NULL, so be careful.
+ * @retval true The doubly linked list is empty.
+ * @retval false The doubly linked list contains nodes.
+ */
+static inline bool iscsi_list_empty(const iscsi_list *list)
+{
+	return (list->head->succ == NULL);
+}
+
+/**
+ * @brief Gets the node from the head of a doubly linked list.
+ *
+ * This function returns NULL if the list is
+ * empty.
+ *
+ * @param[in] list Pointer to doubly linked list to get the
+ * head from. May NOT be NULL, so be careful.
+ * @return Pointer to doubly linked list node of the
+ * head or NULL if the list is empty.
+ */
+static inline iscsi_node *iscsi_list_peek(const iscsi_list *list)
+{
+	iscsi_node *head = list->head;
+
+	return (head->succ != NULL) ? head : NULL;
+}
 
 
 /**
@@ -362,7 +673,7 @@ iscsi_node *iscsi_list_peek(const iscsi_list *list); // Gets the node from the h
  */
 typedef struct iscsi_hashmap_bucket {
     /// Next bucket, must be first element.
-	struct iscsi_hashmap_bucket *next;
+	iscsi_node node;
 
     /// Data used as key, must be aligned to 8 bytes and zero padded.
 	uint8_t *key;
@@ -382,33 +693,26 @@ typedef struct iscsi_hashmap_bucket {
  *
  * This structure is used by the ultra performant hash map
  * implementation. It uses a linked list allowing fast
- * insertions. Elements can be removed and are marked for
- * deletion until a resize operation is necessary.
+ * insertions. Elements can be removed.
  */
 typedef struct iscsi_hashmap {
     /// Linked list containing the hash map buckets.
 	iscsi_hashmap_bucket *buckets;
 
+    /// Doubly linked list for fast insertion.
+    iscsi_list list;
+
+    /// Last inserted unique identifier (primary key).
+	uint64_t last_insert_id;
+
     /// Current bucket capacity, MUST be a power of two.
     uint capacity;
 
     /// Current capacity threshold triggering resize operation.
-	uint cap_load; // Capacity load threshold before next resize
+	uint cap_load;
 
-	/// Current count of buckets including ones marked for removal.
+	/// Current count of buckets.
 	uint count;
-
-    /// Number of buckets marked for removal.
-    uint removed_count;
-
-    /// First linked list bucket for fast insertion.
-    iscsi_hashmap_bucket *first;
-
-    /// Last linked list bucket for faster traversion.
-	iscsi_hashmap_bucket *last;
-
-    /// Last inserted unique identifier (primary key).
-	uint64_t last_insert_id;
 } iscsi_hashmap;
 
 /**
@@ -447,19 +751,18 @@ int iscsi_hashmap_key_destroy_callback(uint8_t *key, const size_t key_size, uint
 int iscsi_hashmap_destroy_value_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data); // Deallocates a value in a hash map
 int iscsi_hashmap_key_destroy_value_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data); // Deallocates a key / value pair in a hash map by calling free (default destructor)
 
-int iscsi_hashmap_put(iscsi_hashmap *map, uint8_t *key, const size_t key_size, uint8_t *value); // Assigns key / value pair to hash map at the tail of linked list without making copies
-int iscsi_hashmap_get_put(iscsi_hashmap *map, uint8_t *key, const size_t key_size, uint8_t **out_in_value); // Assigns key / value pair to hash map without making copies
-int iscsi_hashmap_put_free(iscsi_hashmap *map, uint8_t *key, const size_t key_size, uint8_t *value, iscsi_hashmap_callback callback, uint8_t *user_data); // Assigns key / value pair to hash map without making copies
-                                                                                                                                                                // with callback function in case the key already exists
+int iscsi_hashmap_put(iscsi_hashmap *map, uint8_t *key, const size_t key_size, uint8_t *value); // Assigns key / value pair to hash map at the tail of doubly linked list without making copies
+int iscsi_hashmap_get_put(iscsi_hashmap *map, uint8_t *key, const size_t key_size, uint8_t **out_in_value); // Assigns key / value pair to hash map at the tail of doubly linked list without making copies
+int iscsi_hashmap_put_free(iscsi_hashmap *map, uint8_t *key, const size_t key_size, uint8_t *value, iscsi_hashmap_callback callback, uint8_t *user_data); // Assigns key / value pair to hash map without making copies with callback function in case the key already exists
 bool iscsi_hashmap_contains(iscsi_hashmap *map, const uint8_t *key, const size_t key_size); // Checks whether a specified key exists
 int iscsi_hashmap_get(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, uint8_t **out_value); // Retrieves the value of a specified key
 
-void iscsi_hashmap_remove(iscsi_hashmap *map, const uint8_t *key, const size_t key_size); // Marks an element for removal by setting key and value both to NULL
-void iscsi_hashmap_remove_free(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, iscsi_hashmap_callback callback, uint8_t *user_data); // Marks an element for removal by setting key and value both to NULL,
-                                                                                                                                                    // but invokes a callback function before actual marking for removal.
-uint iscsi_hashmap_size(const iscsi_hashmap *map); // Retrieves the number of elements of the hash map, ignoring elements marked for removal
+void iscsi_hashmap_remove(iscsi_hashmap *map, const uint8_t *key, const size_t key_size); // Removes an element both from the doubly linked list and by setting the key to NULL
+void iscsi_hashmap_remove_free(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, iscsi_hashmap_callback callback, uint8_t *user_data); // Removes an element both from the doubly linked list and by setting the key to NULL and invokes a callback function before actual removal
+
+uint iscsi_hashmap_size(const iscsi_hashmap *map); // Retrieves the number of elements of the hash map
 
-int iscsi_hashmap_iterate(iscsi_hashmap *map, iscsi_hashmap_callback callback, uint8_t *user_data); // Iterator with callback function invoked on each element which has not been removed
+int iscsi_hashmap_iterate(iscsi_hashmap *map, iscsi_hashmap_callback callback, uint8_t *user_data); // Iterator with callback function invoked on each element
 
 /* iSCSI protocol stuff (all WORD/DWORD/QWORD values are big endian by default
    unless specified otherwise). */
@@ -4878,6 +5181,7 @@ typedef struct __attribute__((packed)) iscsi_scsi_cmd_packet {
  */
 #define ISCSI_SCSI_RESPONSE_STATUS_TASK_ABORTED  0x40
 
+
 /// SCSI response code: Command Completed at Target.
 #define ISCSI_SCSI_RESPONSE_CODE_OK              0x00
 
@@ -9178,7 +9482,7 @@ typedef struct __attribute__((packed)) iscsi_reject_packet {
     uint32_t data_r2t_sn;
 
     /// Reserved for future usage, always MUST be 0.
-    uint32_t reserved4[2];
+    uint64_t reserved4;
 
     /// Optional header digest.
     iscsi_header_digest hdr_digest;
@@ -9369,7 +9673,10 @@ typedef struct __attribute__((packed)) iscsi_nop_in_packet {
     uint32_t max_cmd_sn;
 
     /// Reserved for future usage, always MUST be 0.
-    uint32_t reserved2[3];
+    uint32_t reserved2;
+
+    /// Reserved for future usage, always MUST be 0.
+    uint64_t reserved3;
 
     /// Optional header digest.
     iscsi_header_digest hdr_digest;
@@ -9440,54 +9747,8 @@ typedef struct __attribute__((packed)) iscsi_transport_id {
 } iscsi_transport_id;
 
 
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation successful -> iSCSI packet recognized and compliance to protocol specification.
-#define ISCSI_VALIDATE_PACKET_RESULT_OK                         0
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> No packet data specified.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_NO_DATA             -1
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> Packet size smaller than smallest possible iSCSI packet.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_SIZE_TOO_SMALL      -2
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> Packet size doesn't match calculated lengths from BHS.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_SIZE_MISMATCH       -3
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> iSCSI protocol version not supported yet.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_UNSUPPORTED_VERSION -4
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> Valid opcode but violates iSCSI protocol specification.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_PROTOCOL_SPECS      -5
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> Invalid opcode according to iSCSI protocol specification.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_INVALID_OPCODE      -6
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> CRC32C check failed for header (BHS and/or AHS).
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_CRC32C_HDR_DIGEST   -7
-
-/// iSCSI packet validation return code from iscsi_validate_packet function: Validation failed -> CRC32C check failed for data segment.
-#define ISCSI_VALIDATE_PACKET_RESULT_ERROR_CRC32C_DATA_DIGEST  -8
-
-
-iscsi_bhs_packet *iscsi_create_packet(); // Allocate and initialize an iSCSI BHS packet
-void iscsi_destroy_packet(iscsi_bhs_packet *packet_data); // Free resources allocated by iscsi_create_packet
-
-iscsi_bhs_packet *iscsi_append_ahs_packet(iscsi_bhs_packet *packet_data, const uint32_t ahs_len); // Allocate and initialize an iSCSI AHS packet and append to existing data stream
-int iscsi_get_ahs_packets(const iscsi_bhs_packet *packet_data); // Counts number of AHS packets in an iSCSI data packet stream
-iscsi_ahs_packet *iscsi_get_ahs_packet(const iscsi_bhs_packet *packet_data, const int index); // Retrieves the pointer to an specific AHS packet by index
-
-iscsi_bhs_packet *iscsi_append_header_digest_packet(iscsi_bhs_packet *packet_data, const int header_digest_size); // Allocate and initialize an iSCSI header digest (CRC32C) and appends it to existing data stream
-
-iscsi_bhs_packet *iscsi_append_ds_packet(iscsi_bhs_packet *packet_data, const int header_digest_size, const uint32_t ds_len, const int data_digest_size); // Allocate and initialize an iSCSI DS packet and append to existing data stream
-
-void iscsi_calc_header_digest(const iscsi_bhs_packet *packet_data); // Calculate and store iSCSI header digest (CRC32C)
-int iscsi_validate_header_digest(const iscsi_bhs_packet *packet_data); // Validates a stored iSCSI header digest (CRC32C) with actual header data
-
-void iscsi_calc_data_digest(const iscsi_bhs_packet *packet_data, const int header_digest_size); // Calculate iSCSI data digest (CRC32C)
-int iscsi_validate_data_digest(const iscsi_bhs_packet *packet_data, const int header_digest_size); // Validates a stored iSCSI data digest (CRC32C) with actual DataSegment
-
-
 /// Maximum length of a key according to iSCSI specifications.
-#define ISCSI_TEXT_KEY_MAX_LEN 63U
+#define ISCSI_TEXT_KEY_MAX_LEN          63U
 
 /// Maximum length of value for a simple key type.
 #define ISCSI_TEXT_VALUE_MAX_SIMPLE_LEN 255U
@@ -9496,10 +9757,10 @@ int iscsi_validate_data_digest(const iscsi_bhs_packet *packet_data, const int he
 #define ISCSI_TEXT_VALUE_MAX_LEN        8192U
 
 /// Value data shift value for key value alignment enforcement.
-#define ISCSI_TEXT_VALUE_ALIGN_SHIFT 4UL
+#define ISCSI_TEXT_VALUE_ALIGN_SHIFT    4UL
 
 /// Value alignment size is a multiple of 16 bytes for a key value for having work space when changing string representation of integer values.
-#define ISCSI_TEXT_VALUE_ALIGN (1UL << (ISCSI_TEXT_VALUE_ALIGN_SHIFT))
+#define ISCSI_TEXT_VALUE_ALIGN          (1UL << (ISCSI_TEXT_VALUE_ALIGN_SHIFT))
 
 
 /// iSCSI text key=value pair type: Invalid.
@@ -9694,7 +9955,6 @@ extern pthread_rwlock_t iscsi_globvec_rwlock;
 
 
 int iscsi_create(); // Allocates and initializes the iSCSI global vector structure
-int iscsi_global_key_value_pair_destroy_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data); // iSCSI global key and value pair destructor callback for hash map
 void iscsi_destroy(); // Deallocates all resources acquired by iscsi_create
 
 /**
@@ -9768,6 +10028,7 @@ typedef struct iscsi_portal {
 iscsi_portal_group *iscsi_portal_group_create(const uint64_t tag, const int flags); // Creates and initializes an iSCSI portal group
 void iscsi_portal_group_destroy(iscsi_portal_group *portal_group); // Deallocates resources acquired by iscsi_portal_group_create
 int iscsi_portal_group_add_portal(iscsi_portal_group *portal_group, iscsi_portal *portal); // Adds an iSCSI portal to the iSCSI portal group hash map
+void iscsi_portal_group_del_portal(iscsi_portal_group *portal_group, iscsi_portal *portal); // Removes an iSCSI portal from the iSCSI portal group hash map
 
 iscsi_portal *iscsi_portal_create(const uint8_t *host, const uint8_t *port); // Allocates and initializes an iSCSI portal structure
 void iscsi_portal_destroy(iscsi_portal *portal);
@@ -10261,11 +10522,11 @@ void iscsi_scsi_lun_destroy(iscsi_scsi_lun *lun); // Deallocates all resources a
 uint64_t iscsi_scsi_lun_get_from_scsi(const int lun_id); // Converts an internal representation of a LUN identifier to an iSCSI LUN required for packet data
 int iscsi_scsi_lun_get_from_iscsi(const uint64_t lun); // Converts an iSCSI LUN from packet data to internal SCSI LUN identifier
 
-void iscsi_scsi_lun_task_append(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task); // Appends an iSCSI SCSI task to a iSCSI SCSI LUN pending tasks hash map
+void iscsi_scsi_lun_task_append(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task); // Appends an iSCSI SCSI task to a iSCSI SCSI LUN pending tasks doubly linked list
 void iscsi_scsi_lun_tasks_exec(iscsi_scsi_lun *lun); // Executes all iSCSI SCSI pending tasks assigned to a iSCSI SCSI LUN
 void iscsi_scsi_lun_task_run(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task); // Runs an iSCSI SCSI task for a specified iSCSI SCSI LUN
 void iscsi_scsi_lun_task_complete(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task); // Handles iSCSI SCSI task completition
-void iscsi_scsi_lun_task_exec(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task); // Appends iSCSI SCSI task to pending tasks hash map and / or runs it directly
+void iscsi_scsi_lun_task_exec(iscsi_scsi_lun *lun, iscsi_scsi_task *scsi_task); // Appends iSCSI SCSI task to pending tasks doubly linked list and / or runs it directly
 
 int iscsi_scsi_pr_check_scsi2(iscsi_scsi_task *scsi_task); // Checks the iSCSI SCSI Persistent Reservation (PR) SCSI-2 reserve of an iSCSI SCSI task
 int iscsi_scsi_pr_registrant_get_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data); // Finds an iSCSI SCSI Persistent Reservation (PR) registrant by target and initiator port
@@ -10383,32 +10644,6 @@ typedef struct iscsi_scsi_lun {
 } iscsi_scsi_lun;
 
 
-typedef struct iscsi_pdu iscsi_pdu;
-typedef struct iscsi_task iscsi_task;
-
-
-/**
- * @brief iSCSI PDU search and removal by Ready To Transfer Sequence Number (R2TSN).
- *
- * This structure is used by iterating through
- * all SNACK PDUs finding by Ready To Transfer
- * Sequence Number (R2TSN).
- */
-typedef struct iscsi_r2t_remove_pdu {
-    /// Found iSCSI PDU is stored here, should be initialized to NULL.
-    iscsi_pdu *pdu;
-
-    /// iSCSI task containing the Ready To Transfer Sequence Number (R2TSN).
-    iscsi_task *task;
-
-    /// Hash map containing SNACK PDU's associated with this removal task.
-    iscsi_hashmap *pdu_snack;
-
-    /// The Ready To Transfer Sequence Number (R2TSN) to be removed.
-    uint32_t r2t_sn;
-} iscsi_r2t_remove_pdu;
-
-
 /// iSCSI device flags: Allocated.
 #define ISCSI_DEVICE_FLAGS_ALLOCATED (1 << 0)
 
@@ -10459,6 +10694,9 @@ typedef struct iscsi_device_find_lun_id {
 } iscsi_device_find_lun_id;
 
 
+/// iSCSI target node WWN identifier prefix string.
+#define ISCSI_TARGET_NODE_WWN_NAME_PREFIX "wwn-0x"
+
 /// iSCSI target node maximum length
 #define ISCSI_TARGET_NODE_MAX_NAME_LEN 223U
 
@@ -10671,6 +10909,9 @@ typedef struct iscsi_session {
 } iscsi_session;
 
 
+typedef struct iscsi_pdu iscsi_pdu;
+
+
 /// iSCSI connection read packet data return code from iscsi_connection_pdu_read function: Packet parsed successfully.
 #define ISCSI_CONNECT_PDU_READ_OK                                 0
 
@@ -10808,8 +11049,11 @@ typedef struct iscsi_connection {
     /// Doubly linked list containing enqueued SCSI Data In tasks.
     iscsi_list scsi_data_in_queued_tasks;
 
-    /// Hash map containing SNACK PDU's associated with this connection.
-    iscsi_hashmap *pdu_snack;
+    /// Doubly linked list containing writing PDU's associated with this connection.
+    iscsi_list pdus_write;
+
+    /// Doubly linked list containing SNACK PDU's associated with this connection.
+    iscsi_list pdus_snack;
 
     /// Doubly linked list containing active Ready To Transfer (R2T) tasks.
     iscsi_list r2t_tasks_active;
@@ -10894,12 +11138,21 @@ typedef struct iscsi_connection {
 
     /// ExpStatSN.
     uint32_t exp_stat_sn;
+
+    // TODO: Remove after test finish
+    iscsi_hashmap *stat_iscsi_opcodes;
+
+    // TODO: Remove after test finish
+    iscsi_hashmap *stat_scsi_opcodes;
 } iscsi_connection;
 
 
 typedef void (*iscsi_connection_xfer_complete_callback)(uint8_t *user_data); // iSCSI transfer completed callback function.
 
 
+typedef struct iscsi_task iscsi_task;
+
+
 /// iSCSI PDU flags: Rejected.
 #define ISCSI_PDU_FLAGS_REJECTED (1 << 0)
 
@@ -10912,6 +11165,9 @@ typedef void (*iscsi_connection_xfer_complete_callback)(uint8_t *user_data); //
  * and filling the BHS, AHS and DS properly.
  */
 typedef struct iscsi_pdu {
+    /// Doubly linked list node.
+    iscsi_node node;
+
     /// iSCSI Basic Header Segment (BHS) packet data.
     iscsi_bhs_packet *bhs_pkt;
 
@@ -10983,6 +11239,19 @@ typedef struct iscsi_pdu {
 } iscsi_pdu;
 
 
+/**
+ * @brief Callback for iSCSI connection write TCP/IP write operation completion.
+ *
+ * This function is invoked when the sending
+ * TCP/IP transfer has been finished.
+ *
+ * @param[in] user_data Pointer to user data.
+ * @param[in] err 0 if I/O completed successfully or an
+ * error code indicating the problem.
+ */
+typedef void (*iscsi_connection_write_complete_callback)(uint8_t *user_data, int err);
+
+
 /// iSCSI task flags: Ready To Transfer is active.
 #define ISCSI_TASK_FLAGS_R2T_ACTIVE (1 << 0)
 
@@ -11075,7 +11344,7 @@ void iscsi_task_destroy(iscsi_task *task); // Deallocates resources acquired by
 void iscsi_task_queue(iscsi_connection *conn, iscsi_task *task); // Enqueues an iSCSI task
 
 void iscsi_task_xfer_complete_process_read(iscsi_connection *conn, iscsi_task *task, iscsi_task *primary_task); // Processes an iSCSI SCSI task which completed a read data transfer
-bool iscsi_task_xfer_del(iscsi_connection *conn, const uint32_t target_xfer_tag); // Deletes an iSCSI task from the active Ready To Transfer (R2T) hash map by Target Transfer Tag (TTT)
+bool iscsi_task_xfer_del(iscsi_connection *conn, const uint32_t target_xfer_tag); // Deletes an iSCSI task from the active Ready To Transfer (R2T) doubly linked list by Target Transfer Tag (TTT)
 void iscsi_task_xfer_complete_process_other(iscsi_connection *conn, iscsi_task *task, iscsi_task *primary_task); // Processes an iSCSI SCSI task which completed a non-read data transfer
 
 void iscsi_task_response(iscsi_connection *conn, iscsi_task *task); // Creates, initializes and sends an iSCSI task reponse PDU.
@@ -11095,7 +11364,7 @@ int iscsi_target_node_create_callback(uint8_t *key, const size_t key_size, uint8
 iscsi_target_node *iscsi_target_node_create(uint8_t *name, const uint8_t *alias, const int index, const uint luns, const uint queue_depth, const int flags, const int32_t chap_group, const int header_digest, const int data_digest); // Creates and initializes an iSCSI target node
 void iscsi_target_node_destroy(iscsi_target_node *target); // Deallocates all resources acquired by iscsi_target_node_create
 
-int iscsi_target_node_send(iscsi_connection *conn, const uint8_t *dst_iqn, const uint8_t *src_iqn, uint8_t *buf, const uint32_t pos, const uint32_t len); // Sends a buffer from a source iSCSI IQN to target iSCSI IQNs
+int32_t iscsi_target_node_send(iscsi_connection *conn, const uint8_t *dst_iqn, const uint8_t *src_iqn, uint8_t *buf, const uint32_t pos, const uint32_t len); // Sends a buffer from a source iSCSI IQN to target iSCSI IQNs
 uint64_t iscsi_target_node_wwn_get(const uint8_t *name); // Calculates the WWN using 64-bit IEEE Extended NAA for a name
 dnbd3_image_t *iscsi_target_node_image_get(uint8_t *iqn); // Extracts the DNBD3 image out of an iSCSI IQN string and opens the DNBD3 image
 int iscsi_target_node_find_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data); // Finds an iSCSI target node by case insensitive name search
@@ -11124,16 +11393,24 @@ int iscsi_connection_init_key_value_pairs(iscsi_hashmap *key_value_pairs); // In
 int32_t iscsi_negotiate_key_value_pairs(iscsi_connection *conn, iscsi_hashmap *key_value_pairs, uint8_t *buf, const uint32_t pos, const uint32_t len); // Negotiates all key and value pairs required for session authentication
 int iscsi_connection_copy_key_value_pairs(iscsi_connection *conn); // Copies retrieved key and value pairs into SCSI connection and session structures
 int iscsi_connection_save_incoming_key_value_pairs(iscsi_connection *conn, iscsi_hashmap *key_value_pairs, iscsi_pdu *login_response_pdu, const iscsi_pdu *pdu); // Saves incoming key / value pairs from the client of a login request PDU
-
-iscsi_pdu *iscsi_connection_pdu_create(iscsi_connection *conn); // Creates an iSCSI PDU structure used by connections
+void iscsi_connection_login_response_reject(iscsi_pdu *login_response_pdu, const iscsi_pdu *pdu); // Initializes a rejecting login response packet
+iscsi_pdu *iscsi_connection_pdu_create(iscsi_connection *conn, const uint ahs_len, const int header_digest_size, const uint32_t ds_len, const int data_sigest_size ); // Creates an iSCSI PDU structure used by connections
 void iscsi_connection_pdu_destroy(iscsi_pdu *pdu); // Destroys an iSCSI PDU structure used by connections
 void iscsi_connection_pdu_free(iscsi_connection *conn, iscsi_pdu *pdu); // Frees an iSCSI PDU structure used by using connection callback function
 
-void iscsi_connection_pdu_ack_remove(iscsi_connection *conn, const uint32_t exp_stat_sn); // Removes an acknowledged PDU from SNACK PDU hash map by ExpStatSN
+iscsi_bhs_packet *iscsi_connection_pdu_append(iscsi_pdu *pdu, const uint ahs_len, const int header_digest_size, const uint32_t ds_len, const int data_digest_size); // Appends packet data to an iSCSI PDU structure used by connections
+iscsi_ahs_packet *iscsi_connection_pdu_ahs_packet_get(const iscsi_pdu *pdu, const int index); // Retrieves the pointer to an specific AHS packet from an iSCSI PDU by index
+int iscsi_connection_pdu_ahs_packet_count(const iscsi_pdu *pdu); // Counts number of AHS packets of an iSCSI PDU
+
+void iscsi_connection_pdu_digest_header_update(iscsi_header_digest *header_digest, const iscsi_bhs_packet *packet_data, const uint ahs_len); // Calculate and store iSCSI header digest (CRC32C)
+bool iscsi_connection_pdu_digest_header_verify(const iscsi_header_digest *header_digest, const iscsi_bhs_packet *packet_data, const uint ahs_len); // Validates a stored iSCSI header digest (CRC32C) with actual header data
+void iscsi_connection_pdu_digest_data_update(iscsi_data_digest *data_digest, const iscsi_scsi_ds_cmd_data *ds_cmd_data, const uint32_t ds_len); // Calculate iSCSI data digest (CRC32C)
+bool iscsi_connection_pdu_digest_data_verify(const iscsi_data_digest *data_digest, const iscsi_scsi_ds_cmd_data *ds_cmd_data, const uint32_t ds_len); // Validates a stored iSCSI data digest (CRC32C) with actual DataSegment
+
+void iscsi_connection_pdu_ack_remove(iscsi_connection *conn, const uint32_t exp_stat_sn); // Removes an acknowledged PDU from SNACK PDU doubly linked list by ExpStatSN
 
-iscsi_pdu *iscsi_r2t_find_pdu_bhs(iscsi_connection *conn, iscsi_pdu *pdu); // Searches an iSCSI PDU by Basic Header Segment (BHS) in the Ready To Transfer (R2T) active and queued task hash map
+iscsi_pdu *iscsi_r2t_find_pdu_bhs(iscsi_connection *conn, iscsi_pdu *pdu); // Searches an iSCSI PDU by Basic Header Segment (BHS) in the Ready To Transfer (R2T) active and queued task doubly linked list
 int iscsi_r2t_send(iscsi_connection *conn, iscsi_task *task, uint32_t *r2t_sn, const uint32_t pos, const uint32_t len, const uint32_t target_xfer_tag); // Sends an iSCSI Ready To Transfer Sequence Number (R2TSN) packet to the initiator
-int iscsi_r2t_remove_pdu_from_snack_list_callback(uint8_t *key, const size_t key_size, uint8_t *value, uint8_t *user_data); // Finds and removes an iSCSI PDU by Ready To Transfer Sequence Number (R2TSN)
 
 int iscsi_connection_read_data(iscsi_connection *conn, int len, void *buf);
 int iscsi_connection_read_iov_data(iscsi_connection *conn, struct iovec *iov, int iov_count);