- MemoryPool allocation-deallocation bug fixed

- generic Queue implemented - PacketRegistry allocation bug fixed - TCP implementation initials - ALIGN to type macros added
2023-01-17 08:19:29 +01:00 · 2023-01-17 08:19:29 +01:00 · 51696f7341
commit 51696f7341
parent 05288d7a3c
25 changed files with 2648 additions and 64 deletions
--- a/10
+++ b/10
@ -485,7 +485,7 @@ NUM_PROC_THREADS       = 1
 # normally produced when WARNINGS is set to YES.
 # The default value is: NO.

-EXTRACT_ALL            = NO
+EXTRACT_ALL            = YES

 # If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
 # be included in the documentation.
@ -1283,7 +1283,7 @@ HTML_STYLESHEET        =
 # list). For an example see the documentation.
 # This tag requires that the tag GENERATE_HTML is set to YES.

-HTML_EXTRA_STYLESHEET  =
+HTML_EXTRA_STYLESHEET  = ./docs/customdoxygen.css

 # The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
 # other source files which should be copied to the HTML output directory. Note
@ -1582,7 +1582,7 @@ DISABLE_INDEX          = NO
 # The default value is: NO.
 # This tag requires that the tag GENERATE_HTML is set to YES.

-GENERATE_TREEVIEW      = NO
+GENERATE_TREEVIEW      = YES

 # The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
 # doxygen will group on one line in the generated HTML documentation.
@ -2476,7 +2476,7 @@ TEMPLATE_RELATIONS     = NO
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.

-INCLUDE_GRAPH          = YES
+INCLUDE_GRAPH          = NO

 # If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
 # set to YES then doxygen will generate a graph for each documented file showing
@ -2485,7 +2485,7 @@ INCLUDE_GRAPH          = YES
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.

-INCLUDED_BY_GRAPH      = YES
+INCLUDED_BY_GRAPH      = NO

 # If the CALL_GRAPH tag is set to YES then doxygen will generate a call
 # dependency graph for every global function or class method.
--- a/arp_cache.c
+++ b/arp_cache.c
@ -28,7 +28,7 @@ static int arpc_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) {

 ArpCache *arpc_new(EthInterface * intf, uint16_t size) {
    // allocate table
-    ArpCache * arcp = (ArpCache *) dynmem_alloc(2 * sizeof(uint16_t) + sizeof(ConnBlock) + size * sizeof(ArpEntry));
+    ArpCache * arcp = (ArpCache *) dynmem_alloc(sizeof(ArpCache) + size * sizeof(ArpEntry));
    arcp->size = size;
    arcp->fill = 0;

--- a/docs/customdoxygen.css
+++ b/docs/customdoxygen.css
--- a/gen_queue.c
+++ b/gen_queue.c
@ -0,0 +1,58 @@
+//
+// Created by epagris on 2023.01.13..
+//
+
+#include <memory.h>
+#include <stdbool.h>
+#include "gen_queue.h"
+#include "dynmem.h"
+
+Queue *q_create(uint32_t length, uint32_t elemSize) {
+    Queue * q = (Queue *) dynmem_alloc(sizeof(Queue) + length * elemSize);
+    q->length = length;
+    q->elemSize = elemSize;
+    q->readIdx = 0;
+    q->writeIdx = 0;
+    memset(q->elements, 0, length * elemSize);
+
+    return q;
+}
+
+void q_clear(Queue * q) {
+    q->readIdx = 0;
+    q->writeIdx = 0;
+    memset(q->elements, 0, q->length * q->elemSize);
+}
+
+uint32_t q_avail(const Queue * q) {
+    return q->writeIdx - q->readIdx;
+}
+
+#define MQ_NEXT(size,current) (((current)+1)%(size))
+
+bool q_push(Queue * q, const void * src) {
+    if (MQ_NEXT(q->length, q->writeIdx) == q->readIdx) { // cannot push, queue is full
+        return false;
+    }
+
+    // can push
+    memcpy(q->elements + q->writeIdx * q->elemSize, src, q->elemSize);
+    q->writeIdx++;
+
+    // advance write pointer
+    q->writeIdx = MQ_NEXT(q->length, q->writeIdx);
+
+    return true;
+}
+
+void q_top(Queue * q, void * dest) {
+    memcpy(dest, q->elements + q->readIdx, q->elemSize);
+}
+
+void q_pop(Queue * q) {
+    if (q_avail(q) > 0) { // if there's something to pop
+        q->readIdx = MQ_NEXT(q->length, q->readIdx);
+    }
+}
+
+
--- a/gen_queue.h
+++ b/gen_queue.h
@ -0,0 +1,65 @@
+#ifndef ETHERLIB_TEST_QUEUE_H
+#define ETHERLIB_TEST_QUEUE_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * Generic Queue.
+ */
+typedef struct {
+    uint32_t writeIdx; ///< Next block to write
+    uint32_t readIdx; ///< Next block to read
+    uint32_t length; ///< Size of circular buffer
+    uint32_t elemSize; ///< Element size
+    uint8_t elements[]; ///< Array of packets
+} Queue;
+
+/**
+ * Create Queue.
+ * @param length length of circular buffer
+ * @param elemSize element size
+ * @return pointer to Queue instance OR NULL on failure
+ */
+Queue * q_create(uint32_t length, uint32_t elemSize);
+
+/**
+ * Create Queue based on storage type.
+ */
+#define Q_CREATE_T(length,T) q_create((length), sizeof(T))
+
+/**
+ * Clear circular buffer.
+ * @param q pointer to Queue
+ */
+void q_clear(Queue * q);
+
+/**
+ * Get number of available elements.
+ * @param q pointer to Queue
+ * @return number of available elements
+ */
+uint32_t q_avail(const Queue * q);
+
+/**
+ * Push element to the Queue.
+ * @param q pointer to Queue
+ * @param raw pointer to raw packet
+ * @return true on success, false on failure (e.g.: queue full)
+ */
+bool q_push(Queue * q, const void * src);
+
+/**
+ * Get top element.
+ * @param q pointer to Queue
+ * @return top element (COPY, NOT POINTER!)
+ */
+void q_top(Queue * q, void * dest);
+
+/**
+ * Pop top element.
+ * @param q pointer to Queue
+ */
+void q_pop(Queue * q);
+
+#endif //ETHERLIB_TEST_QUEUE_H
--- a/global_state.c
+++ b/global_state.c
@ -7,6 +7,7 @@
 #include "prefab/packet_parsers/arp_packet.h"
 #include "prefab/packet_parsers/icmp_packet.h"
 #include "etherlib/prefab/packet_parsers/igmp_packet.h"
+#include "etherlib/prefab/packet_parsers/tcp_segment.h"

 EthState gEthState;

@ -59,6 +60,14 @@ static void register_packet_parsers() {
    cdesc.hdrInsFn = insert_igmp_header;
    cdesc.propertySize = sizeof(IgmpProps);
    packreg_add_class(E.pcktReg, &cdesc);
+
+    // TCP packet parser
+    cdesc.class = ETH_TCP_PACKET_CLASS;
+    cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
+    cdesc.procFun = parse_tcp;
+    cdesc.hdrInsFn = insert_tcp_header;
+    cdesc.propertySize = sizeof(TcpProps);
+    packreg_add_class(E.pcktReg, &cdesc);
 }

 void ethlib_init() {
--- a/memory_pool.c
+++ b/memory_pool.c
@ -8,11 +8,12 @@

 MP *mp_init(uint8_t *p, uint32_t size) {
    ASSERT_BAD_ALIGN(p); // check for alignment
+    size = FLOOR_TO_4(size); // force alignment on size
    MP *pool = (MP *) p; // fill in properties
    pool->p = p + sizeof(MP); // compute beginning of the allocatable area
    pool->poolSize = size; // save total pool size
-    pool->blockRegistry = (MPAllocRecord *) ALIGN((p + size - sizeof(MPAllocRecord)), uint32_t); // determine block registry address
-    pool->freeSpace = ((uint8_t *) pool->blockRegistry) - pool->p - sizeof(MPAllocRecord); // calculate free space size
+    pool->blockRegistry = (MPAllocRecord *) (p + size - sizeof(MPAllocRecord)); // determine block registry address (points to the TOPMOST record, NOT one block further!)
+    pool->freeSpace = ((uint8_t *) pool->blockRegistry) - pool->p - 1 * sizeof(MPAllocRecord); // calculate free space size (with SENTRY and first FREE block)

    // place sentry element
    pool->blockRegistry[-1].addrStart = 0;
@ -34,19 +35,19 @@ uint8_t *mp_alloc(MP *mp, uint32_t size) {
    // contiguous block

    // round size to make it divisible by 4
-    size = ALIGN(size, uint32_t);
+    size = CEIL_TO_4(size);

-    // badness = area left free on the candidate free block after allocating the requested block
+    // badness = area left unclaimed on the candidate free block after allocating the requested block
    MPAllocRecord *bestBlock = NULL;
    uint32_t leastBadness = ~0;
    MPAllocRecord *recIter = mp->blockRegistry; // allocation record
    while (recIter->type != MPRT_SENTRY && leastBadness > 0) { // at badness = 0 just break, since it's a perfectly fitting block
        // look for suitable free block
-        if (recIter->type == MPRT_FREE && recIter->size > size) {
+        if (recIter->type == MPRT_FREE && recIter->size >= size) {
            uint32_t iterBadness = recIter->size - size;
            if (iterBadness < leastBadness) { // calculate badness
                bestBlock = recIter;
-                leastBadness = recIter->size - size;
+                leastBadness = iterBadness;
            }
        }
        recIter--; // step to next block
@ -58,15 +59,20 @@ uint8_t *mp_alloc(MP *mp, uint32_t size) {
        bestBlock->type = MPRT_ALLOCATED;
        ptr = bestBlock->addrStart;
    } else { // if there are some bytes left between allocated blocks
+        // examine, that an allocated block was not blocking registry table downward growth
+        if (mp->blockRegistry[0].type != MPRT_FREE) { // if cannot allocate, return NULL
+            return NULL;
+        }
+
        // shift the registry below best block
-        MPAllocRecord *rec = mp->blockRegistry - (mp->blockRecCnt + 1); // bottom of the registry
+        MPAllocRecord *rec = mp->blockRegistry - (mp->blockRecCnt); // bottom of the FILLED registry (there's one unfilled entry below this point, the new record)
        while (rec != bestBlock) {
-            *(rec) = *(rec + 1);
+            *(rec - 1) = *(rec);
            rec++;
        }

        // store information on allocated
-        MPAllocRecord *allocated = bestBlock - 1;
+        MPAllocRecord *allocated = bestBlock - 1; // don't copy best block!
        allocated->type = MPRT_ALLOCATED;
        allocated->size = size;
        allocated->addrStart = bestBlock->addrStart;
@ -76,6 +82,9 @@ uint8_t *mp_alloc(MP *mp, uint32_t size) {
        bestBlock->size -= size;
        bestBlock->addrStart += size;

+        // decrease last free block (adjacent to block registry) size with the increase in the block registry
+        mp->blockRegistry[0].size -= sizeof(MPAllocRecord);
+
        // decrease free size with the increase in the registry size
        mp->freeSpace -= sizeof(MPAllocRecord);

@ -105,6 +114,7 @@ static void mp_join_free_blocks(MP *mp) {
                joinIter--;
            }
            mp->freeSpace += sizeof(MPAllocRecord);
+            mp->blockRegistry[0].size += sizeof(MPAllocRecord); // grow the last record size
            mp->blockRecCnt--;
        } else {
            recIter--;
@ -117,8 +127,7 @@ void mp_free(MP *mp, const uint8_t *p) {
    bool success = false;
    MPAllocRecord *recIter = mp->blockRegistry;
    while (recIter->type != MPRT_SENTRY) {
-        if ((recIter->type == MPRT_ALLOCATED) &&
-            ((recIter->addrStart <= p) && ((recIter->addrStart + recIter->size) > p))) { // ...block found
+        if ((recIter->type == MPRT_ALLOCATED) && (recIter->addrStart == p)) { // ...block found
            recIter->type = MPRT_FREE;
            mp->freeSpace += recIter->size;
            success = true;
@ -142,7 +151,33 @@ void mp_report(MP *mp) {
        recIter--;
        bi++;
    }
+    INFO("%05u %s\n", bi, "SENTRY");
+
    INFO("----------------------\n");
    INFO("Used: %u (mgmt: %u)\nFree: %u of %u\n\n", (mp->poolSize - mp->freeSpace),
-         (mp->blockRecCnt + 1) * sizeof(MPAllocRecord), mp->freeSpace, mp->poolSize);
+         (mp->blockRecCnt) * sizeof(MPAllocRecord), mp->freeSpace, mp->poolSize);
+}
+
+uint32_t mp_largest_free_block_size(MP *mp) {
+    MPAllocRecord *recIter = mp->blockRegistry;
+    if (recIter->type == MPRT_ALLOCATED) { // if topmost block is allocated, then registry table cannot be grown
+        return 0;
+    }
+
+    uint32_t largestFreeSize = 0;
+    while (recIter->type != MPRT_SENTRY) {
+        largestFreeSize = (recIter->size > largestFreeSize) ? recIter->size : largestFreeSize;
+        recIter--;
+    }
+    return largestFreeSize;
+}
+
+void mp_foreach_block(MP * mp, MPForeachFn * fn, void * userData, bool inclFree) {
+    MPAllocRecord *recIter = mp->blockRegistry;
+    while (recIter->type != MPRT_SENTRY) {
+        if (recIter->type != MPRT_FREE || inclFree) {
+            fn(mp, recIter, userData);
+        }
+        recIter--;
+    }
 }
--- a/memory_pool.h
+++ b/memory_pool.h
@ -76,4 +76,22 @@ void mp_free(MP * mp, const uint8_t * p);
 */
 void mp_report(MP * mp);

+/**
+ * Get largest contiguous free block size.
+ * @param mp pointer to memory pool
+ * @return largest block size (zero if no more allocatable space is available)
+ */
+uint32_t mp_largest_free_block_size(MP *mp);
+
+typedef void (MPForeachFn)(MP * mp, const MPAllocRecord * rec, void * userData);
+
+/**
+ * Run function on each block.
+ * @param mp pointer to memory pool
+ * @param fn callback function pointer
+ * @param userData data passed to callback function
+ * @param inclFree if true, free blocks are also included into the enumeration
+ */
+void mp_foreach_block(MP * mp, MPForeachFn * fn, void * userData, bool inclFree);
+
 #endif //ETHERLIB_MEMORY_POOL_H
--- a/packet_registry.c
+++ b/packet_registry.c
@ -23,12 +23,13 @@ PcktRegistry * packreg_new() {
 void packreg_add_class(PcktRegistry * packReg, const PcktClassDesc * classDesc) {
    // if buffer is full and reallocation and resize is needed
    if (packReg->reservedCnt == packReg->entCnt) {
-        PcktClassDesc * newEnts = (PcktClassDesc *) dynmem_alloc(packReg->reservedCnt * 2);
+        PcktClassDesc * newEnts = (PcktClassDesc *) dynmem_alloc(packReg->reservedCnt * 2 * sizeof(PcktClassDesc));
        ASSERT_NULL(newEnts);
        memcpy(newEnts, packReg->ents, packReg->reservedCnt * sizeof(PcktClassDesc));
        PcktClassDesc * oldEnts = packReg->ents;
        packReg->ents = newEnts;
        dynmem_free(oldEnts);
+        packReg->reservedCnt *= 2;
    }

    // append new item
--- a/packet_registry.h
+++ b/packet_registry.h
@ -36,7 +36,8 @@ typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement
    uint16_t headerSize; /**< Header size in bytes */ \
    uint16_t containedPacketClass; /**< Class of contained packet. Zero if no packet contained. */ \
    uint16_t ownPacketClass; /**< Our own packet class */   \
-    uint16_t accumulatedOffset; /** Accumulated offset from the beginning of the packet */ \
+    uint16_t accumulatedOffset; /** Accumulated offset from the beginning of the packet */           \
+    uint16_t bytesToEnd; /** Remaining bytes to the end of payload */                    \
    bool8_t validityOK; /**< Indicates that checksum is OK. */                                     \

 typedef struct {
--- a/packet_sieve.c
+++ b/packet_sieve.c
@ -8,11 +8,11 @@
 #include "dynmem.h"
 #include "utils.h"

-bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilterCondition * c2) {
+bool packfiltcond_cmp(const PcktSieveFilterCondition *c1, const PcktSieveFilterCondition *c2) {
    return !memcmp(c1, c2, sizeof(PcktSieveFilterCondition));
 }

-void packfiltcond_zero(PcktSieveFilterCondition * cond) {
+void packfiltcond_zero(PcktSieveFilterCondition *cond) {
    memset(cond, 0, sizeof(PcktSieveFilterCondition));
 }

@ -25,7 +25,7 @@ PcktSieve *packsieve_new() {

 void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthInterface_ *intf) {
    // extract fields
-    uint8_t * data = rawPckt->payload;
+    uint8_t *data = rawPckt->payload;
    uint32_t size = rawPckt->size;

    // process payload, fetch packet class etc.
@ -63,29 +63,35 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
        }
        offset += header->props.headerSize;
        header->props.accumulatedOffset = offset;
+        header->props.bytesToEnd = size - header->props.accumulatedOffset;
+        header->props.hdrInsFn = cdesc->hdrInsFn;

        ownClass = containedClass;
        lastHeader = header;
-    } while (ownClass != 0);
+    } while ((ownClass != 0) && lastHeader->props.validityOK);

    // ------------------------------------

+    if (!lastHeader->props.validityOK) { // if packet is not valid, then drop
+        goto header_release; // GOTO here!
+    }
+
    Pckt packet;
    packet.time_s = rawPckt->time_s;
    packet.time_ns = rawPckt->time_ns;

    // lookup headers in the sieve
-    const PcktHeaderElement * headerIter = outermostHeader;
+    const PcktHeaderElement *headerIter = outermostHeader;
    const PcktSieveLayer *layer = &sieve->layer0; // innermost matched sieve layer
    bool found = true; // first structure is always an Ethernet-frame
    while (found && headerIter) {
-        const PcktSieveLayer * nodeIter = layer->nodes;
+        const PcktSieveLayer *nodeIter = layer->nodes;
        found = false;
        while (nodeIter && !found) {
            found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props, intf); // specific or general match
            if (found) {
                layer = nodeIter; // advance in the sieve tree
-                const PcktHeaderElement * containedHeader = headerIter->next; // advance on headers
+                const PcktHeaderElement *containedHeader = headerIter->next; // advance on headers
                if (layer->cbFn != NULL) { // if defined, invoke layer callback function
                    offset = containedHeader->props.accumulatedOffset; // accumulated offset + own header size
                    packet.header = containedHeader;
@ -107,10 +113,12 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
 //        INFO("Packet headers not fully processed!\n");
 //    }

+
    // release header chain blocks
-    PcktHeaderElement * iter = outermostHeader;
+    header_release:; // empty line, solely for label placement
+    PcktHeaderElement *iter = outermostHeader;
    while (iter != NULL) {
-        PcktHeaderElement * next = iter->next;
+        PcktHeaderElement *next = iter->next;
        dynmem_free(iter);
        iter = next;
    }
@ -118,7 +126,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI

 PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass) {
    // search for matching layer
-    PcktSieveLayer * nodeIter = parent->nodes;
+    PcktSieveLayer *nodeIter = parent->nodes;
    bool alreadyExists = false;
    while (nodeIter != NULL && !alreadyExists) {
        if ((packfiltcond_cmp(&nodeIter->filtCond, filtCond) || (nodeIter->matchAny && matchAny)) && (nodeIter->filtFn == filtFn)) { // if matching... [search for specific match OR any match]
@ -147,7 +155,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte
                layer->prev = layer;
            }
        } else { // for 'any' match if at least a single node is already present
-            PcktSieveLayer * iter = parent->nodes;
+            PcktSieveLayer *iter = parent->nodes;
            while (iter->next != NULL) { // find the last node
                iter = iter->next;
            }
@ -167,14 +175,14 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte
    }
 }

-bool packsieve_remove_layer(PcktSieveLayer * layer) {
+bool packsieve_remove_layer(PcktSieveLayer *layer) {
    // avoid NULL-operations
    if (layer == NULL) {
        return true;
    }

    // remove parent elements if their only subnode is the one we're deleting
-    PcktSieveLayer * parent;
+    PcktSieveLayer *parent;
    while (layer != NULL && layer->nodes == NULL) {
        parent = layer->parent; // store parent

@ -208,8 +216,8 @@ void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
    } else {
        INFO("%*c└─┤%d├───\n", indent, ' ', layer->packetClass);
    }
-    const PcktSieveLayer * nodeIter = layer->nodes;
-    while(nodeIter) {
+    const PcktSieveLayer *nodeIter = layer->nodes;
+    while (nodeIter) {
        packsieve_report(nodeIter, indent + ETH_SIEVE_LAYER_INDENT_PER_LEVEL);
        nodeIter = nodeIter->next;
    }
@ -222,7 +230,7 @@ void pckthdr_chain_free(PcktHeaderElement *hdr) {
    }

    // free
-    PcktHeaderElement * next;
+    PcktHeaderElement *next;
    while (hdr != NULL) {
        next = hdr->next;
        dynmem_free(hdr);
--- a/prefab/conn_blocks/tcp/tcp_window.c
+++ b/prefab/conn_blocks/tcp/tcp_window.c
@ -0,0 +1,52 @@
+//
+// Created by epagris on 2023.01.16..
+//
+
+#include <memory.h>
+#include <stdbool.h>
+#include "tcp_window.h"
+#include "etherlib/dynmem.h"
+
+TcpWindow *tcpw_create(uint32_t size) {
+    uint32_t allocSize = sizeof(TcpWindow) + size; // calculate full allocation size
+    TcpWindow * win = (TcpWindow *) dynmem_alloc(allocSize); // allocate data block at the end
+    mp_init(win->data, size); // initialize memory pool
+
+    return win;
+}
+
+#define TCP_WINDOW_MAX_WINSIZE_PADDING (16) // keep-out zone, this way allocated blocks will not block registry table growth TODO: not the best solution
+
+uint32_t tcpw_get_max_window_size(TcpWindow * tcpw) {
+    return mp_largest_free_block_size(&tcpw->pool) - sizeof(TcpWindowSegment) - TCP_WINDOW_MAX_WINSIZE_PADDING;
+}
+
+TcpWindowSegment * tcpw_store_segment(TcpWindow * tcpw, const uint8_t * data, uint32_t size, uint32_t seqNum) {
+    TcpWindowSegment * seg = (TcpWindowSegment *) mp_alloc(&tcpw->pool, sizeof(TcpWindowSegment) + size);
+    if (seg == NULL) { // could not store...
+        return NULL;
+    }
+
+    // store segment descriptor
+    seg->size = size;
+    seg->seqNum = seqNum;
+
+    // store segment
+    memcpy(seg->data, data, size);
+
+    return seg;
+}
+
+static void tcpw_segment_search(MP *mp, const MPAllocRecord * rec, void * userData) {
+    TcpWindow * tcpw = (TcpWindow *) userData;
+
+}
+
+uint32_t tcpw_get_acknowledge(TcpWindow * tcpw) {
+    mp_foreach_block(&tcpw->pool, tcpw_segment_search, tcpw, false);
+    return 0;
+}
+
+void tcpw_set_sequence_number(TcpWindow * tcpw, uint32_t seqNum) {
+    tcpw->seqNum = seqNum;
+}
--- a/prefab/conn_blocks/tcp/tcp_window.h
+++ b/prefab/conn_blocks/tcp/tcp_window.h
@ -0,0 +1,26 @@
+#ifndef ETHERLIB_TEST_TCP_WINDOW_H
+#define ETHERLIB_TEST_TCP_WINDOW_H
+
+#include <stdint.h>
+#include "etherlib/memory_pool.h"
+
+/**
+ * TCP segment descriptor
+ */
+typedef struct {
+    uint32_t seqNum; ///< sequence number (sequence number of the first octet)
+    uint32_t size; ///< segment size
+    uint8_t data[]; ///< Segment data
+} TcpWindowSegment;
+
+typedef struct {
+    bool ackAvail; ///< acknowledge is available
+    uint32_t seqNum; ///< Sequence number
+    uint32_t lastAcked; ///< last acknowledged byte
+    MP pool; ///< Pool for segment management
+    uint8_t data[]; ///< Window data storage
+} TcpWindow;
+
+TcpWindow * tcpw_create(uint32_t size);
+
+#endif //ETHERLIB_TEST_TCP_WINDOW_H
--- a/prefab/conn_blocks/tcp_connblock.c
+++ b/prefab/conn_blocks/tcp_connblock.c
@ -0,0 +1,115 @@
+#include "tcp_connblock.h"
+
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "../packet_parsers/packet_parsers.h"
+#include "../../utils.h"
+#include "../../dynmem.h"
+#include "etherlib/pckt_assembler.h"
+#include "etherlib/eth_interface.h"
+#include "ipv4_connblock.h"
+#include "etherlib/prefab/packet_parsers/tcp_segment.h"
+#include "etherlib/gen_queue.h"
+#include "etherlib_options.h"
+
+static bool filtTcp(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, EthInterface *intf) {
+    IPv4Props *ipProps = (IPv4Props *) contProps;
+    TcpProps *tcpProps = (TcpProps *) ownProps;
+
+    return ipProps->Protocol == ETH_UDP_PACKET_CLASS && (TCP_PORT_FROM_FILTCOND(filtCond) == tcpProps->DestinationPort);
+}
+
+/**
+ * TCP state.
+ */
+typedef struct {
+    uint16_t localPort;
+    uint16_t remotePort;
+    ip4_addr remoteAddr;
+    uint32_t sequenceNumber;
+    uint32_t ackNumber;
+    uint16_t window;
+    uint8_t * txWindow;
+    uint8_t * rxWindow;
+    Queue * txQueue;
+    Queue * rxQueue;
+} TcpState;
+
+void tcps_init(TcpState *tcps, uint16_t localPort) {
+    tcps->sequenceNumber = (uint32_t) rand();
+    tcps->localPort = localPort;
+    tcps->remotePort = 0;
+    tcps->remoteAddr = 0;
+    tcps->ackNumber = 0;
+    tcps->window = 0;
+    tcps->txWindow = dynmem_alloc(ETHLIB_DEF_TCP_WINDOW_SIZE);
+    tcps->rxWindow = dynmem_alloc(ETHLIB_DEF_TCP_WINDOW_SIZE);
+    /*tcps->txQueue = Q_CREATE_T(ETHLIB_DEF_TCP_QUEUE_SIZE, TcpQueueTicker);
+    tcps->rxQueue = Q_CREATE_T(ETHLIB_DEF_TCP_QUEUE_SIZE, TcpQueueTicker);*/
+}
+
+void tcp_bind(ConnBlock * connBlock, ip4_addr remoteAddr, uint16_t remotePort) {
+    TcpState * tcps = (TcpState *) connBlock->tag;
+    tcps->remoteAddr = remoteAddr;
+    tcps->remotePort = remotePort;
+}
+
+int tcps_receive_data_segment(TcpState * tcps, const Pckt * pckt) {
+
+}
+
+ConnBlock tcp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) {
+    ConnBlock tcpConnB;
+    ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block
+
+    PcktSieveFilterCondition filtCond;
+    packfiltcond_zero(&filtCond);
+    TCP_PORT_TO_FILTCOND(&filtCond, port);
+
+    PcktSieveLayerTag tag; // store TCP state into sieve layer's tag
+    tag.p = dynmem_alloc(sizeof(TcpState));
+    memset(tag.p, 0, sizeof(TcpState));
+
+    tcpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtTcp, cbFn, tag, ETH_TCP_PACKET_CLASS);
+    ASSERT_NULL(tcpConnB.sieveLayer);
+
+    tcpConnB.intf = intf;
+
+    SNPRINTF(tcpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "TCP port: %d", port);
+
+    return tcpConnB;
+}
+
+int tcp_send_segment(const struct ConnBlock_ *connBlock, const uint8_t *data, uint32_t size) {
+    // allocate headers
+    PcktHeaderElement *tcpHeader = ALLOC_HEADER_ELEMENT(TcpProps);
+    PcktHeaderElement *ipHeader = ALLOC_HEADER_ELEMENT(IPv4Props);
+    PcktHeaderElement *ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
+    tcpHeader->next = NULL;
+    tcpHeader->prev = ipHeader;
+    ipHeader->next = tcpHeader;
+    ipHeader->prev = ethHeader;
+    ethHeader->next = ipHeader;
+    ethHeader->prev = NULL;
+
+    // prepare headers
+    TcpProps *tcpProps = HEADER_FETCH_PROPS(TcpProps, tcpHeader);
+    IPv4Props *ipProps = HEADER_FETCH_PROPS(IPv4Props, ipHeader);
+    EthernetProps *ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);
+
+    // get TCP state
+    PcktSieveLayer *layer = connBlock->sieveLayer; // TCP layer
+    TcpState *tcpState = (TcpState *) layer->tag.p;
+
+    // fetch sieve layers and fill transmit headers
+    tcpProps->SourcePort = tcpState->localPort;
+    tcpProps->DestinationPort = tcpState->remotePort;
+    tcpProps->SequenceNumber = tcpState->sequenceNumber;
+    tcpProps->AcknowledgementNumber = tcpState->ackNumber;
+    tcpProps->Window = tcpState->window;
+    tcpProps->Checksum = 0;
+    tcpProps->UrgentPtr = 0;
+
+    return 0;
+}
--- a/prefab/conn_blocks/tcp_connblock.h
+++ b/prefab/conn_blocks/tcp_connblock.h
@ -0,0 +1,31 @@
+#ifndef ETHERLIB_TEST_TCP_CONNBLOCK_H
+#define ETHERLIB_TEST_TCP_CONNBLOCK_H
+
+#define TCP_PORT_FROM_FILTCOND(fc) ((fc)->uw[7])
+#define TCP_PORT_TO_FILTCOND(fc,port) (((fc)->uw[7]) = (port))
+
+#include <stdint.h>
+#include "../packet_parsers/ipv4_types.h"
+#include "../../connection_block.h"
+
+struct EthInterface_;
+
+/**
+ * Create new TCP connection block
+ * @param intf associated Ethernet interface
+ * @param ipAddr local IP-address
+ * @param port local port
+ * @param cbFn receive callback function
+ * @return TCP connection block
+ */
+ConnBlock tcp_new_connblock(struct EthInterface_ *intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn);
+
+/**
+ * Bind TCP connection to remote socket.
+ * @param connBlock pointer to TCP connection block
+ * @param remoteAddr remote socket address
+ * @param remotePort remote socket port
+ */
+void tcp_bind(ConnBlock * connBlock, ip4_addr remoteAddr, uint16_t remotePort);
+
+#endif //ETHERLIB_TEST_TCP_CONNBLOCK_H
--- a/prefab/packet_parsers/icmp_packet.c
+++ b/prefab/packet_parsers/icmp_packet.c
@ -7,6 +7,8 @@
 #include "ipv4_packet.h"

 int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
+    const uint8_t * hdrStart = hdr;
+
    // parse header
    IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, pcktHdrLe);
    FETCH_BYTE_ADVANCE(&icmpProps->type, hdr);
@ -16,7 +18,8 @@ int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
    FETCH_WORD_ADVANCE(&icmpProps->sequenceNumber, hdr);

    // fill-in common fields
-    icmpProps->validityOK = true; // TODO...
+    IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, pcktHdrLe->prev);
+    icmpProps->validityOK = chksum(hdrStart, ipProps->TotalLength - ETH_IPv4_HEADER_SIZE, false) == 0;
    icmpProps->containedPacketClass = 0;
    icmpProps->headerSize = ETH_ICMP_HEADER_SIZE;

@ -35,8 +38,8 @@ void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
    FILL_ADVANCE(hdr, &icmpProps->sequenceNumber, 2);

    IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, headers->prev);
-    icmpProps->checksum = chksum(hdrStart, ipProps->TotalLength - ETH_IPv4_HEADER_SIZE);
-    FILL_WORD_H2N_ADVANCE(ChkSumPtr, icmpProps->checksum);
+    icmpProps->checksum = chksum(hdrStart, ipProps->TotalLength - ETH_IPv4_HEADER_SIZE, false);
+    FILL_WORD_ADVANCE(ChkSumPtr, icmpProps->checksum);
 }


--- a/prefab/packet_parsers/igmp_packet.c
+++ b/prefab/packet_parsers/igmp_packet.c
@ -36,7 +36,7 @@ void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
    FILL_WORD_ADVANCE(hdr, igmpProps->checksum);
    FILL_DWORD_ADVANCE(hdr, igmpProps->groupAddr);

-    igmpProps->checksum = chksum(hdrBeg, ETH_IGMP_HEADER_SIZE);
+    igmpProps->checksum = chksum(hdrBeg, ETH_IGMP_HEADER_SIZE, false);
    memcpy(ChkSumPtr, &igmpProps->checksum, 2);
 }

@ -49,7 +49,7 @@ int parse_igmp(const uint8_t * hdr, uint32_t size, PcktHeaderElement * pcktHdrLe
    FETCH_WORD_ADVANCE(&igmpProps->groupAddr, hdr);

    // fill-in common fields
-    uint16_t calcChkSum = chksum(hdr, ETH_IGMP_HEADER_SIZE);
+    uint16_t calcChkSum = chksum(hdr, ETH_IGMP_HEADER_SIZE, false);
    igmpProps->validityOK = (calcChkSum == 0);
    igmpProps->containedPacketClass = 0;
    igmpProps->headerSize = ETH_IGMP_HEADER_SIZE;
--- a/prefab/packet_parsers/ipv4_packet.c
+++ b/prefab/packet_parsers/ipv4_packet.c
@ -15,7 +15,8 @@ static bool check_ipv4_validity(const uint8_t * hdr, const IPv4Props *ipProps) {
    bool valid =
            (ipProps->Version == 4) &&
            (ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) &&
-            (ntohs(ipProps->HeaderChecksum) == chksum(hdr, ETH_IPv4_HEADER_SIZE));
+            (chksum(hdr, ETH_IPv4_HEADER_SIZE, false) == 0) &&
+            (ipProps->FragmentOffset == 0) && !(ipProps->Flags & 0x02); // discard if fragmented
    return valid;
 }

@ -77,7 +78,7 @@ void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
    FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);

    // calculate checksum after filling header
-    ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE);
+    ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE, false);
    memcpy(ChkSumPtr, &ipProps->HeaderChecksum, 2);
 }

--- a/prefab/packet_parsers/tcp_segment.c
+++ b/prefab/packet_parsers/tcp_segment.c
@ -0,0 +1,179 @@
+//
+// Created by epagris on 2023.01.14..
+//
+
+#include "tcp_segment.h"
+#include "etherlib/utils.h"
+#include "ethernet_frame.h"
+#include "etherlib/dynmem.h"
+
+#include "tcp_udp_common.h"
+#include "ipv4_packet.h"
+
+#define TCP_SINGLE_BYTE_OPTION(kind) (((kind) == TCP_OPT_KIND_EOL) || ((kind) == TCP_OPT_KIND_NOP))
+
+static int tcp_fetch_options(const uint8_t * hdr, uint8_t size, TcpOption ** optLe) {
+    uint8_t idx = 0;
+    TcpOption * lastOpt = NULL;
+    int optCnt = 0;
+    while (idx < size) {
+        uint8_t kind = hdr[idx + 0]; // read option kind
+
+        // skip further reading when EOL or NOP has been detected
+        if (TCP_SINGLE_BYTE_OPTION(kind)) {
+            idx++;
+            continue;
+        }
+
+        uint8_t valueSize = hdr[idx + 1] - 2; // read option length
+
+        TcpOption * opt = (TcpOption *) dynmem_alloc(sizeof(TcpOption) + valueSize); // allocate option
+
+        // fill-in
+        opt->kind = kind;
+        opt->size = valueSize;
+        memcpy(opt->value, hdr + idx + 2, opt->size); // copy option value
+        opt->next = NULL;
+
+        // store option
+        if (*optLe == NULL) {
+            *optLe = opt;
+        } else {
+            lastOpt->next = opt;
+        }
+
+        lastOpt = opt; // advance last opt
+        idx += opt->size + 2; // advance index
+        optCnt++; // increase opt count
+    }
+
+    return optCnt;
+}
+
+static void tcp_insert_options(uint8_t * hdr, uint8_t size, TcpOption * optLe) {
+    TcpOption * opt = optLe;
+    uint8_t idx = 0;
+    while (opt != NULL && idx < size) {
+        hdr[idx + 0] = opt->kind; // insert kind
+
+        // skip further reading when EOL or NOP has been detected
+        if (TCP_SINGLE_BYTE_OPTION(opt->kind)) {
+            idx++;
+            opt = opt->next;
+            continue;
+        }
+
+        uint8_t length = opt->size + 2; // insert length
+        hdr[idx + 1] = length;
+        memcpy(hdr + idx + 2, opt->value, opt->size); // insert value
+
+        idx += length; // advance idx
+        opt = opt->next; // advance iterator
+    }
+
+    // insert NOP padding if needed
+    if (idx < (size - 1)) {
+        uint8_t paddingNop = size - idx - 1;
+        memset(hdr + idx, 1, paddingNop);
+    }
+
+    // insert EOL if needed
+    if (idx < size) {
+        memset(hdr + idx, 0, 1);
+    }
+}
+
+static uint32_t tcp_get_options_size(TcpOption * optLe) {
+    TcpOption * opt = optLe;
+    uint32_t size = 0;
+    while (opt != NULL) {
+        if (TCP_SINGLE_BYTE_OPTION(opt->kind)) {
+            size += 1;
+        } else {
+            size += opt->size + 2;
+        }
+        opt = opt->next;
+    }
+
+    // round up to the neareast integer multiple of four bytes
+    size = CEIL_TO_4(size);
+
+    return size;
+}
+
+int parse_tcp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
+    const uint8_t * hdrBegin = hdr;
+
+    // fetch fixed portion of the segment header
+    TcpProps * tcpProps = HEADER_FETCH_PROPS(TcpProps, pcktHdrLe);
+    FETCH_WORD_H2N_ADVANCE(&tcpProps->SourcePort, hdr);
+    FETCH_WORD_H2N_ADVANCE(&tcpProps->DestinationPort, hdr);
+    FETCH_DWORD_H2N_ADVANCE(&tcpProps->SequenceNumber, hdr);
+    FETCH_DWORD_H2N_ADVANCE(&tcpProps->AcknowledgementNumber, hdr);
+    uint8_t dataOffset_Flags;
+    FETCH_BYTE_ADVANCE(&dataOffset_Flags, hdr);
+    tcpProps->DataOffset = dataOffset_Flags >> 4;
+    uint8_t flagsLower;
+    FETCH_BYTE_ADVANCE(&flagsLower, hdr);
+    tcpProps->Flags = flagsLower | ((dataOffset_Flags & 0x01) << 8);
+    FETCH_WORD_H2N_ADVANCE(&tcpProps->Window, hdr);
+    FETCH_WORD_H2N_ADVANCE(&tcpProps->Checksum, hdr);
+    FETCH_WORD_H2N_ADVANCE(&tcpProps->UrgentPtr, hdr);
+
+    // fetch options
+    tcpProps->options = NULL; // no options by default
+    uint8_t fullHeaderSize = tcpProps->DataOffset * 4;
+    if (fullHeaderSize > ETH_TCP_HEADER_LENGTH) {
+        uint8_t optSize = fullHeaderSize - ETH_TCP_HEADER_LENGTH;
+        tcp_fetch_options(hdr, optSize, &tcpProps->options);
+    }
+
+    // verify checksum
+    const IPv4Props *ipProps = (IPv4Props *) &pcktHdrLe->prev->props;
+    uint16_t headerAndPayloadLength = ipProps->bytesToEnd;
+    IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_TCP_PACKET_CLASS, htons(headerAndPayloadLength)};
+    uint16_t chkSum = tcp_udp_checksum(&ph, hdrBegin, headerAndPayloadLength);
+
+    tcpProps->headerSize = fullHeaderSize;
+    tcpProps->validityOK = (chkSum == 0);
+    tcpProps->containedPacketClass = 0;
+
+    return tcpProps->validityOK ? 0 : -1;
+}
+
+void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+    uint8_t * hdrBegin = hdr;
+    TcpProps * tcpProps = HEADER_FETCH_PROPS(TcpProps, headers); // fetch header
+
+    // get option area size and computer full area size
+    uint32_t optSize = tcp_get_options_size(tcpProps->options);
+    uint32_t fullHeaderSize = ETH_TCP_HEADER_LENGTH + optSize;
+    tcpProps->DataOffset = fullHeaderSize >> 2;
+
+    // fill beginning of the header
+    FILL_WORD_H2N_ADVANCE(hdr, tcpProps->SourcePort);
+    FILL_WORD_H2N_ADVANCE(hdr, tcpProps->DestinationPort);
+    FILL_DWORD_H2N_ADVANCE(hdr, tcpProps->SequenceNumber);
+    FILL_DWORD_H2N_ADVANCE(hdr, tcpProps->AcknowledgementNumber);
+    uint8_t dataOffset_Flags = (tcpProps->DataOffset << 4) | (tcpProps->Flags >> 8);
+    FILL_BYTE_ADVANCE(hdr, &dataOffset_Flags);
+    uint8_t flagsLower = tcpProps->Flags & 0xFF;
+    FILL_BYTE_ADVANCE(hdr, &flagsLower);
+    FILL_WORD_H2N_ADVANCE(hdr, tcpProps->Window);
+    tcpProps->Checksum = 0;
+    uint8_t * ChkSumPtr = hdr;
+    FILL_WORD_H2N_ADVANCE(hdr, tcpProps->Checksum);
+    FILL_WORD_H2N_ADVANCE(hdr, tcpProps->UrgentPtr);
+
+    // fill options
+    tcp_insert_options(hdr, optSize, tcpProps->options);
+
+    // calculate checksum
+    const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props;
+    uint16_t headerAndPayloadLength = fullHeaderSize + headers->props.bytesToEnd;
+    IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_TCP_PACKET_CLASS, htons(headerAndPayloadLength)};
+    tcpProps->Checksum = tcp_udp_checksum(&ph, hdrBegin, headerAndPayloadLength);
+    memcpy(ChkSumPtr, &tcpProps->Checksum, 2);
+}
+
+
--- a/prefab/packet_parsers/tcp_segment.h
+++ b/prefab/packet_parsers/tcp_segment.h
@ -0,0 +1,81 @@
+#ifndef ETHERLIB_TEST_TCP_SEGMENT_H
+#define ETHERLIB_TEST_TCP_SEGMENT_H
+
+#include <stdint.h>
+#include "../../packet_sieve.h"
+
+#define ETH_TCP_PACKET_CLASS (6)
+#define ETH_TCP_HEADER_LENGTH (20)
+
+#define TCP_MAX_OPT_VAL_LEN (8)
+
+/**
+ * TCP option kinds.
+ */
+typedef enum {
+    TCP_OPT_KIND_EOL = 0, // End of Operation List Option
+    TCP_OPT_KIND_NOP = 1, // No-Operation
+    TCP_OPT_KIND_MSS = 2  // Maximum Segment Size
+} TcpOptionKind;
+
+/**
+ * TCP options
+ */
+typedef struct TcpOption_ {
+    uint8_t kind; ///< Kind of the specific option (~type)
+    uint8_t size; ///< Size of value (NOT including 'kind' and 'length' fields) [=TCP opt length - 2]
+    struct TcpOption_ * next; ///< Next option in the list
+    uint8_t value[]; ///< Value
+} TcpOption;
+
+/**
+ * TCP segment flags
+ */
+typedef enum {
+    TCP_FLAG_FIN = 0x01,
+    TCP_FLAG_SYN = 0x02,
+    TCP_FLAG_RESET = 0x04,
+    TCP_FLAG_PUSH = 0x08,
+    TCP_FLAG_ACK = 0x10,
+    TCP_FLAG_URGENT = 0x20,
+    TCP_FLAG_ECNECHO = 0x40,
+    TCP_FLAG_CWR = 0x80,
+    TCP_FLAG_NONCE = 0x100
+} TcpFlag;
+
+/**
+ * TCP segment properties.
+ */
+typedef struct {
+    PcktPropsHeader
+    uint16_t SourcePort; ///< Source port
+    uint16_t DestinationPort; ///< Destination port
+    uint32_t SequenceNumber; ///< TCP sequence number
+    uint32_t AcknowledgementNumber; ///< TCP ack. number
+    uint8_t DataOffset; ///< Data offset in 32-bits (4 bit-long field)
+    uint16_t Flags; ///< TCP flags (9-bit, 8 + 4, but upper 3 is reserved)
+    uint16_t Window; ///< TCP window
+    uint16_t Checksum; ///< Checksum
+    uint16_t UrgentPtr; ///< Urgent pointer
+    TcpOption * options; ///< Linked list of TCP options
+} TcpProps;
+
+struct EthInterface_;
+
+/**
+ * Parse raw TCP segments.
+ * @param hdr pointer to the TCP packet header
+ * @param size total packet size
+ * @param pcktHdrLe pointer to property storage
+ * @return 0 on success or -1 on failure
+ */
+int parse_tcp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
+
+/**
+ * Insert TCP header.
+ * @param hdr space where the header is to be inserted
+ * @param headers linked list of header, top is always relevant
+ */
+void insert_tcp_header(uint8_t * hdr, const PcktHeaderElement * headers);
+
+#endif //ETHERLIB_TEST_TCP_SEGMENT_H
--- a/prefab/packet_parsers/tcp_udp_common.c
+++ b/prefab/packet_parsers/tcp_udp_common.c
@ -0,0 +1,12 @@
+#include <stdbool.h>
+
+#include "tcp_udp_common.h"
+#include "../../utils.h"
+
+uint16_t tcp_udp_checksum(const IPv4PseudoHeader *pseudoHeader, const uint8_t * hdr, uint32_t size) {
+    uint32_t sum = chksum((const uint8_t *) pseudoHeader, sizeof(IPv4PseudoHeader), true) + chksum(hdr, size, true);
+    while (sum >> 16) {
+        sum = (sum & 0xFFFF) + (sum >> 16);
+    }
+    return htonl(sum);
+}
--- a/prefab/packet_parsers/tcp_udp_common.h
+++ b/prefab/packet_parsers/tcp_udp_common.h
@ -0,0 +1,23 @@
+#ifndef ETHERLIB_TEST_TCP_UDP_COMMON_H
+#define ETHERLIB_TEST_TCP_UDP_COMMON_H
+
+#include <stdint.h>
+
+typedef struct {
+    uint32_t sourceIpAddr;
+    uint32_t destIpAddr;
+    uint8_t zero;
+    uint8_t protocol;
+    uint16_t udpLength;
+} IPv4PseudoHeader;
+
+/**
+ * Calculate TCP or UDP checksum.
+ * @param pseudoHeader pointer to IPv4 pseudo-header
+ * @param hdr pointer to data
+ * @param size size of data
+ * @return checksum
+ */
+uint16_t tcp_udp_checksum(const IPv4PseudoHeader *pseudoHeader, const uint8_t * hdr, uint32_t size);
+
+#endif //ETHERLIB_TEST_TCP_UDP_COMMON_H
--- a/prefab/packet_parsers/udp_packet.c
+++ b/prefab/packet_parsers/udp_packet.c
@ -4,25 +4,21 @@
 #include "ipv4_packet.h"
 #include "ethernet_frame.h"

+#include "tcp_udp_common.h"
+
 #define ETH_UDP_HEADER_SIZE (8)

-typedef struct {
-    uint32_t sourceIpAddr;
-    uint32_t destIpAddr;
-    uint8_t zero;
-    uint8_t protocol;
-    uint16_t udpLength;
-} UdpPseudoHeader;
-
-static uint16_t udp_checksum(const UdpPseudoHeader *pseudoHeader, const uint8_t * hdr, uint32_t size) {
-    uint32_t sum = chksum((const uint8_t *)pseudoHeader, sizeof(UdpPseudoHeader)) + chksum(hdr, size);
-    while (sum >> 16) {
-        sum = (sum & 0xFFFF) + (sum >> 16);
-    }
-    return sum;
+static bool check_udp_validity(const uint8_t * hdr, const PcktHeaderElement *pcktHdrLe) {
+    const UdpProps *udpProps = HEADER_FETCH_PROPS(UdpProps, pcktHdrLe);
+    const IPv4Props *ipProps = (IPv4Props *) &pcktHdrLe->prev->props;
+    IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)};
+    uint16_t chkSum = tcp_udp_checksum(&ph, hdr, udpProps->Length);
+    bool valid = chkSum == 0;
+    return valid;
 }

 int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
+    const uint8_t * hdrBegin = hdr;
    UdpProps *udpProps = HEADER_FETCH_PROPS(UdpProps, pcktHdrLe);
    FETCH_WORD_H2N_ADVANCE(&udpProps->SourcePort, hdr);
    FETCH_WORD_H2N_ADVANCE(&udpProps->DestinationPort, hdr);
@ -31,7 +27,7 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s

    // common fields...
    udpProps->headerSize = ETH_UDP_HEADER_SIZE;
-    udpProps->validityOK = (size == udpProps->Length); // TODO UDP checksum validation!
+    udpProps->validityOK = check_udp_validity(hdrBegin, pcktHdrLe);
    udpProps->containedPacketClass = 0;

    return udpProps->validityOK ? 0 : -1;
@ -49,8 +45,8 @@ void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {

    // calculate checksum
    const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props;
-    UdpPseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)};
-    udpProps->Checksum = udp_checksum(&ph, hdrBegin, udpProps->Length);
+    IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)};
+    udpProps->Checksum = tcp_udp_checksum(&ph, hdrBegin, udpProps->Length);
    memcpy(ChkSumPtr, &udpProps->Checksum, 2);
 }

--- a/utils.c
+++ b/utils.c
@ -60,12 +60,12 @@ uint32_t crc32(const uint8_t * data, uint32_t size) {
    return checksum;
 }

-uint16_t chksum(const uint8_t * data, uint32_t size) {
+uint16_t chksum(const uint8_t *data, uint32_t size, int swap) {
    // sum fields
    uint32_t sum = 0;
    const uint16_t *pField = (const uint16_t *) data;
-    for (uint8_t i = 0; i < (size / sizeof(uint16_t)); i++) {
-        uint16_t field = pField[i];
+    for (uint16_t i = 0; i < (size / sizeof(uint16_t)); i++) {
+        uint16_t field = swap ? htons(pField[i]) : pField[i];
        sum += field;
    }

--- a/utils.h
+++ b/utils.h
@ -44,6 +44,8 @@
 #define ASSERT_NULL(p) if ((p) == NULL) ERROR("NULL in function '%s' in file '%s' on line %d!\n", __func__, __FILE__, __LINE__)

 #define ALIGN(p,t) (((size_t)(p) + (sizeof(t) - 1)) & ~(sizeof(t) - 1))
+#define FLOOR_TO_4(x) ((x) & ~0b11)
+#define CEIL_TO_4(x) ((((x) >> 2) + (((x) & 0b11) ? 1 : 0)) << 2)

 #define FETCH_ADVANCE(dst,src,n) memcpy((dst), (src), n), (src) += n
 #define FILL_ADVANCE(dst,src,n) memcpy((dst), (src), n), (dst) += n
@ -81,6 +83,6 @@ uint32_t crc32(const uint8_t * data, uint32_t size);
 * @param size size of data block
 * @return checksum
 */
-uint16_t chksum(const uint8_t * data, uint32_t size);
+uint16_t chksum(const uint8_t *data, uint32_t size, int swap);

 #endif //ETHERLIB_UTILS_H