diff --git a/Doxyfile b/Doxyfile
index dd02eeb..0e9640e 100644
--- a/Doxyfile
+++ b/Doxyfile
@@ -58,7 +58,7 @@ PROJECT_LOGO           =
 # entered, it will be relative to the location where doxygen was started. If
 # left blank the current directory will be used.
 
-OUTPUT_DIRECTORY       = /home/epagris/VCSDEV/EtherLib/docs
+OUTPUT_DIRECTORY       = ./../docs
 
 # If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
 # directories (in 2 levels) under the output directory of each output format and
@@ -316,7 +316,7 @@ OPTIMIZE_OUTPUT_SLICE  = NO
 # Fortran (fixed format Fortran: FortranFixed, free formatted Fortran:
 # FortranFree, unknown formatted Fortran: Fortran. In the later case the parser
 # tries to guess whether the code is fixed or free formatted code, this is the
-# default for Fortran class files). For instance to make doxygen treat .inc files
+# default for Fortran type files). For instance to make doxygen treat .inc files
 # as Fortran files (default is PHP), and .f files as C (default is Fortran),
 # use: inc=Fortran f=C.
 #
@@ -385,7 +385,7 @@ SIP_SUPPORT            = NO
 # getter and setter methods for a property. Setting this option to YES will make
 # doxygen to replace the get and set methods by a property in the documentation.
 # This will only work if the methods are indeed getting or setting a simple
-# class. If this is not the case, or you want to show the methods anyway, you
+# type. If this is not the case, or you want to show the methods anyway, you
 # should set this option to NO.
 # The default value is: YES.
 
@@ -406,9 +406,9 @@ DISTRIBUTE_GROUP_DOC   = NO
 
 GROUP_NESTED_COMPOUNDS = NO
 
-# Set the SUBGROUPING tag to YES to allow class member groups of the same class
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
 # (for instance a group of public functions) to be put as a subgroup of that
-# class (e.g. under the Public Functions section). Set it to NO to prevent
+# type (e.g. under the Public Functions section). Set it to NO to prevent
 # subgrouping. Alternatively, this can be done per class using the
 # \nosubgrouping command.
 # The default value is: YES.
@@ -680,7 +680,7 @@ SORT_GROUP_NAMES       = NO
 SORT_BY_SCOPE_NAME     = NO
 
 # If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
-# class resolution of all parameters of a function it will reject a match between
+# type resolution of all parameters of a function it will reject a match between
 # the prototype and the implementation of a member function even if there is
 # only one candidate or it is obvious which candidate to choose by doing a
 # simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
@@ -864,7 +864,7 @@ WARN_LOGFILE           =
 # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
 # Note: If this tag is empty the current directory is searched.
 
-INPUT                  = /home/epagris/VCSDEV/EtherLib/etherlib
+INPUT                  = .
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
@@ -943,7 +943,7 @@ FILE_PATTERNS          = *.c \
 # be searched for input files as well.
 # The default value is: NO.
 
-RECURSIVE              = NO
+RECURSIVE              = YES
 
 # The EXCLUDE tag can be used to specify files and/or directories that should be
 # excluded from the INPUT source files. This way you can easily exclude a
@@ -1153,7 +1153,7 @@ VERBATIM_HEADERS       = YES
 # clang parser (see:
 # http://clang.llvm.org/) for more accurate parsing at the cost of reduced
 # performance. This can be particularly helpful with template rich C++ code for
-# which doxygen's built-in parser lacks the necessary class information.
+# which doxygen's built-in parser lacks the necessary type information.
 # Note: The availability of this option depends on whether or not doxygen was
 # generated with the -Duse_libclang=ON option for CMake.
 # The default value is: NO.
@@ -1840,7 +1840,7 @@ LATEX_MAKEINDEX_CMD    = makeindex
 
 COMPACT_LATEX          = NO
 
-# The PAPER_TYPE tag can be used to set the paper class that is used by the
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
 # printer.
 # Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
 # 14 inches) and executive (7.25 x 10.5 inches).
@@ -2218,7 +2218,7 @@ ENABLE_PREPROCESSING   = YES
 # The default value is: NO.
 # This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
 
-MACRO_EXPANSION        = NO
+MACRO_EXPANSION        = YES
 
 # If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
 # the macro expansion is limited to the macros specified with the PREDEFINED and
@@ -2226,7 +2226,7 @@ MACRO_EXPANSION        = NO
 # The default value is: NO.
 # This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
 
-EXPAND_ONLY_PREDEF     = YES
+EXPAND_ONLY_PREDEF     = NO
 
 # If the SEARCH_INCLUDES tag is set to YES, the include files in the
 # INCLUDE_PATH will be searched if a #include is found.
@@ -2240,7 +2240,7 @@ SEARCH_INCLUDES        = YES
 # preprocessor.
 # This tag requires that the tag SEARCH_INCLUDES is set to YES.
 
-INCLUDE_PATH           =
+INCLUDE_PATH           = .
 
 # You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
 # patterns (like *.h and *.hpp) to filter out the header-files in the
@@ -2429,7 +2429,7 @@ UML_LOOK               = NO
 # If the UML_LOOK tag is enabled, the fields and methods are shown inside the
 # class node. If there are many fields or methods and many nodes the graph may
 # become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
-# number of items for each class to make the size more manageable. Set this to 0
+# number of items for each type to make the size more manageable. Set this to 0
 # for no limit. Note that the threshold may be exceeded by 50% before the limit
 # is enforced. So when you set the threshold to 10, up to 15 fields may appear,
 # but if the number exceeds 15, the total amount of fields shown is limited to
@@ -2441,7 +2441,7 @@ UML_LIMIT_NUM_FIELDS   = 10
 
 # If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and
 # methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS
-# tag is set to YES, doxygen will add class and arguments for attributes and
+# tag is set to YES, doxygen will add type and arguments for attributes and
 # methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen
 # will not generate fields with class member information in the UML graphs. The
 # class diagrams will look similar to the default class diagrams but using UML
@@ -2453,7 +2453,7 @@ UML_LIMIT_NUM_FIELDS   = 10
 DOT_UML_DETAILS        = NO
 
 # The DOT_WRAP_THRESHOLD tag can be used to set the maximum number of characters
-# to display on a single line. If the actual line length_type exceeds this threshold
+# to display on a single line. If the actual line length exceeds this threshold
 # significantly it will wrapped across multiple lines. Some heuristics are apply
 # to avoid ugly line breaks.
 # Minimum value: 0, maximum value: 1000, default value: 17.
diff --git a/connection_block.c b/connection_block.c
new file mode 100644
index 0000000..b043c5a
--- /dev/null
+++ b/connection_block.c
@@ -0,0 +1,13 @@
+//
+// Created by epagris on 2022.11.07..
+//
+
+#include "connection_block.h"
+
+#include <stddef.h>
+
+void connb_remove(ConnBlock *connb) {
+    if (connb != NULL) {
+        packsieve_remove_layer(connb->sieveLayer);
+    }
+}
diff --git a/connection_block.h b/connection_block.h
new file mode 100644
index 0000000..65c2200
--- /dev/null
+++ b/connection_block.h
@@ -0,0 +1,23 @@
+#ifndef ETHERLIB_CONNECTION_BLOCK_H
+#define ETHERLIB_CONNECTION_BLOCK_H
+
+#include "packet_sieve.h"
+#include "eth_interface.h"
+
+struct ConnBlock_;
+
+typedef int (*ConnBlockTransmitFn)(EthInterface * intf, const uint8_t * data, uint32_t size, const struct ConnBlock_ * connBlock);
+
+typedef struct ConnBlock_ {
+    EthInterface * intf; ///< Ethernet interface
+    PcktSieveLayer * sieveLayer; ///< Sieve layer
+    ConnBlockTransmitFn transmitFn; ///< Conn-block related transmit function
+} ConnBlock;
+
+/**
+ * Remove connection block.
+ * @param connb pointer to existing connection block
+ */
+void connb_remove(ConnBlock * connb);
+
+#endif //ETHERLIB_CONNECTION_BLOCK_H
diff --git a/eth_interface.c b/eth_interface.c
new file mode 100644
index 0000000..fef4ed8
--- /dev/null
+++ b/eth_interface.c
@@ -0,0 +1,22 @@
+//
+// Created by epagris on 2022.10.20..
+//
+
+#include "eth_interface.h"
+#include "dynmem.h"
+#include "utils.h"
+
+EthInterface *ethintf_new() {
+    EthInterface * ethIntf = (EthInterface *)dynmem_alloc(sizeof(EthInterface));
+    ASSERT_NULL(ethIntf);
+    memset(&ethIntf->sieve.layer0, 0, sizeof(PcktSieveLayer));
+    return ethIntf;
+}
+
+void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt) {
+    packsieve_input(&intf->sieve, rawPckt);
+}
+
+void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt) {
+    intf->ioDef.llTx(rawPckt);
+}
diff --git a/eth_interface.h b/eth_interface.h
new file mode 100644
index 0000000..7620a37
--- /dev/null
+++ b/eth_interface.h
@@ -0,0 +1,37 @@
+#ifndef ETHERLIB_ETH_INTERFACE_H
+#define ETHERLIB_ETH_INTERFACE_H
+
+#include "packet_sieve.h"
+#include "prefab/packet_parsers/packet_parsers.h"
+
+typedef struct {
+    int (*llTx)(const RawPckt * rawPckt); ///< Function pointer to low-level transmit function
+    int (*llTxDone)(const RawPckt * rawPckt); ///< Transmission done (interrupt) callback
+    int (*llLinkChg)(int linkState); ///< Link change interrupt
+    int (*llError)(int error); ///< Low-level error interrupt
+} EthIODef;
+
+typedef struct {
+    PcktSieve sieve; ///< Packet sieve
+    EthIODef ioDef; ///< Low-level IO definitions
+    EthernetAddress mac; ///< Ethernet address
+} EthInterface;
+
+/**
+ * Allocate a new Ethernet interface
+ * @return new interface instance
+ */
+EthInterface * ethintf_new();
+
+/**
+ * Receive uncooked packet.
+ * @param rawPckt
+ */
+void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt);
+
+/**
+ * Transmit packet.
+ */
+void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt);
+
+#endif //ETHERLIB_ETH_INTERFACE_H
diff --git a/global_state.c b/global_state.c
index aa40c09..e9b062c 100644
--- a/global_state.c
+++ b/global_state.c
@@ -3,6 +3,7 @@
 #include "dynmem.h"
 
 #include "prefab/prefab.h"
+#include "packet_sieve.h"
 
 EthState gEthState;
 
@@ -13,69 +14,27 @@ static void register_packet_parsers() {
     cdesc.containerClass = 0;
     cdesc.procFun = parse_ethernet;
     cdesc.propertySize = sizeof(EthernetProps);
-    packreg_add(E.pcktReg, &cdesc);
+    packreg_add_class(E.pcktReg, &cdesc);
 
     // IPv4 packet parser
     cdesc.class = ETH_IPv4_PACKET_CLASS;
     cdesc.containerClass = 0;
     cdesc.procFun = parse_ipv4;
     cdesc.propertySize = sizeof(IPv4Props);
-    packreg_add(E.pcktReg, &cdesc);
+    packreg_add_class(E.pcktReg, &cdesc);
 
     // UDP packet parser
     cdesc.class = ETH_UDP_PACKET_CLASS;
     cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
     cdesc.procFun = parse_udp;
-    cdesc.propertySize = sizeof(UDPProps);
-    packreg_add(E.pcktReg, &cdesc);
+    cdesc.propertySize = sizeof(UdpProps);
+    packreg_add_class(E.pcktReg, &cdesc);
 }
 
 void ethlib_init() {
     dynmem_init(); // initialize dynamic memory subsystem
     E.pcktReg = packreg_new(); // create new packet registry
     register_packet_parsers(); // register packet parsers
-}
 
-typedef struct PcktHeaderElement_ {
-    struct PcktHeaderElement_ * next, * prev; ///< Next and previous header in the linked list
-    PcktProps props; ///< Properties (allocated to appropriate size)
-} PcktHeaderElement;
-
-#define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *))
-
-void process_raw_packet(const uint8_t * data, uint32_t size) {
-    uint16_t ownClass = 0, containerClass = 0; // Ethernet...
-    uint16_t offset = 0;
-    PcktHeaderElement * lastHeader = NULL;
-    do {
-        // get packet descriptor
-        const PcktClassDesc * cdesc = packreg_get_by_class(E.pcktReg, ownClass, containerClass);
-        if (cdesc == NULL) {
-            break;
-        }
-
-        // allocate property object
-        PcktHeaderElement * header = (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize);
-        header->props.ownPacketClass = ownClass;
-        header->props.propSize = cdesc->propertySize;
-        header->prev = lastHeader;
-        if (lastHeader) {
-            lastHeader->next = header;
-        }
-
-        // call parsing function
-        cdesc->procFun(data + offset, size - offset, &header->props);
-        uint16_t containedClass = header->props.containedPacketClass;
-        if (containedClass != 0) {
-            containerClass = ownClass;
-            offset += header->props.headerSize;
-            //dynmem_free(props);
-        }
-        ownClass = containedClass;
-        lastHeader = header;
-    } while (ownClass != 0);
-
-    // TODO process innermost packet
-
-    return;
+    E.ethIntf = ethintf_new(); // create new Ethernet interface
 }
\ No newline at end of file
diff --git a/global_state.h b/global_state.h
index f9475bf..68c3f58 100644
--- a/global_state.h
+++ b/global_state.h
@@ -3,6 +3,8 @@
 
 #include "memory_pool.h"
 #include "packet_registry.h"
+#include "packet_sieve.h"
+#include "eth_interface.h"
 
 /**
  * Global EtherLib state.
@@ -10,6 +12,7 @@
 typedef struct {
     MP * mp; ///< Memory pool for dynamic allocations
     PcktRegistry * pcktReg; ///< Packet registry
+    EthInterface * ethIntf; ///< Array of Ethernet interfaces
 } EthState;
 
 extern EthState gEthState;
diff --git a/memory_pool.c b/memory_pool.c
index e3ed283..d12a4c2 100644
--- a/memory_pool.c
+++ b/memory_pool.c
@@ -140,5 +140,7 @@ void mp_report(MP *mp) {
         recIter--;
         bi++;
     }
-    INFO("\n");
+    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);
 }
\ No newline at end of file
diff --git a/common_types.c b/packet.c
similarity index 63%
rename from common_types.c
rename to packet.c
index 2b15f59..f5f02ec 100644
--- a/common_types.c
+++ b/packet.c
@@ -2,4 +2,4 @@
 // Created by epagris on 2022.10.17..
 //
 
-#include "common_types.h"
+#include "packet.h"
diff --git a/common_types.h b/packet.h
similarity index 57%
rename from common_types.h
rename to packet.h
index 50e5c33..7a5cb40 100644
--- a/common_types.h
+++ b/packet.h
@@ -1,5 +1,5 @@
-#ifndef ETHERLIB_COMMON_TYPES_H
-#define ETHERLIB_COMMON_TYPES_H
+#ifndef ETHERLIB_PACKET_H
+#define ETHERLIB_PACKET_H
 
 #include <stdint.h>
 
@@ -10,22 +10,26 @@
  typedef struct {
      uint8_t * payload; ///> Pointer to raw packet payload.
      uint32_t size; ///> Raw packet size.
+     uint32_t time_s; ///> Timestamp seconds field
+     uint32_t time_ns; ///> Timestamp nanoseconds field
  } RawPckt;
 
+struct PcktHeaderElement_;
+
 /**
- * \struct Pckt
- * \brief Generic packet
+ * @struct Pckt
+ * @brief Generic packet
  *
  * Fields have been reordered so that fields are aligned.
  */
 typedef struct {
     uint64_t time_s; ///< Timestamp seconds part.
     uint32_t time_ns; ///< Timestamp nanoseconds part.
-    uint8_t * header; ///< Pointer to packet header. Points to the outmost header (e.g. Ethernet-header in case of a UDP-packet)
+    const struct PcktHeaderElement_ * header; ///< Pointer to packet header. Points to the innermost header
     uint8_t * payload; ///< Pointer to (innermost) payload.
     uint16_t headerSize; ///< Packet header size in bytes.
     uint16_t payloadSize; ///< Payload size in bytes.
-    uint16_t type; ///< Packet class indicator (e.g. UDP, TCP, IPv4 etc.)
+    //uint16_t type; ///< Packet class indicator (e.g. UDP, TCP, IPv4 etc.)
 } Pckt;
 
-#endif //ETHERLIB_COMMON_TYPES_H
+#endif //ETHERLIB_PACKET_H
diff --git a/packet_input.c b/packet_input.c
deleted file mode 100644
index 7f9fdcd..0000000
--- a/packet_input.c
+++ /dev/null
@@ -1,5 +0,0 @@
-//
-// Created by epagris on 2022.10.20..
-//
-
-#include "packet_input.h"
diff --git a/packet_input.h b/packet_input.h
deleted file mode 100644
index e73619d..0000000
--- a/packet_input.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef ETHERLIB_PACKET_INPUT_H
-#define ETHERLIB_PACKET_INPUT_H
-
-void input_packet();
-
-#endif //ETHERLIB_PACKET_INPUT_H
diff --git a/packet_registry.c b/packet_registry.c
index 4267d93..3facbe5 100644
--- a/packet_registry.c
+++ b/packet_registry.c
@@ -18,7 +18,7 @@ PcktRegistry * packreg_new() {
     return packReg;
 }
 
-void packreg_add(PcktRegistry * packReg, const PcktClassDesc * classDesc) {
+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);
diff --git a/packet_registry.h b/packet_registry.h
index 0196f96..7a0bed8 100644
--- a/packet_registry.h
+++ b/packet_registry.h
@@ -21,11 +21,12 @@ typedef uint8_t bool8_t;
 struct PcktClassDesc_;
 
 #define PcktPropsHeader \
-    uint16_t propSize; /* Size of this property object */ \
-    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 */                                           \
-    bool8_t validityOK; /*< Indicates that checksum is OK. */ \
+    uint16_t propSize; /**< Size of this property object */ \
+    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 */ \
+    bool8_t validityOK; /**< Indicates that checksum is OK. */ \
 
 
 typedef struct {
@@ -78,7 +79,7 @@ PcktRegistry *packreg_new();
  * Add new packet class. (e.g. UDP, IP...)
  * @param classDesc Packet class descriptor
  */
-void packreg_add(PcktRegistry *packReg, const PcktClassDesc *classDesc);
+void packreg_add_class(PcktRegistry *packReg, const PcktClassDesc *classDesc);
 
 /**
  * Get packet descriptor by own class and container class.
@@ -89,4 +90,6 @@ void packreg_add(PcktRegistry *packReg, const PcktClassDesc *classDesc);
  */
 const PcktClassDesc *packreg_get_by_class(const PcktRegistry *packReg, uint16_t ownClass, uint16_t containerClass);
 
+
+
 #endif //ETHERLIB_PACKET_REGISTRY_H
\ No newline at end of file
diff --git a/packet_sieve.c b/packet_sieve.c
new file mode 100644
index 0000000..6743eca
--- /dev/null
+++ b/packet_sieve.c
@@ -0,0 +1,190 @@
+//
+// Created by epagris on 2022.11.06..
+//
+
+#include <stddef.h>
+#include "packet_sieve.h"
+#include "global_state.h"
+#include "dynmem.h"
+#include "utils.h"
+
+bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilterCondition * c2) {
+    return !memcmp(c1, c2, sizeof(PcktSieveFilterCondition));
+}
+
+void packfiltcond_zero(PcktSieveFilterCondition * cond) {
+    memset(cond, 0, sizeof(PcktSieveFilterCondition));
+}
+
+PcktSieve *packsieve_new() {
+    PcktSieve *sieve = (PcktSieve *) dynmem_alloc(sizeof(PcktSieve));
+    ASSERT_NULL(sieve);
+    memset(&sieve->layer0, 0, sizeof(PcktSieveLayer)); // clear layer0 data
+    return sieve;
+}
+
+void packsieve_input(const PcktSieve *sieve, const RawPckt * rawPckt) {
+    // extract fields
+    uint8_t * data = rawPckt->payload;
+    uint32_t size = rawPckt->size;
+
+    // process payload, fetch packet class etc.
+    uint16_t ownClass = 0, containerClass = 0; // Ethernet...
+    uint16_t offset = 0;
+    PcktHeaderElement *lastHeader = NULL, *outermostHeader = NULL;
+
+    do {
+        // get packet descriptor
+        const PcktClassDesc *cdesc = packreg_get_by_class(E.pcktReg, ownClass, containerClass);
+        if (cdesc == NULL) {
+            break;
+        }
+
+        // allocate property object
+        PcktHeaderElement *header = (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize);
+        header->props.ownPacketClass = ownClass;
+        header->props.propSize = cdesc->propertySize;
+        header->prev = lastHeader;
+        if (lastHeader) {
+            lastHeader->next = header;
+        }
+        if (outermostHeader == NULL) {
+            outermostHeader = lastHeader;
+        }
+
+        // call parsing function
+        cdesc->procFun(data + offset, size - offset, &header->props);
+        uint16_t containedClass = header->props.containedPacketClass;
+        if (containedClass != 0) {
+            containerClass = ownClass;
+            //dynmem_free(props);
+        }
+        offset += header->props.headerSize;
+        header->props.accumulatedOffset = offset;
+
+        ownClass = containedClass;
+        lastHeader = header;
+    } while (ownClass != 0);
+
+    // ------------------------------------
+
+    Pckt packet;
+    packet.time_s = rawPckt->time_s;
+    packet.time_ns = rawPckt->time_ns;
+
+    // lookup headers in the sieve
+    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;
+        found = false;
+        while (nodeIter && !found) {
+            found |= nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props);
+            if (found) {
+                layer = nodeIter; // advance in the sieve tree
+                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;
+                    packet.payload = data + offset;
+                    packet.headerSize = offset;
+                    packet.payloadSize = size - offset;
+                    layer->cbFn(&packet);
+                }
+                headerIter = containedHeader;
+            } else {
+                nodeIter = nodeIter->next; // advance on linked list and countinue search
+            }
+        }
+    }
+
+    // if there are no more sieve layers (cannot process headers further) BUT
+    // meaningful headers have been left unprocessed
+//    if (layer->next == NULL && headerIter) {
+//        INFO("Packet headers not fully processed!\n");
+//    }
+
+    return;
+}
+
+PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass) {
+    // search for matching layer
+    PcktSieveLayer * nodeIter = parent->nodes;
+    bool alreadyExists = false;
+    while (nodeIter != NULL && !alreadyExists) {
+        if (packfiltcond_cmp(&nodeIter->filtCond, filtCond) && (nodeIter->filtFn == filtFn)) { // if matching...
+            alreadyExists = true;
+        } else {
+            nodeIter = nodeIter->next;
+        }
+    }
+
+    // if found, then return with the pointer to the existing layer
+    if (alreadyExists) {
+        return nodeIter; // OK
+    } else { // if allocation of a new layer is required
+        PcktSieveLayer *layer = (PcktSieveLayer *) dynmem_alloc(sizeof(PcktSieveLayer));
+        ASSERT_NULL(layer);
+
+        PcktSieveLayer *oldListFirst = parent->nodes;
+        parent->nodes = layer; // replace first element (it's the fastest way of new element insertion, since element position does not carry any meaning this case)
+        layer->packetClass = pcktClass;
+        layer->parent = parent;
+        layer->prev = NULL;
+        layer->next = oldListFirst;
+        if (oldListFirst != NULL) {
+            layer->prev = layer;
+        }
+        layer->nodes = NULL;
+        layer->filtCond = *filtCond;
+        layer->filtFn = filtFn;
+        layer->cbFn = cbFn;
+        layer->tag = tag;
+        return 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;
+    while (layer != NULL && layer->nodes == NULL) {
+        parent = layer->parent; // store parent
+
+        // chain out our layer
+        if (layer->next == NULL && layer->prev == NULL) { // we are the only subnode
+            parent->nodes = NULL;
+        } else { // there are multiple subnodes, just chain us out
+            if (layer->next != NULL) {
+                layer->next->prev = layer->prev;
+            }
+            if (layer->prev != NULL) {
+                layer->prev->next = layer->next;
+            }
+            if (parent->nodes == layer) {
+                parent->nodes = layer->next;
+            }
+        }
+
+        dynmem_free(layer); // deallocate layer
+        layer = parent; // advance in the tree
+    }
+
+    return layer == NULL;
+}
+
+#define ETH_SIEVE_LAYER_INDENT_PER_LEVEL (4)
+
+void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
+    INFO("%*c\\--|%d|---\n", indent, ' ', layer->packetClass);
+    const PcktSieveLayer * nodeIter = layer->nodes;
+    while(nodeIter) {
+        packsieve_report(nodeIter, indent + ETH_SIEVE_LAYER_INDENT_PER_LEVEL);
+        nodeIter = nodeIter->next;
+    }
+}
diff --git a/packet_sieve.h b/packet_sieve.h
new file mode 100644
index 0000000..f2565d7
--- /dev/null
+++ b/packet_sieve.h
@@ -0,0 +1,104 @@
+#ifndef ETHERLIB_PACKET_SIEVE_H
+#define ETHERLIB_PACKET_SIEVE_H
+
+#include <stdbool.h>
+
+#include "packet_registry.h"
+#include "packet.h"
+
+typedef struct PcktHeaderElement_ {
+    struct PcktHeaderElement_ * next, * prev; ///< Next and previous header in the linked list
+    PcktProps props; ///< Properties (allocated to appropriate size)
+} PcktHeaderElement;
+
+#define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *))
+
+/**
+ * @union PcktSieveFilterCondition
+ * Union for storing filter conditions.
+ */
+typedef union {
+    uint16_t uw[8];
+    uint32_t u[4];
+    uint64_t lu[2];
+    int16_t sw[8];
+    int32_t i[4];
+    int64_t li[2];
+    uint8_t d[16];
+} PcktSieveFilterCondition;
+
+/**
+ * Function for comparing filter conditions.
+ * @param c1 condition 1
+ * @param c2 condition 2
+ * @return true if cond1 matches cond2, otherwise false
+ */
+bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilterCondition * c2);
+
+/**
+ * Clear packet filter condition structure.
+ * @param cond pointer to existing filter condition structure
+ */
+void packfiltcond_zero(PcktSieveFilterCondition * cond);
+
+typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps);
+
+struct PcktSieveLayer_;
+
+typedef int (*SieveCallBackFn)(const Pckt * pckt);
+
+typedef union {
+    void * p;
+    uint32_t u;
+} PcktSieveLayerTag;
+
+typedef struct PcktSieveLayer_ {
+    uint16_t packetClass; ///< Packet class (e.g. IP)
+    PcktSieveFilterCondition filtCond; ///< Filter condition, arbitrary type (e.g. destination IP-address)
+    SieveFilterFn filtFn; ///< Filter function pointer
+    SieveCallBackFn cbFn; ///< Associated callback function
+    PcktSieveLayerTag tag; ///< Layer tag (arbitrary information)
+    struct PcktSieveLayer_ * parent; ///< Pointer to parent node in the sieve tree
+    struct PcktSieveLayer_ * next, * prev; ///< Next and previous sieve layer on the same level
+    struct PcktSieveLayer_ * nodes; ///< Subnodes on the sieve tree
+} PcktSieveLayer;
+
+typedef struct {
+    PcktSieveLayer layer0; ///< Top of sieve tree
+} PcktSieve;
+
+/**
+ * Create new packet sieve.
+ * @return pointer to packet sieve object or NULL on failure
+ */
+PcktSieve * packsieve_new();
+
+/**
+ * Process packet with packet sieve.
+ * @param bottomLayer
+ * @param data
+ * @param size
+ */
+void packsieve_input(const PcktSieve * sieve, const RawPckt * rawPckt);
+
+/**
+ * Create a new sieve filter layer.
+ * @param parent parent layer
+ * @return pointer to new layer object or NULL on failure
+ */
+PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass);
+
+/**
+ * Remove sieve layer from packet sieve.
+ * @param layer Layer to remove and deallocate
+ */
+bool packsieve_remove_layer(PcktSieveLayer * layer);
+
+/**
+ * Display layer on terminal.
+ * @param layer pointer to existing layer
+ * @param indent sub-tree indentation
+ */
+void packsieve_report(const PcktSieveLayer *layer, uint32_t indent);
+
+#endif //ETHERLIB_PACKET_SIEVE_H
diff --git a/prefab/conn_blocks/ipv4_connblock.c b/prefab/conn_blocks/ipv4_connblock.c
new file mode 100644
index 0000000..2a2263e
--- /dev/null
+++ b/prefab/conn_blocks/ipv4_connblock.c
@@ -0,0 +1,27 @@
+//
+// Created by epagris on 2022.11.07..
+//
+
+#include "ipv4_connblock.h"
+
+#include "../packet_parsers/packet_parsers.h"
+#include "../../utils.h"
+
+static bool filtIPv4(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
+    EthernetProps * etherProps = (EthernetProps *) contProps;
+    IPv4Props * ipProps = (IPv4Props *) ownProps;
+
+    return etherProps->length_type == ETH_IPv4_PACKET_CLASS && filtCond->u[0] == ipProps->DestIPAddr;
+}
+
+ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackFn cbFn) {
+    ConnBlock connb;
+    PcktSieveFilterCondition filtCond;
+    packfiltcond_zero(&filtCond);
+    filtCond.u[0] = ipAddr;
+    PcktSieveLayerTag tag;
+    tag.u = 0;
+    connb.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, filtIPv4, cbFn, tag, ETH_IPv4_PACKET_CLASS);
+    ASSERT_NULL(connb.sieveLayer);
+    return connb;
+}
diff --git a/prefab/conn_blocks/ipv4_connblock.h b/prefab/conn_blocks/ipv4_connblock.h
new file mode 100644
index 0000000..f69e039
--- /dev/null
+++ b/prefab/conn_blocks/ipv4_connblock.h
@@ -0,0 +1,29 @@
+#ifndef ETHERLIB_IPV4_CONNBLOCK_H
+#define ETHERLIB_IPV4_CONNBLOCK_H
+
+#include <stdint.h>
+#include "../../connection_block.h"
+#include "../../eth_interface.h"
+
+typedef uint32_t ip4_addr;
+
+/**
+ * Create new IPv4 connection block.
+ * @param intf interface to the connection block will be associated
+ * @param ipAddr address of the connection block
+ * @param cbFn callback function fired when connection block receives data
+ * @return instance of a new connection block
+ */
+ConnBlock ipv4_new_connblock(EthInterface * intf, ip4_addr ipAddr, SieveCallBackFn cbFn);
+
+/**
+ * Send IPv4 packet to specified IPv4 address.
+ * @param intf interface we will use
+ * @param addr address to send to
+ * @param data data to be transmitted
+ * @param size data size
+ * @param headers linked list of headers, always list top contains relevant info
+ */
+void ipv4_send(EthInterface * intf, ip4_addr addr, const uint8_t * data, uint32_t size, const PcktHeaderElement * headers);
+
+#endif //ETHERLIB_IPV4_CONNBLOCK_H
diff --git a/prefab/conn_blocks/udp_connblock.c b/prefab/conn_blocks/udp_connblock.c
new file mode 100644
index 0000000..1ad997a
--- /dev/null
+++ b/prefab/conn_blocks/udp_connblock.c
@@ -0,0 +1,56 @@
+//
+// Created by epagris on 2022.11.07..
+//
+
+#include "udp_connblock.h"
+
+#include <stddef.h>
+
+#include "../packet_parsers/packet_parsers.h"
+#include "../../utils.h"
+#include "../../dynmem.h"
+
+static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
+    IPv4Props * ipProps = (IPv4Props *) contProps;
+    UdpProps * udpProps = (UdpProps *) ownProps;
+
+    return ipProps->Protocol == ETH_UDP_PACKET_CLASS && filtCond->uw[0] == udpProps->DestinationPort;
+}
+
+ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) {
+    ConnBlock udpConnB;
+    ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block
+
+    PcktSieveFilterCondition filtCond;
+    packfiltcond_zero(&filtCond);
+    filtCond.uw[0] = port;
+    PcktSieveLayerTag tag;
+    tag.u = 0;
+    udpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, filtUdp, cbFn, tag, ETH_UDP_PACKET_CLASS);
+    ASSERT_NULL(udpConnB.sieveLayer);
+    return udpConnB;
+}
+
+#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
+#define HEADER_FETCH_PROPS(T,h) (T *)(&h->props)
+
+int udp_send(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_t size) {
+    // allocate headers
+    PcktHeaderElement * udpHeader = ALLOC_HEADER_ELEMENT(UdpProps);
+    PcktHeaderElement * ipHeader = ALLOC_HEADER_ELEMENT(IPv4Props);
+    PcktHeaderElement * ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
+    udpHeader->next = NULL;
+    udpHeader->prev = ipHeader;
+    ipHeader->next = udpHeader;
+    ipHeader->prev = ethHeader;
+    ethHeader->next = ipHeader;
+    ethHeader->prev = NULL;
+
+    UdpProps * udpProps = HEADER_FETCH_PROPS(UdpProps, udpHeader);
+    IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, udpHeader);
+    EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, udpHeader);
+
+    udpProps->Length = size;
+
+    return 0;
+}
diff --git a/prefab/conn_blocks/udp_connblock.h b/prefab/conn_blocks/udp_connblock.h
new file mode 100644
index 0000000..a88d9b2
--- /dev/null
+++ b/prefab/conn_blocks/udp_connblock.h
@@ -0,0 +1,24 @@
+#ifndef ETHERLIB_UDP_CONNBLOCK_H
+#define ETHERLIB_UDP_CONNBLOCK_H
+
+#include <stdint.h>
+#include "../../connection_block.h"
+#include "../../eth_interface.h"
+#include "ipv4_connblock.h"
+
+/**
+ * Create new IPv4 connection block.
+ * @param intf interface to the connection block will be associated
+ * @param ipAddr address of the connection block
+ * @param port port of the connection block
+ * @param cbFn callback function fired when connection block receives data or some event occurs
+ * @return instance of a new connection block
+ */
+ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn);
+
+/**
+ * UDP transmit callback.
+ */
+int udp_send(const struct ConnBlock_ * connBlock, const uint8_t * data, uint32_t size);
+
+#endif //ETHERLIB_UDP_CONNBLOCK_H
diff --git a/prefab/packet_parsers/ethernet_frame.c b/prefab/packet_parsers/ethernet_frame.c
index ef5f7e1..959621a 100644
--- a/prefab/packet_parsers/ethernet_frame.c
+++ b/prefab/packet_parsers/ethernet_frame.c
@@ -5,16 +5,16 @@
 #include <memory.h>
 #include <stdbool.h>
 #include "ethernet_frame.h"
-#include "etherlib/utils.h"
+#include "../../utils.h"
 
 #define ETH_ETHERTYPE_LENGTH_THRESHOLD (1500)
 #define ETH_ETHERNET_HEADER_SIZE (14)
 
 int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktProps * props) {
     EthernetProps * frameProps = (EthernetProps *)props;
-    COPY_ADVANCE(frameProps->destAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
-    COPY_ADVANCE(frameProps->sourceAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
-    COPY_WORD_H2N_ADVANCE(&frameProps->length_type, hdr); // copy length/type field
+    FETCH_ADVANCE(frameProps->destAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
+    FETCH_ADVANCE(frameProps->sourceAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
+    FETCH_WORD_H2N_ADVANCE(&frameProps->length_type, hdr); // copy length/type field
 
     // fill-in packet property header fields
     frameProps->validityOK = true;
@@ -23,4 +23,11 @@ int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktProps * props) {
     frameProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
 
     return frameProps->containedPacketClass;
-}
\ No newline at end of file
+}
+
+void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+    EthernetProps * ethProps = (EthernetProps *) &headers->props;
+    FILL_ADVANCE(hdr, ethProps->destAddr, ETH_HW_ADDR_LEN);
+    FILL_ADVANCE(hdr, ethProps->sourceAddr, ETH_HW_ADDR_LEN);
+    FILL_WORD_H2N_ADVANCE(hdr, ethProps->length_type);
+}
diff --git a/prefab/packet_parsers/ethernet_frame.h b/prefab/packet_parsers/ethernet_frame.h
index 2e264b7..ef07732 100644
--- a/prefab/packet_parsers/ethernet_frame.h
+++ b/prefab/packet_parsers/ethernet_frame.h
@@ -6,19 +6,23 @@
 #define ETHERLIB_ETHERNET_FRAME_H
 
 #include <stdint.h>
-#include "etherlib/packet_registry.h"
+#include "../../packet_registry.h"
+#include "../../packet_sieve.h"
 
 #define ETH_HW_ADDR_LEN (6)
+#define ETH_ETHERNET_HEADER_SIZE (14)
+
+typedef uint8_t EthernetAddress[ETH_HW_ADDR_LEN];
 
 /**
  * @struct EthernetProps
  * Ethernet frame properties
  */
 typedef struct {
-    PcktPropsHeader
-    uint8_t destAddr[ETH_HW_ADDR_LEN]; ///< destination address
-    uint8_t sourceAddr[ETH_HW_ADDR_LEN]; ///< source address
-    uint16_t length_type; /// < frame length_type
+    PcktPropsHeader;
+    EthernetAddress destAddr[ETH_HW_ADDR_LEN]; ///< destination address
+    EthernetAddress sourceAddr[ETH_HW_ADDR_LEN]; ///< source address
+    uint16_t length_type; ///< frame length_type
 } EthernetProps;
 
 /**
@@ -30,4 +34,11 @@ typedef struct {
  */
 int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktProps * props);
 
+/**
+ * Insert Ethernet header to specified address.
+ * @param hdr space where the header is to be inserted
+ * @param headers linked list of header, top is always relevant
+ */
+void insert_ethernet_header(uint8_t * hdr, const PcktHeaderElement * headers);
+
 #endif //ETHERLIB_ETHERNET_FRAME_H
diff --git a/prefab/packet_parsers/ipv4_packet.c b/prefab/packet_parsers/ipv4_packet.c
index 5e6c8b6..0cd9f9f 100644
--- a/prefab/packet_parsers/ipv4_packet.c
+++ b/prefab/packet_parsers/ipv4_packet.c
@@ -4,7 +4,7 @@
 
 #include <stdbool.h>
 #include "ipv4_packet.h"
-#include "etherlib/utils.h"
+#include "../../utils.h"
 
 static uint16_t ip_checksum(const IPv4Props * ipProps) {
     // sum fields
@@ -44,21 +44,23 @@ static bool check_ipv4_validity(const IPv4Props * ipProps) {
 int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props) {
     IPv4Props * ipProps = (IPv4Props *)props;
     uint8_t version_length;
-    COPY_ADVANCE(&version_length, hdr, 1);
+    FETCH_ADVANCE(&version_length, hdr, 1);
     ipProps->Version = (version_length) >> 4;
     ipProps->IHL = (version_length) & 0x0F;
-    COPY_ADVANCE(&ipProps->DSF, hdr, 1);
-    COPY_WORD_H2N_ADVANCE(&ipProps->TotalLength, hdr);
-    COPY_WORD_H2N_ADVANCE(&ipProps->Identification, hdr);
+    FETCH_ADVANCE(&ipProps->DSF, hdr, 1);
+    FETCH_WORD_H2N_ADVANCE(&ipProps->TotalLength, hdr);
+    FETCH_WORD_H2N_ADVANCE(&ipProps->Identification, hdr);
     uint16_t flags_fragOffset;
-    COPY_WORD_H2N_ADVANCE(&flags_fragOffset, hdr);
+    FETCH_WORD_H2N_ADVANCE(&flags_fragOffset, hdr);
     ipProps->Flags = (flags_fragOffset >> 13) & 0x07;
     ipProps->FragmentOffset = (flags_fragOffset & ~(0x07 << 13));
-    COPY_ADVANCE(&ipProps->TTL, hdr, 1);
-    COPY_ADVANCE(&ipProps->Protocol, hdr, 1);
-    COPY_WORD_H2N_ADVANCE(&ipProps->HeaderChecksum, hdr);
-    COPY_DWORD_H2N_ADVANCE(&ipProps->SourceIPAddr, hdr);
-    COPY_DWORD_H2N_ADVANCE(&ipProps->DestIPAddr, hdr);
+    FETCH_ADVANCE(&ipProps->TTL, hdr, 1);
+    FETCH_ADVANCE(&ipProps->Protocol, hdr, 1);
+    FETCH_WORD_H2N_ADVANCE(&ipProps->HeaderChecksum, hdr);
+    //FETCH_DWORD_H2N_ADVANCE(&ipProps->SourceIPAddr, hdr);
+    //FETCH_DWORD_H2N_ADVANCE(&ipProps->DestIPAddr, hdr);
+    FETCH_ADVANCE(&ipProps->SourceIPAddr, hdr, 4);
+    FETCH_ADVANCE(&ipProps->DestIPAddr, hdr, 4);
 
     // fill-in common packet header fields
     ipProps->validityOK = check_ipv4_validity(ipProps);
@@ -66,4 +68,20 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props) {
     ipProps->headerSize = ETH_IP_HEADER_LENGTH;
 
     return ipProps->validityOK ? ipProps->Protocol : -1;
+}
+
+void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+    IPv4Props * ipProps = (IPv4Props *)&headers->props;
+    uint8_t version_length = (ipProps->Version << 4) | (ipProps->IHL & 0x0F);
+    FILL_ADVANCE(hdr, &version_length, 1);
+    FILL_ADVANCE(hdr, &ipProps->DSF, 1);
+    FILL_WORD_H2N_ADVANCE(hdr, ipProps->TotalLength);
+    FILL_WORD_H2N_ADVANCE(hdr, ipProps->Identification);
+    uint16_t flags_fragOffset = ((ipProps->Flags & 0x07) << 13) | (flags_fragOffset & ~(0x07 << 13));
+    FILL_ADVANCE(hdr, &ipProps->TTL, 1);
+    FILL_ADVANCE(hdr, &ipProps->Protocol, 1);
+    uint16_t checksum = ip_checksum(ipProps);
+    FILL_WORD_H2N_ADVANCE(hdr, checksum);
+    FILL_ADVANCE(hdr, &ipProps->SourceIPAddr, 4);
+    FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);
 }
\ No newline at end of file
diff --git a/prefab/packet_parsers/ipv4_packet.h b/prefab/packet_parsers/ipv4_packet.h
index a84ca71..c30eca4 100644
--- a/prefab/packet_parsers/ipv4_packet.h
+++ b/prefab/packet_parsers/ipv4_packet.h
@@ -2,11 +2,14 @@
 #define ETHERLIB_IPV4_PACKET_H
 
 #include <stdint.h>
-#include "etherlib/packet_registry.h"
+#include "../../packet_registry.h"
+#include "../../packet_sieve.h"
 
 #define ETH_IPv4_PACKET_CLASS (0x0800)
 
-// IPv4 fejlécet leíró struktúra
+/**
+ * IPv4 header structure
+ */
 typedef struct {
     PcktPropsHeader
 
@@ -22,7 +25,7 @@ typedef struct {
     uint16_t FragmentOffset; ///< packet fragmentation offset
 
     // 3. 32-bit
-    uint8_t TTL; // Time-To-Live
+    uint8_t TTL; ///< Time-To-Live
     uint8_t Protocol; ///< Protocol
     uint16_t HeaderChecksum; ///< header IP-checksum
 
@@ -40,4 +43,11 @@ typedef struct {
  */
 int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props);
 
+/**
+ * Insert IPv4 header.
+ * @param hdr space where the header is to be inserted
+ * @param headers linked list of header, top is always relevant
+ */
+void insert_ipv4_header(uint8_t * hdr, const PcktHeaderElement * headers);
+
 #endif //ETHERLIB_IPV4_PACKET_H
diff --git a/prefab/packet_parsers/udp_packet.c b/prefab/packet_parsers/udp_packet.c
index 9cf7d77..35715d9 100644
--- a/prefab/packet_parsers/udp_packet.c
+++ b/prefab/packet_parsers/udp_packet.c
@@ -1,15 +1,46 @@
 #include <stdbool.h>
 #include "udp_packet.h"
-#include "etherlib/utils.h"
+#include "../../utils.h"
+#include "ipv4_packet.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 * headerPayload, uint32_t headerPayloadSize) {
+    uint16_t chksum = 0;
+    uint16_t i;
+
+    // pseudoheader
+    for (i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) {
+        chksum += ~((uint16_t *)pseudoHeader)[i];
+    }
+
+    // UDP header and payload
+    for (i = 0; i < headerPayloadSize / 2; i++) {
+        if (i != 3) { // skip Checksum field
+            chksum += ~((uint16_t *) headerPayload)[i];
+        }
+    }
+
+    // pad if packet size is odd
+    chksum += ~((uint16_t)headerPayload[headerPayloadSize - 1] << 8);
+
+    return chksum;
+}
+
 int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props) {
-    UDPProps * udpProps = (UDPProps *)props;
-    COPY_WORD_H2N_ADVANCE(&udpProps->SourcePort, hdr);
-    COPY_WORD_H2N_ADVANCE(&udpProps->DestinationPort, hdr);
-    COPY_WORD_H2N_ADVANCE(&udpProps->Length, hdr);
-    COPY_WORD_H2N_ADVANCE(&udpProps->Checksum, hdr);
+    UdpProps * udpProps = (UdpProps *)props;
+    FETCH_WORD_H2N_ADVANCE(&udpProps->SourcePort, hdr);
+    FETCH_WORD_H2N_ADVANCE(&udpProps->DestinationPort, hdr);
+    FETCH_WORD_H2N_ADVANCE(&udpProps->Length, hdr);
+    FETCH_WORD_H2N_ADVANCE(&udpProps->Checksum, hdr);
 
     // common fields...
     udpProps->headerSize = ETH_UDP_HEADER_SIZE;
@@ -18,3 +49,19 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props) {
 
     return udpProps->validityOK ? 0 : -1;
 }
+
+void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+    UdpProps * udpProps = (UdpProps *) &headers->props;
+    FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort);
+    FILL_WORD_H2N_ADVANCE(hdr, udpProps->DestinationPort);
+    FILL_WORD_H2N_ADVANCE(hdr, udpProps->Length);
+
+    // calculate checksum
+    const IPv4Props * ipProps = (IPv4Props *)&headers->prev->props;
+    UdpPseudoHeader ph = { ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, udpProps->Length };
+    udpProps->Checksum = udp_checksum(&ph, hdr, udpProps->Length);
+    FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
+}
+
+
+
diff --git a/prefab/packet_parsers/udp_packet.h b/prefab/packet_parsers/udp_packet.h
index bcab46f..f230bb8 100644
--- a/prefab/packet_parsers/udp_packet.h
+++ b/prefab/packet_parsers/udp_packet.h
@@ -2,12 +2,13 @@
 #define ETHERLIB_UDP_PACKET_H
 
 #include <stdint.h>
-#include "etherlib/packet_registry.h"
+#include "../../packet_registry.h"
+#include "../../packet_sieve.h"
 
 #define ETH_UDP_PACKET_CLASS (17)
 
 /**
- * @struct UDPProps
+ * @struct UdpProps
  * UDP packet properties.
  */
 typedef struct {
@@ -17,7 +18,7 @@ typedef struct {
     uint16_t DestinationPort; ///> destination port
     uint16_t Length; ///> packet size
     uint16_t Checksum; ///> UDP checksum
-} UDPProps;
+} UdpProps;
 
 /**
  * Parse raw UDP packets.
@@ -28,4 +29,11 @@ typedef struct {
  */
 int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props);
 
+/**
+ * Insert UDP header.
+ * @param hdr space where the header is to be inserted
+ * @param headers linked list of header, top is always relevant
+ */
+void insert_udp_header(uint8_t * hdr, const PcktHeaderElement * headers);
+
 #endif //ETHERLIB_UDP_PACKET_H
diff --git a/utils.h b/utils.h
index 1c6ed93..7bee598 100644
--- a/utils.h
+++ b/utils.h
@@ -33,15 +33,23 @@
 
 #define ERROR(...) printf(__VA_ARGS__)
 #define INFO(...) printf(__VA_ARGS__)
+#define MSG(...) printf(__VA_ARGS__)
+
+#define IPv4(a,b,c,d) ((a) | (b << 8) | (c << 16) | (d << 24))
+#define PRINT_IPv4(ip) MSG("%u.%u.%u.%u", (ip & 0xFF), ((ip >> 8) & 0xFF), ((ip >> 16) & 0xFF), ((ip >> 24) & 0xFF))
 
 #define ASSERT_BAD_ALIGN(p) if ((size_t)(p) & 0b11) ERROR("Bad memory alignment in function '%s' in file '%s' on line %d!\n", __func__, __FILE__, __LINE__)
 #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 COPY_ADVANCE(dst,src,n) memcpy((dst), (src), n), src += n
-#define COPY_WORD_H2N_ADVANCE(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2); w += 2; }
-#define COPY_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du; memcpy(&du, dw, 4); du = htonl(du); memcpy((dst), &du, 4); dw += 4; }
+#define FETCH_ADVANCE(dst,src,n) memcpy((dst), (src), n), (src) += n
+#define FILL_ADVANCE(dst,src,n) memcpy((dst), (src), n), (dst) += n
+#define FETCH_WORD_H2N_ADVANCE(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2); (w) += 2; }
+#define FILL_WORD_H2N_ADVANCE(dst,w) { uint16_t u = htons(w); memcpy((dst), &u, 2); (dst) += 2; }
+#define FETCH_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du; memcpy(&du, dw, 4); du = htonl(du); memcpy((dst), &du, 4); (dw) += 4; }
+#define FILL_DWORD_H2N_ADVANCE(dst,w) { uint32_t du = htonl(dw); memcpy(dst, &du, 4); (dst) += 4; }
+
 
 // ------------------