Fixed some typos and changed header digest CRC32C functions to store the result directly in the iSCSI packet data. Also added CRC32C calculation for data digest with big endian storage functions. Finally, added a ultra performant hash map implementation for the text key=value handling of iSCSI packets.

1 files changed, 638 insertions, 24 deletions

diff --git a/src/server/iscsi.c b/src/server/iscsi.c
index 14dadc6..925e4e8 100644
--- a/src/server/iscsi.c
+++ b/src/server/iscsi.c
@@ -39,10 +39,567 @@
  */
 
 /**
+ * Creates a ultra hardcore speed optimized empty hash map and allocates
+ * enough buckets to hold default capacity elements.
+ * The speed optimizations require all keys having a size of
+ * a multiple of 8 bytes with zero padding.
+ * TODO: Move all hash map related functions to different source file
+ * later and implement in a lock-free way for better concurrency.
+ *
+ * @return A pointer to the hash map structure or NULL in case of an error.
+ */
+iscsi_hashmap *iscsi_hashmap_create()
+{
+	iscsi_hashmap *map = (iscsi_hashmap *) malloc( sizeof(iscsi_hashmap) );
+
+	if ( map == NULL ) {
+		logadd( LOG_ERROR, "iscsi_hashmap_create: Out of memory while allocating iSCSI hash map" );
+
+		return map;
+	}
+
+	map->capacity = (1UL << ISCSI_HASHMAP_DEFAULT_CAPACITY);
+
+	map->buckets = (iscsi_hashmap_bucket *) calloc( map->capacity, sizeof(struct iscsi_hashmap_bucket) );
+
+	if ( map->buckets == NULL ) {
+		free( map );
+
+		logadd( LOG_ERROR, "iscsi_hashmap_create: Out of memory while allocating iSCSI hash map buckets" );
+
+		return NULL;
+	}
+
+	map->count         = 0;
+	map->removed_count = 0;
+	map->first         = NULL;
+	map->last          = (iscsi_hashmap_bucket *) &map->first;
+
+	return map;
+}
+
+/**
+ * Deallocates all buckets and the hash map itself allocated
+ * by iscsi_hashmap_create. The elements associated with the
+ * buckets are NOT freed by this function, this has to be done
+ * either manually or using the function iscsi_hashmap_iterate.
+ *
+ * @param[in] map Pointer to hash map and its buckets to deallocate.
+ * If this is NULL, nothing is done.
+ */
+void iscsi_hashmap_destroy(iscsi_hashmap *map)
+{
+	if ( map != NULL ) {
+		if ( map->buckets != NULL )
+			free( map->buckets );
+
+		free( 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 - 1);
+
+	for ( ;; ) {
+		iscsi_hashmap_bucket *entry = &map->buckets[index];
+
+		if ( entry->key == NULL ) {
+			*entry = *old_entry;
+
+			return entry;
+		}
+
+		index = (index + 1) & (map->capacity - 1);
+	}
+}
+
+/**
+ * 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_FACTOR;
+
+	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->last          = (iscsi_hashmap_bucket *) &map->first;
+	map->count        -= map->removed_count;
+	map->removed_count = 0;
+
+	do {
+		iscsi_hashmap_bucket *current = map->last->next;
+
+		if ( current->key == NULL ) {
+			map->last->next = current->next;
+
+			continue;
+		}
+
+		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;
+}
+
+/**
+ * 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)
+{
+	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 ^= *(uint64_t *) data++; // Hash 8 bytes of data at once
+		hash *= ISCSI_HASHMAP_HASH_XOR;
+	} while ( --num_blocks > 0UL );
+
+	return (uint32_t) (hash ^ hash >> 32ULL);
+}
+
+/**
+ * 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 - 1);
+
+	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 + 1) & (map->capacity - 1);
+	}
+}
+
+/**
+ * Creates a key from data and size and ensures
+ * its requirements for usage in hash map buckets.
+ * Currently keys to be used in a hash map bucket
+ * require a size of multiple by 8 bytes with
+ * the zero padding.
+ *
+ * @param[in] data Pointer to data to construct the key
+ * from and may NOT be NULL, so be careful.
+ * @param[in] len Length of the data to construct the key
+ * from, MUST be larger than 0, so be careful.
+ * @return Pointer to generated usable key or NULL in
+ * case of an error (usually memory exhaustion).
+ */
+uint8_t *iscsi_hashmap_create_key(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 );
+
+	if ( key == NULL ) {
+		logadd( LOG_ERROR, "iscsi_hashmap_create_key: Out of memory while allocating iSCSI hash map key" );
+
+		return key;
+	}
+
+	memcpy( key, data, len );
+	memset( key + len, 0, (key_size - len) ); // Zero pad additional bytes in case length is not a multiple of 8
+
+	return key;
+}
+
+/**
+ * Deallocates a key allocated with the function
+ * iscsi_hashmap_create_key.
+ *
+ * @param[in] key Pointer to key to deallocate, may NOT
+ * be NULL, so be careful.
+ */
+void iscsi_hashmap_destroy_key(uint8_t *key) {
+	free( key );
+}
+
+/**
+ * Adds a key / value pair to a specified hash map
+ * bucket list, if it doesn't exist already. The
+ * buckets are resized automatically if required.
+ * This function neither does make a copy of the key,
+ * nor of the value. Keys should be allocated using
+ * the function iscsi_hashmap_create_key or freed by
+ * using iscsi_hashmap_destroy_key in order to
+ * ensure the alignment and padding requirements.
+ *
+ * @param[in] map Pointer to hash map where the key and
+ * value pair should be added to, may NOT be NULL, so
+ * be careful.
+ * @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, MUST
+ * be a multiple of 8 bytes which is NOT checked, so
+ * be careful.
+ * @param[in] value Value of the key to add, NULL is
+ * allowed.
+ * @retval -1 Adding key / value pair would have required
+ * hash map resizing which failed (probably due to
+ * memory exhaustion).
+ * @retval 0 Key / value pair was added successfully.
+ */
+int iscsi_hashmap_put(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, uint8_t *value)
+{
+	if ( ((map->count + 1) > map->capacity) && (iscsi_hashmap_resize( map ) < 0) )
+		return -1;
+
+	const uint32_t hash = iscsi_hashmap_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++;
+
+		entry->key      = key;
+		entry->key_size = key_size;
+		entry->hash     = hash;
+	}
+
+	entry->value = value;
+
+	return 0;
+}
+
+/**
+ * Adds a key / value pair if it doesn't exist
+ * using the value of `*out_in_val`. If the pair
+ * does exist, value will be set in `*out_in`,
+ * meaning the value of the pair will be in
+ * '*out_in` regardless of whether or not it it
+ * existed in the first place.
+ * The buckets are resized automatically if required.
+ * This function neither does make a copy of the key,
+ * nor of the value. Keys should be allocated using
+ * the function iscsi_hashmap_create_key or freed by
+ * using iscsi_hashmap_destroy_key in order to
+ * ensure the alignment and padding requirements.
+ *
+ * @param[in] map Pointer to hash map where the key and
+ * value pair should be added to, may NOT be NULL, so
+ * be careful.
+ * @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, MUST
+ * be a multiple of 8 bytes which is NOT checked, so
+ * be careful.
+ * @param[in,out] out_in_value Value of the key to add,
+ * NULL is allowed.
+ * @retval -1 Adding key / value pair would have required
+ * hash map resizing which failed (probably due to
+ * memory exhaustion).
+ * @retval 0 Key / value pair was added successfully.
+ * @retval 1 Key already existed.
+ */
+int iscsi_hashmap_get_put(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, uint8_t **out_in_value)
+{
+	if ( ((map->count + 1) > map->capacity) && (iscsi_hashmap_resize( map ) < 0) )
+		return -1;
+
+	const uint32_t hash = iscsi_hashmap_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++;
+
+		entry->value    = *out_in_value;
+		entry->key      = key;
+		entry->key_size = key_size;
+		entry->hash     = hash;
+
+		return 0;
+	}
+
+	*out_in_value = entry->value;
+
+	return 1;
+}
+
+/**
+ * Adds a key / value pair to a specified hash map
+ * bucket list. If the key already exists, it will
+ * be overwritten and a callback function will be
+ * invoked in order to allow, e.g. deallocation of
+ * resources, if necessary. The buckets are resized
+ * automatically if required. This function neither
+ * does make a copy of the key, nor of the value.
+ * Keys should be allocated using the function
+ * iscsi_hashmap_create_key or freed by using
+ * iscsi_hashmap_destroy_key in order to ensure the
+ * alignment and padding requirements.
+ *
+ * @param[in] map Pointer to hash map where the key and
+ * value pair should be added to, may NOT be NULL, so
+ * be careful.
+ * @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, MUST
+ * be a multiple of 8 bytes which is NOT checked, so
+ * be careful.
+ * @param[in] value Value of the key to add, NULL is
+ * allowed.
+ * @param[in] callback Callback function which allows,
+ * for example, a dallocation of resources for the
+ * overwritten key and value pair. The function is
+ * invoked just before overwriting the old values.
+ * This may NOT be NULL, so take caution.
+ * @param[in] user_data Pointer to user specific data
+ * passed to the callback function in case more
+ * information is needed.
+ * @return -1 in case adding key / value pair would
+ * have required hash map resizing which failed
+ * (probably due to memory exhaustion), 0 if the
+ * Key / value pair was added successfully and
+ * the callback function also returned 0, otherwise
+ * the return value got by the callbuck function.
+ */
+int iscsi_hashmap_put_free(iscsi_hashmap *map, const uint8_t *key, const size_t key_size, uint8_t *value, iscsi_hashmap_callback callback, uint8_t *user_data)
+{
+	if ( ((map->count + 1) > map->capacity) && (iscsi_hashmap_resize( map ) < 0) )
+		return -1;
+
+	const uint32_t hash = iscsi_hashmap_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++;
+
+		entry->key      = key;
+		entry->key_size = key_size;
+		entry->hash     = hash;
+		entry->value    = value;
+
+		return 0;
+	}
+
+	int err = callback( entry->key, key_size, entry->value, user_data );
+
+	entry->key   = key;
+	entry->value = value;
+
+	return err;
+}
+
+/**
+ * Retrieves the value of a specified key from a hash map. Since the
+ * hash map supports NULL values, it is stored in an output variable.
+ *
+ * @param[in] map Pointer to the hash map to be searched
+ * for the key of which the value should be retrieved and
+ * may not be NULL, so take caution.
+ * @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, MUST
+ * be a multiple of 8 bytes which is NOT checked, so
+ * be careful.
+ * @param[out] out_value Pointer where the value of the found key
+ * is stored, maybe NULL if either the key's value is NULL or
+ * in case the key was not found. The pointer to the value itself
+ * may NOT be NULL, so be careful.
+ * @retval TRUE The key has been found and its value stored
+ * in the 'out_value' parameter.
+ * @retval FALSE The key has not been found and NULL has been
+ * stored in the 'out_value' parameter.
+ */
+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 );
+	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
+
+	*out_value = entry->value;
+
+	return entry->key != 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. If the specified key already has been
+ * removed, this function will do nothing.
+ *
+ * @param[in] map Pointer to the hash map to remove from
+ * and may not be NULL, so take caution.
+ * @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, MUST
+ * be a multiple of 8 bytes which is NOT checked, so
+ * be careful.
+ */
+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 );
+	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
+
+	if ( entry->key != NULL ) {
+		entry->key   = NULL;
+		entry->value = NULL;
+
+		map->removed_count++;
+	}
+}
+
+/**
+ * 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. 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.
+ * @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, MUST
+ * be a multiple of 8 bytes which is NOT checked, so
+ * be careful.
+ * @param[in] callback Callback function which allows,
+ * for example, a dallocation of resources for the
+ * key and value pair to be removed. The function is
+ * invoked just before marking the key / value pair
+ * as removed. This may NOT be NULL, so take caution.
+ * @param[in] user_data Pointer to user specific data
+ * passed to the callback function in case more
+ * information is needed.
+ */
+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 );
+	iscsi_hashmap_bucket *entry = iscsi_hashmap_find_entry( map, key, key_size, hash );
+
+	if ( entry->key != NULL ) {
+		callback( entry->key, entry->key_size, entry->value, user_data );
+
+		entry->key   = NULL;
+		entry->value = NULL;
+
+		map->removed_count++;
+	}
+}
+
+/**
+ * Returns the number of elements stored in the specified
+ * hash map. Elements marked for removal are not included.
+ *
+ * @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.
+ */
+int iscsi_hashmap_size(iscsi_hashmap *map)
+{
+	return (map->count - map->removed_count);
+}
+
+/**
+ * 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.
+ *
+ * @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 value of the callback
+ * function is below zero, the iteration will stop.
+ * @param[in] user_data Pointer to user specific data
+ * passed to the callback function in case more
+ * information is needed.
+ * @return The return code from the last invoked
+ * callback function. A negative value indicates an
+ * abortion of the iteration process.
+ */
+int iscsi_hashmap_iterate(iscsi_hashmap *map, iscsi_hashmap_callback callback, uint8_t *user_data)
+{
+	iscsi_hashmap_bucket *current = map->first;
+	int err = 0;
+
+	while ( current != NULL ) {
+		if ( current->key != NULL ) {
+			err = callback( current->key, current->key_size, current->value, user_data );
+
+			if ( err < 0 )
+				break;
+		}
+
+		current = current->next;
+	}
+
+	return err;
+}
+
+/**
  * Allocates an iSCSI packet data Basic Header Segment (BHS)
  * and zero fills the structure.
  *
- * @return Returns a pointer to BHS structure with all fields
+ * @return a pointer to BHS structure with all fields
  * initialized or NULL if the allocation failed.
  */
 iscsi_bhs_packet *iscsi_create_packet()
@@ -106,7 +663,7 @@ iscsi_bhs_packet *iscsi_append_ahs_packet(iscsi_bhs_packet *packet_data, const u
 	}
 
 	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 + iscsi_align(ahs_len, 4);
+	const uint32_t new_pkt_size = old_pkt_size + iscsi_align(ahs_len, ISCSI_ALIGN_SIZE);
 
 	if ( new_pkt_size > (sizeof(struct iscsi_bhs_packet) + 1020UL) ) {
 		logadd( LOG_ERROR, "iscsi_append_ahs_packet: Total numer of AHS packets exceeds 255 DWORDs" );
@@ -153,7 +710,7 @@ int iscsi_get_ahs_packets(const iscsi_bhs_packet *packet_data)
 
 	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, 4);
+		len = iscsi_align(len, ISCSI_ALIGN_SIZE);
 
 		ahs_len -= len;
 		ahs_pkt = ((uint8_t *) ahs_pkt) + (len - offsetof(struct iscsi_ahs_packet, data)); // Advance pointer to next AHS packet
@@ -188,7 +745,7 @@ iscsi_ahs_packet *iscsi_get_ahs_packet(const iscsi_bhs_packet *packet_data, cons
 			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, 4);
+		len = iscsi_align(len, ISCSI_ALIGN_SIZE);
 
 		ahs_len -= len;
 		ahs_pkt = ((uint8_t *) ahs_pkt) + (len - offsetof(struct iscsi_ahs_packet, data)); // Advance pointer to next AHS packet
@@ -199,13 +756,6 @@ iscsi_ahs_packet *iscsi_get_ahs_packet(const iscsi_bhs_packet *packet_data, cons
 	return NULL;
 }
 
-static inline void iscsi_put_be24(uint8_t *data, uint32_t val)
-{
-	data[0] = (uint8_t) (val >> 16UL);
-	data[1] = (uint8_t) (val >> 8UL);
-	data[2] = (uint8_t) val;
-}
-
 /**
  * Constructs and appends DataSegment (DS) to already allocated packet data.
  * There is no guarantee that the pointer stays the same. Any references
@@ -238,12 +788,12 @@ iscsi_bhs_packet *iscsi_append_ds_packet(iscsi_bhs_packet *packet_data, const in
 	}
 
 	const uint32_t old_pkt_size = (const uint32_t) sizeof(struct iscsi_bhs_packet) + ((uint32_t) packet_data->total_ahs_len << 2UL);
-	const uint32_t new_pkt_size = old_pkt_size + header_digest_size + iscsi_align(ds_len, 4) + data_digest_size;
+	const uint32_t new_pkt_size = old_pkt_size + header_digest_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_ahs_packet: Out of memory while allocating iSCSI AHS packet data for appending" );
+		logadd( LOG_ERROR, "iscsi_append_ds_packet: Out of memory while allocating iSCSI DS packet data for appending" );
 
 		return packet_data;
 	}
@@ -290,18 +840,17 @@ static const uint32_t crc32c_lut[] = { // Created with a polynomial reflect valu
 
 /**
  * Calculates CRC32C with 0x82F63B78 polynomial reflect according to iSCSI specs
+ * 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 Returns CRC32C value. THis function cannot fail.
+ * @return CRC32C value. THis function cannot fail.
  */
 static inline uint32_t iscsi_crc32c_update(const uint8_t *data, const uint len, uint32_t crc32c)
 {
-	uint i;
-
-	for ( i = 0; i < len; i += 4 ) {
+	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];
@@ -312,16 +861,81 @@ static inline uint32_t iscsi_crc32c_update(const uint8_t *data, const uint len,
 }
 
 /**
- * Calculates CRC32C with 0x82F63B78 polynomial reflect according to iSCSI specs
+ * 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.
- * @return Returns CRC32C value of BHS packet. THis function cannot fail.
  */
-uint32_t iscsi_calc_header_digest(iscsi_bhs_packet *packet_data)
+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, 4), ISCSI_CRC32C_INITIAL ) ^ ISCSI_CRC32C_XOR;
+
+	iscsi_put_be32( hdr_digest, crc32c );
+}
+
+/**
+ * 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 crc32c = iscsi_crc32c_update( (uint8_t *) packet_data, ISCSI_BHS_SIZE + ((uint32_t) packet_data->total_ahs_len << 2UL), ISCSI_CRC32C_INITIAL );
+	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 crc32c ^ ISCSI_CRC32C_XOR;
+	return iscsi_get_be32(pkt_crc32c) == crc32c;
+}
+
+/**
+ * Calculates data digest (CRC32) with 0x82F63B78 polynomial reflect
+ * of a whole DataSegment (CRC32C) according to the iSCSI specs.
+ * 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, 4);
+	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 );
+}
+
+/**
+ * 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, 4);
+	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;
 }
 
 /**
@@ -334,9 +948,9 @@ uint32_t iscsi_calc_header_digest(iscsi_bhs_packet *packet_data)
  * not be NULL.
  * @param[in] len Length of packet data to be checked for iSCSI, must be
  * at least 48 bytes (minimum BHS header size).
- * @return Returns 0 if it's clearly a iSCSI packet (detected without
+ * @return 0 if it's clearly a iSCSI packet (detected without
  * heuristics) or a positive integfer value up to 65536, if heuristics
- * were necessaty. A negative value is returned in case of an error or
+ * were necessary. A negative value is returned in case of an error or
  * if it's clearly not an iSCSI packet.
  */
 int iscsi_validate_packet(const struct iscsi_bhs_packet *packet_data, const uint32_t len)