Timestamping and bunch of bugfix and optimization

- Timestamping management added
- Errors due to reading uninitialized data in ARP fixed
- EthInterface reworked, incoming packet notification and payload readout separated (through which fixing concurrent access problems)
- RX and TX offloads added
- Capability to add a packet sieve layer without prior registration of specific packet class added (this makes it possible to register arbitrary EtherType connection blocks, for example)

arp_cache.c

@@ -114,7 +114,7 @@ void arpc_ask(ArpCache *arpc, ip4_addr addr) {
 }
 
 void arpc_respond(ArpCache *arpc, const EthernetAddress hwAddr, ip4_addr ipAddr) {
-    ArpProps arpProps;
+    ArpProps arpProps = { 0 };
 
     arpProps.HTYPE = 1;
     arpProps.PTYPE = ETH_IPv4_PACKET_CLASS;

connection_block.c

@@ -7,6 +7,10 @@
 
 #include <stddef.h>
 
+void connb_init_defaults(ConnBlock *connb) {
+    // ...
+}
+
 void connb_remove(ConnBlock *connb) {
     if (connb != NULL) {
         packsieve_remove_layer(connb->sieveLayer);

connection_block.h

@@ -13,9 +13,14 @@ typedef int (*ConnBlockTransmitFn)(struct EthInterface_ * intf, const uint8_t *
 typedef struct ConnBlock_ {
     PcktSieve * sieve; ///< Ethernet interface
     PcktSieveLayer * sieveLayer; ///< Sieve layer
-    void * tag; ///< Arbitrary tagging
 } ConnBlock;
 
+/**
+ * Initialize non-mandatory fields to default values.
+ * @param connb
+ */
+void connb_init_defaults(ConnBlock * connb);
+
 /**
  * Remove connection block.
  * @param connb pointer to existing connection block

dynmem.h

@@ -7,6 +7,11 @@
 #include <stdio.h>
 #endif
 
+#ifdef VALGRIND_DEBUG
+#include <stdlib.h>
+#include <stdio.h>
+#endif
+
 /**
  * Initialize EtherLib dynamic memory management subsystem,
  * based on heap pointer and size given in etherlib_options.h
@@ -22,6 +27,8 @@ void * dynmem_alloc_(uint32_t size);
 
 #ifdef DYNMEM_DEBUG
 #define dynmem_alloc(size) dynmem_alloc_(size); MSG("ALLOC: %d %s() in %s:%d\n", size, __FUNCTION__, __FILE__, __LINE__)
+#elif defined(VALGRIND_DEBUG)
+#define dynmem_alloc(size) malloc(size)
 #else
 #define dynmem_alloc(size) dynmem_alloc_(size)
 #endif
@@ -34,6 +41,8 @@ void dynmem_free_(void * ptr);
 
 #ifdef DYNMEM_DEBUG
 #define dynmem_free(ptr) MSG("FREE:  %s() in %s:%d\n", __FUNCTION__, __FILE__, __LINE__), dynmem_free_(ptr)
+#elif defined(VALGRIND_DEBUG)
+#define dynmem_free(p) free(p)
 #else
 #define dynmem_free(ptr) dynmem_free_(ptr)
 #endif

eth_interface.c

@@ -6,16 +6,22 @@
 #include "dynmem.h"
 #include "utils.h"
 #include "etherlib_options.h"
-#include "etherlib/prefab/conn_blocks/ethernet_info.h"
+#include "etherlib/prefab/conn_blocks/ethernet_connblock.h"
 
 static int ethintf_llrecv(EthIODef * io, const RawPckt * pckt) {
     ethinf_receive((EthInterface *) io->tag, pckt);
     return 0;
 }
 
+static int ethintf_llrxnotify(EthIODef * io) {
+    ethinf_notify((EthInterface *) io->tag);
+    return 0;
+}
+
 static void ethintf_register(EthInterface * intf) {
     intf->ioDef->tag = intf;
-    intf->ioDef->llRxDone = ethintf_llrecv;
+    intf->ioDef->llRxStore = ethintf_llrecv;
+    intf->ioDef->llRxNotify = ethintf_llrxnotify;
 }
 
 // interface processing thread
@@ -36,11 +42,13 @@ EthInterface *ethintf_new(EthIODef * io) {
     ethIntf->txQ = mq_create(ETHLIB_DEF_MQ_SIZE);
     ethIntf->rxQ = mq_create(ETHLIB_DEF_MQ_SIZE);
     ETHLIB_OS_SEM_CREATE(&ethIntf->rxSem, ETHLIB_DEF_MQ_SIZE);
-    ETHLIB_OS_THREAD_DEFINE(ethinf_proc_thread, ipp, osPriorityHigh, 4096, ethIntf);
+    ETHLIB_OS_THREAD_DEFINE(ethinf_proc_thread, ethrx, osPriorityHigh, 4096, ethIntf);
-    ETHLIB_OS_THREAD_CREATE(ipp, ethIntf);
+    ETHLIB_OS_THREAD_CREATE(ethrx, ethIntf);
 
     ethIntf->ipra = ipra_new();
 
+    ethIntf->capabilities = 0;
+
     return ethIntf;
 }
 
@@ -48,7 +56,8 @@ static void * ethinf_proc_thread(void * param) {
     EthInterface * intf = (EthInterface *) param;
     while (true) {
         ETHLIB_OS_SEM_WAIT(&intf->rxSem);
-        if (mq_avail(intf->rxQ) > 0) {
+        intf->ioDef->llRxRead(intf->ioDef, intf->rxQ); // read packets
+        while (mq_avail(intf->rxQ) > 0) {
             RawPckt rawPckt = mq_top(intf->rxQ);
             mq_pop(intf->rxQ);
             packsieve_input(&intf->sieve, &rawPckt);
@@ -59,9 +68,7 @@ static void * ethinf_proc_thread(void * param) {
 
 void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt) {
     bool pushOK = mq_push(intf->rxQ, rawPckt);
-    if (pushOK) {
+    if (!pushOK) {
-        ETHLIB_OS_SEM_POST(&intf->rxSem);
-    } else {
         dynmem_free(rawPckt->payload);
         ERROR("Input queue full, packet dropped!\n");
     }
@@ -72,3 +79,11 @@ void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt) {
 
     intf->ioDef->llTxTrigger(intf->ioDef, intf->txQ);
 }
+
+void ethinf_notify(EthInterface *intf) {
+    ETHLIB_OS_SEM_POST(&intf->rxSem);
+}
+
+void ethinf_set_capabilities(EthInterface *intf, uint32_t cap) {
+    intf->capabilities = cap;
+}

eth_interface.h

@@ -17,12 +17,27 @@ typedef struct EthIODef_ {
     int (*llTxTrigger)(struct EthIODef_ * io, MsgQueue * mq); ///< Function pointer to low-level transmit function trigger
     int (*llTxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Transmission done (interrupt) callback
     int (*llLinkChg)(struct EthIODef_ * io, int linkState); ///< Link change interrupt
-    int (*llRxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Receive done callback
+    int (*llRxStore)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Receive done callback
     int (*llError)(struct EthIODef_ * io, int error); ///< Low-level error interrupt
+    int (*llRxRead)(struct EthIODef_ * io, MsgQueue * mq); ///< Read received packets
+    int (*llRxNotify)(struct EthIODef_ * io); ///< Notify of received packets
     void * tag; ///< Some arbitrary tagging
 
 } EthIODef;
 
+/**
+ * Ethernet interface capabilities
+ */
+typedef enum {
+    ETHINF_CAP_NONE = 0,
+    ETHINF_CAP_TX_CRC_OFFLOAD = 1,
+    ETHINF_CAP_TX_IPCHKSUM_OFFLOAD = 2,
+    ETHINF_CAP_TX_TCPUDPCHKSUM_OFFLOAD = 4,
+    ETHINF_CAP_RX_IPCHKSUM_OFFLOAD = 8,
+    ETHINF_CAP_RX_TCPUDPCHKSUM_OFFLOAD = 16,
+    ETHINF_CAP_ALL_RX_TX_CHECKSUM_OFFLOADS = 0b11111,
+} EthIntfCap;
+
 /**
  * Ethernet interface representation.
  */
@@ -30,6 +45,7 @@ typedef struct EthInterface_ {
     PcktSieve sieve; ///< Packet sieve
     EthIODef * ioDef; ///< Low-level IO definitions
     EthernetAddress mac; ///< Ethernet address
+    uint32_t capabilities; ///< Ethernet interface capabilities
     ip4_addr ip; ///< IP address
     ip4_addr router; ///< Router IP address
     ip4_addr netmask; ///< Subnet mask
@@ -55,9 +71,23 @@ EthInterface *ethintf_new(EthIODef * io);
  */
 void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt);
 
+/**
+ * Notify interface of a received, buffered packet, which needs transferred
+ * from the low level Ethernet-driver.
+ * @param intf Pointer to Ethernet-interface
+ */
+void ethinf_notify(EthInterface *intf);
+
 /**
  * Transmit packet.
  */
 void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt);
 
+/**
+ * Set interface capabilities.
+ * @param intf pointer to Ethernet interface
+ * @param cap OR-ed bitfield of capabilities
+ */
+void ethinf_set_capabilities(EthInterface *intf, uint32_t cap);
+
 #endif //ETHERLIB_ETH_INTERFACE_H

etherlib.h

@@ -0,0 +1,14 @@
+#ifndef ETHERLIB_ETHERLIB_H
+#define ETHERLIB_ETHERLIB_H
+
+#include "prefab/prefab.h"
+#include "global_state.h"
+#include "timestamping.h"
+#include "utils.h"
+
+// TODO group these includes..
+#include "prefab/conn_blocks/igmp_connblock.h"
+#include "prefab/conn_blocks/udp_connblock.h"
+#include "prefab/conn_blocks/custom_ethertype_connblock.h"
+
+#endif //ETHERLIB_ETHERLIB_H

global_state.c

@@ -92,3 +92,7 @@ void close_connection(cbd d) {
 
     connb_remove(&connBlock); // remove connection block
 }
+
+EthInterface *get_default_interface() {
+    return E.ethIntf;
+}

global_state.h

@@ -35,4 +35,10 @@ void process_raw_packet(const uint8_t * data, uint32_t size);
  */
 void close_connection(cbd d);
 
+/**
+ * Get default Ethernet interface
+ * @return pointer to default Ethernet interface
+ */
+EthInterface * get_default_interface();
+
 #endif //ETHERLIB_GLOBAL_STATE_H

memory_pool.c

@@ -137,6 +137,10 @@ static void mp_join_free_blocks(MP *mp) {
 }
 
 void mp_free(MP *mp, const uint8_t *p) {
+    if (p == NULL) { // don't do anything with a NULL pointer
+        return;
+    }
+
     // look for registry record
     bool success = false;
     MPAllocRecord *recIter = mp->blockRegistry;

msg_queue.c

@@ -10,7 +10,7 @@
 #include <stdio.h>
 
 MsgQueue *mq_create(uint32_t size) {
-    MsgQueue * mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
+    MsgQueue *mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
     mq->size = size;
     mq->readIdx = 0;
     mq->writeIdx = 0;
@@ -19,19 +19,23 @@ MsgQueue *mq_create(uint32_t size) {
     return mq;
 }
 
-void mq_clear(MsgQueue * mq) {
+void mq_clear(MsgQueue *mq) {
     mq->readIdx = 0;
     mq->writeIdx = 0;
     memset(mq->pckts, 0, mq->size * sizeof(RawPckt));
 }
 
-uint32_t mq_avail(const MsgQueue * mq) {
+uint32_t mq_avail(const MsgQueue *mq) {
-    return mq->writeIdx - mq->readIdx;
+    int32_t delta = (int32_t) (mq->writeIdx - mq->readIdx);
+    if (delta < 0) { // modulo...
+        delta += (int32_t) mq->size;
+    }
+    return delta;
 }
 
-#define MQ_NEXT(size,current) (((current)+1)%(size))
+#define MQ_NEXT(size, current) (((current)+1)%(size))
 
-bool mq_push(MsgQueue * mq, const RawPckt * raw) {
+bool mq_push(MsgQueue *mq, const RawPckt *raw) {
     if (MQ_NEXT(mq->size, mq->writeIdx) == mq->readIdx) { // cannot push, queue is full
         return false;
     }
@@ -45,11 +49,11 @@ bool mq_push(MsgQueue * mq, const RawPckt * raw) {
     return true;
 }
 
-RawPckt mq_top(MsgQueue * mq) {
+RawPckt mq_top(MsgQueue *mq) {
     return mq->pckts[mq->readIdx];
 }
 
-void mq_pop(MsgQueue * mq) {
+void mq_pop(MsgQueue *mq) {
     if (mq_avail(mq) > 0) { // if there's anything to pop
         mq->readIdx = MQ_NEXT(mq->size, mq->readIdx);
     }

packet.h

@@ -10,8 +10,16 @@
  typedef struct {
      uint8_t * payload; ///> Pointer to raw packet payload.
      uint32_t size; ///> Raw packet size.
+     union {
+         struct {
              uint32_t time_s; ///> Timestamp seconds field
              uint32_t time_ns; ///> Timestamp nanoseconds field
+         } rx;
+         struct {
+             void(*txTsCb)(uint32_t ts_s, uint32_t ts_ns, uint32_t tag); ///> Transmit timestamp callback pointer
+             uint32_t arg; ///> User-defined argument
+         } tx;
+     } ext; ///> Extensions
  } RawPckt;
 
 struct PcktHeaderElement_;
@@ -23,7 +31,7 @@ struct PcktHeaderElement_;
  * Fields have been reordered so that fields are aligned.
  */
 typedef struct {
-    uint64_t time_s; ///< Timestamp seconds part.
+    uint64_t time_s; ///< Timestamp seconds part
     uint32_t time_ns; ///< Timestamp nanoseconds part.
     struct PcktHeaderElement_ * header; ///< Pointer to packet header. Points to the innermost header
     uint8_t * payload; ///< Pointer to (innermost) payload.

packet_registry.h

@@ -28,7 +28,7 @@ struct PcktHeaderElement_;
  * @param hdr buffer to insert header into
  * @param pcktHdrLe linked list of packet headers
  */
-typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe);
+typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe, struct EthInterface_ * intf);
 
 #define PcktPropsHeader \
     PcktHeaderInsertFn hdrInsFn; /**< Header insert function pointer (used only on transmission)  */ \

packet_sieve.c

@@ -16,8 +16,8 @@ void packfiltcond_zero(PcktSieveFilterCondition *cond) {
     memset(cond, 0, sizeof(PcktSieveFilterCondition));
 }
 
-PcktSieve *packsieve_new(EthInterface * intf) {
+PcktSieve* packsieve_new(EthInterface *intf) {
-    PcktSieve *sieve = (PcktSieve *) dynmem_alloc(sizeof(PcktSieve));
+    PcktSieve *sieve = (PcktSieve*) dynmem_alloc(sizeof(PcktSieve));
     ASSERT_NULL(sieve);
     memset(&sieve->layer0, 0, sizeof(PcktSieveLayer)); // clear layer0 data
     sieve->intf = intf;
@@ -30,7 +30,7 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
     uint32_t size = rawPckt->size;
     bool mrd = true; // 'Must Release Data': at the end of processing not only release headers but data buffer as well
 
-    restart:;
+    restart: ;
     // process payload, fetch packet class etc.
     uint16_t ownClass = 0, containerClass = 0; // Ethernet...
     uint16_t offset = 0;
@@ -48,16 +48,14 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
         uint32_t hdrSize = ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize;
         // look for possible former allocated cache area
         PcktHeaderElement *header = NULL;
-        if (cdesc->cacheSize <= hdrSize) { // allocate cache area
+        if (cdesc->cacheSize < hdrSize) { // allocate cache area if previously allocated area is too small
-            if (cdesc->cacheArea != NULL) {
+            dynmem_free(cdesc->cacheArea); // does nothing if cacheArea is NULL
-                dynmem_free(cdesc->cacheArea);
+            cdesc->cacheArea = (uint8_t*) dynmem_alloc(hdrSize);
-            }
-            cdesc->cacheArea = (uint8_t *)dynmem_alloc(hdrSize);
             ASSERT_NULL(cdesc->cacheArea);
-            cdesc->cacheSize = hdrSize;
+            cdesc->cacheSize = hdrSize; // retain cache size
         }
 
-        header = (PcktHeaderElement *)cdesc->cacheArea;
+        header = (PcktHeaderElement*) cdesc->cacheArea;
 
         memset(header, 0, hdrSize);
         header->props.ownPacketClass = ownClass;
@@ -81,7 +79,8 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
             mrd = pb.b;
             // NO BREAK!
         case PROC_FN_RET_ABORT:
-                goto header_release; // GOTO :D!
+            goto header_release;
+            // GOTO :D!
             break;
         case PROC_FN_RET_OK:
             default:
@@ -102,17 +101,22 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
         lastHeader = header;
     } while ((ownClass != 0) && lastHeader->props.validityOK);
 
+    if (lastHeader == NULL) { // if found nothing, don't attempt to process
+        goto data_release;
+    }
+
     lastHeader->next = NULL;
 
     // ------------------------------------
 
     if (!lastHeader->props.validityOK) { // if packet is not valid, then drop
-        goto header_release; // GOTO here!
+        goto header_release;
+        // GOTO here!
     }
 
     Pckt packet;
-    packet.time_s = rawPckt->time_s;
+    packet.time_s = rawPckt->ext.rx.time_s;
-    packet.time_ns = rawPckt->time_ns;
+    packet.time_ns = rawPckt->ext.rx.time_ns;
 
     // lookup headers in the sieve
     const PcktHeaderElement *headerIter = outermostHeader;
@@ -125,7 +129,10 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
             found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props, sieve->intf); // specific or general match
             if (found) {
                 layer = nodeIter; // advance in the sieve tree
-                const PcktHeaderElement *containedHeader = headerIter->next; // advance on headers
+                PcktHeaderElement *containedHeader = headerIter;
+                if (headerIter->next != NULL) {
+                    containedHeader = containedHeader->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;
@@ -156,9 +163,8 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
 //        INFO("Packet headers not fully processed!\n");
 //    }
 
-
     // release header chain blocks
-    header_release:; // empty line, solely for label placement
+    header_release: ; // empty line, solely for label placement
 //    PcktHeaderElement *iter = outermostHeader;
 //    while (iter != NULL) {
 //        PcktHeaderElement *next = iter->next;
@@ -168,13 +174,14 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) {
     if (procRet == PROC_FN_RET_REPRST) { // if a restart was requested, then run everything again!
         goto restart;
     }
-    data_release:;
+    data_release: ;
     if (mrd) {
         dynmem_free(data);
     }
 }
 
-PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass) {
+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;
     bool alreadyExists = false;
@@ -190,7 +197,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte
     if (alreadyExists) {
         return nodeIter; // OK
     } else { // if allocation of a new layer is required
-        PcktSieveLayer *layer = (PcktSieveLayer *) dynmem_alloc(sizeof(PcktSieveLayer));
+        PcktSieveLayer *layer = (PcktSieveLayer*) dynmem_alloc(sizeof(PcktSieveLayer));
         ASSERT_NULL(layer);
 
         PcktSieveLayer *oldListFirst = parent->nodes;
@@ -221,6 +228,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte
         layer->cbFn = cbFn;
         layer->tag = tag;
         layer->connBReportFn = NULL;
+        layer->txTsCb = NULL;
         return layer;
     }
 }
@@ -260,10 +268,10 @@ bool packsieve_remove_layer(PcktSieveLayer *layer) {
 
 #define ETH_SIEVE_LAYER_INDENT_PER_LEVEL (4)
 
-void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint32_t indent) {
+void packsieve_report(const PcktSieve *sieve, const PcktSieveLayer *layer, uint32_t indent) {
     if (layer->connBReportFn != NULL) {
         INFO("%*c└─┤", indent, ' ');
-        const ConnBlock connBlock = { sieve, layer, NULL }; // tag not required
+        ConnBlock connBlock = { sieve, layer }; // tag not required
         layer->connBReportFn(&connBlock);
         INFO("├───\n");
     } else {
@@ -276,11 +284,11 @@ void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint
     }
 }
 
-void packsieve_layer_info(const PcktSieve * sieve, const PcktSieveLayer * layer) {
+void packsieve_layer_info(const PcktSieve *sieve, const PcktSieveLayer *layer) {
     const PcktSieveLayer *iter = layer;
     while (iter != NULL) { // climb up to the top in the sieve tree
         if (iter->connBReportFn != NULL) {
-            const ConnBlock connBlock = {sieve, iter, NULL}; // tag not required
+            ConnBlock connBlock = { sieve, iter }; // tag not required
             iter->connBReportFn(&connBlock);
         } else {
             INFO("[%d]", layer->packetClass);

packet_sieve.h

@@ -10,7 +10,7 @@
  * Packet header information.
  */
 typedef struct PcktHeaderElement_ {
-    struct PcktHeaderElement_ * next, * prev; ///< Next and previous header in the linked list
+    struct PcktHeaderElement_ *next, *prev; ///< Next and previous header in the linked list
     PcktProps props; ///< Properties (allocated to appropriate size)
 } PcktHeaderElement;
 
@@ -18,7 +18,7 @@ typedef struct PcktHeaderElement_ {
  * Free chain of packet header. List is auto-rewound.
  * @param hdr pointer to header chain element
  */
-void pckthdr_chain_free(PcktHeaderElement * hdr);
+void pckthdr_chain_free(PcktHeaderElement *hdr);
 
 #define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *))
 
@@ -42,25 +42,25 @@ typedef union {
  * @param c2 condition 2
  * @return true if cond1 matches cond2, otherwise false
  */
-bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilterCondition * c2);
+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);
+void packfiltcond_zero(PcktSieveFilterCondition *cond);
 
 struct EthInterface_;
 
 /**
  * Sieve filter function type.
  */
-typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, struct EthInterface_ * intf);
+typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, struct EthInterface_ *intf);
 
 struct PcktSieveLayer_;
 
 typedef union {
-    void * p;
+    void *p;
     uint32_t u;
 } PcktSieveLayerTag;
 
@@ -70,14 +70,15 @@ typedef union {
 /**
  * Callback function type for packet sieve match.
  */
-typedef int (*SieveCallBackFn)(const Pckt * pckt, PcktSieveLayerTag tag);
+typedef int (*SieveCallBackFn)(const Pckt *pckt, PcktSieveLayerTag tag);
 
 /**
  * Callback function for printing connblock report.
  */
 struct ConnBlock_;
-typedef void (*ConnBlockReportFn)(const struct ConnBlock_ * connBlock);
+typedef void (*ConnBlockReportFn)(const struct ConnBlock_ *connBlock);
 
+typedef void(*TxTimeStampCBFn)(uint32_t ts_s, uint32_t ts_ns, uint32_t tag);
 
 #define PCKT_SIEVE_INFOTAG_LEN (24)
 
@@ -91,9 +92,10 @@ typedef struct PcktSieveLayer_ {
     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
+    TxTimeStampCBFn txTsCb; ///< Transmit timestamp callback pointer
-    struct PcktSieveLayer_ * next, * prev; ///< Next and previous sieve layer on the same level
+    struct PcktSieveLayer_ *parent; ///< Pointer to parent node in the sieve tree
-    struct PcktSieveLayer_ * nodes; ///< Subnodes in the sieve tree
+    struct PcktSieveLayer_ *next, *prev; ///< Next and previous sieve layer on the same level
+    struct PcktSieveLayer_ *nodes; ///< Subnodes in the sieve tree
     ConnBlockReportFn connBReportFn; ///< Connection block report function pointer
 } PcktSieveLayer;
 
@@ -102,14 +104,14 @@ typedef struct PcktSieveLayer_ {
  */
 typedef struct PcktSieve_ {
     PcktSieveLayer layer0; ///< Top of sieve tree
-    struct EthInterface_ * intf; ///< Ethernet interface
+    struct EthInterface_ *intf; ///< Ethernet interface
 } PcktSieve;
 
 /**
  * Create new packet sieve.
  * @return pointer to packet sieve object or NULL on failure
  */
-PcktSieve * packsieve_new();
+PcktSieve *packsieve_new();
 
 /**
  * Process packet with packet sieve.
@@ -136,7 +138,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte
  * Remove sieve layer from packet sieve.
  * @param layer Layer to remove and deallocate
  */
-bool packsieve_remove_layer(PcktSieveLayer * layer);
+bool packsieve_remove_layer(PcktSieveLayer *layer);
 
 /**
  * Recursively report layer structure on terminal.
@@ -144,7 +146,7 @@ bool packsieve_remove_layer(PcktSieveLayer * layer);
  * @param layer pointer to existing layer
  * @param indent sub-tree indentation
  */
-void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint32_t indent);
+void packsieve_report(const PcktSieve *sieve, const PcktSieveLayer *layer, uint32_t indent);
 
 #define packsieve_report_full(sieve) packsieve_report((sieve), &(sieve)->layer0, 0)
 
@@ -153,6 +155,6 @@ void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint
  * @param sieve pointer to packet sieve structure
  * @param layer pointer to an existing layer
  */
-void packsieve_layer_info(const PcktSieve * sieve, const PcktSieveLayer * layer);
+void packsieve_layer_info(const PcktSieve *sieve, const PcktSieveLayer *layer);
 
 #endif //ETHERLIB_PACKET_SIEVE_H

pckt_assembler.c

@@ -10,7 +10,7 @@
 #include "global_state.h"
 #include "utils.h"
 
-int pckt_assemble(RawPckt *raw, Pckt *cooked) {
+int pckt_assemble(RawPckt *raw, Pckt *cooked, EthInterface *intf) {
     // calculate frame size
     uint16_t frameSize = 0;
     uint16_t headerSize = 0;
@@ -27,7 +27,11 @@ int pckt_assemble(RawPckt *raw, Pckt *cooked) {
         lastHdr = hdrIter;
         hdrIter = hdrIter->next;
     }
-    frameSize = headerSize + cooked->payloadSize + 4; // header + payload + CRC32 checksum area
+    frameSize = headerSize + cooked->payloadSize;// + 4; // header + payload
+    bool computeCRC = !(intf->capabilities & ETHINF_CAP_TX_CRC_OFFLOAD);
+    if (computeCRC) { // + CRC32 checksum area if needed
+        frameSize += 4;
+    }
 
     // calculate padding size
     uint16_t padding = (frameSize < ETH_FRAME_MIN_SIZE) ? (ETH_FRAME_MIN_SIZE - frameSize) : 0;
@@ -58,7 +62,7 @@ int pckt_assemble(RawPckt *raw, Pckt *cooked) {
     }
 
     while (hdrIter != NULL) {
-        hdrIter->props.hdrInsFn(headerInsPtr, hdrIter); // insert header
+        hdrIter->props.hdrInsFn(headerInsPtr, hdrIter, intf); // insert header
         hdrIter = hdrIter->prev; // step to previous header
         if (hdrIter != NULL) {
             headerInsPtr -= hdrIter->props.headerSize; // advance data pointer
@@ -69,9 +73,14 @@ int pckt_assemble(RawPckt *raw, Pckt *cooked) {
         }
     }
 
-    // insert CRC32
+    // insert CRC32 if interface is not capable of inserting CRC
+    if (computeCRC) {
         uint32_t crc = crc32(raw->payload, frameSize - 4);
         memcpy(raw->payload + frameSize - 4, (const uint8_t *)&crc, 4);
+    }
+
+    // turn off TX timestamping by default
+    raw->ext.tx.txTsCb = NULL;
 
     return 0;
 }

pckt_assembler.h

@@ -13,6 +13,6 @@ struct EthInterface_;
  * @param cooked packet information and headers
  * @return 0 on success OR -1 on failure
  */
-int pckt_assemble(RawPckt *raw, Pckt *cooked);
+int pckt_assemble(RawPckt *raw, Pckt *cooked, struct EthInterface_ *intf);
 
 #endif //ETHERLIB_PCKT_ASSEMBLER_H

prefab/conn_blocks/arp_connblock.c

@@ -14,6 +14,7 @@ static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps *
 
 ConnBlock arp_new_connblock(EthInterface * intf, SieveCallBackFn cb) {
     ConnBlock arpConnB; // create ARP connblock
+    connb_init_defaults(&arpConnB);
 
     PcktSieveFilterCondition filtCond;
     packfiltcond_zero(&filtCond);
@@ -53,12 +54,13 @@ void arp_send(const ConnBlock * connBlock, const ArpProps * props) {
     uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_ARP_HEADER_SIZE;
     uint32_t txBufSize = MAX(txHeaderSize + 4, 60);
     uint8_t * txBuf = dynmem_alloc(txBufSize);
+    memset(txBuf, 0, txBufSize);
 
     // insert Ethernet header
-    insert_ethernet_header(txBuf, ethHeader);
+    insert_ethernet_header(txBuf, ethHeader, NULL);
 
     // insert ARP header
-    insert_arp_header(txBuf + ETH_ETHERNET_HEADER_SIZE, arpHeader);
+    insert_arp_header(txBuf + ETH_ETHERNET_HEADER_SIZE, arpHeader, NULL);
 
     // release headers
     dynmem_free(arpHeader);
@@ -72,8 +74,7 @@ void arp_send(const ConnBlock * connBlock, const ArpProps * props) {
     RawPckt rpckt;
     rpckt.size = txBufSize;
     rpckt.payload = txBuf;
-    rpckt.time_s = 0;
+    rpckt.ext.tx.txTsCb = NULL;
-    rpckt.time_ns = 0;
     ethinf_transmit(connBlock->sieve->intf, &rpckt);
 
     // release transmit buffer

prefab/conn_blocks/custom_ethertype_connblock.c

@@ -0,0 +1,92 @@
+//
+// Created by epagris on 2023.04.25..
+//
+
+#include "custom_ethertype_connblock.h"
+#include "../../eth_interface.h"
+#include "../../utils.h"
+#include "../../dynmem.h"
+#include "../../global_state.h"
+#include "etherlib/pckt_assembler.h"
+
+static bool filtCET(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, EthInterface *intf) {
+    EthernetProps *ethProps = (EthernetProps *) contProps;
+    //MSG("FILT! %X %X\n", ethProps->length_type, ETHERTYPE_FROM_FILTCOND(filtCond));
+    return ethProps->length_type == ETHERTYPE_FROM_FILTCOND(filtCond);
+}
+
+cbd cet_new_connblock(struct EthInterface_ *intf, uint16_t etherType, SieveCallBackFn cbFn) {
+    ConnBlock cetConnB;
+    connb_init_defaults(&cetConnB);
+
+    PcktSieveFilterCondition filtCond;
+    packfiltcond_zero(&filtCond);
+    ETHERTYPE_TO_FILTCOND(&filtCond, etherType);
+    PcktSieveLayerTag tag;
+    tag.u = 0;
+    cetConnB.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, false, filtCET, cbFn, tag, etherType);
+    ASSERT_NULL(cetConnB.sieveLayer);
+    cetConnB.sieveLayer->connBReportFn = cet_print_report;
+
+    cetConnB.sieve = &intf->sieve;
+
+    // store connection block to CBDT
+    cbd d = cbdt_alloc_new(E.cbdt, &cetConnB);
+    if (d == CBDT_ERR) { // on error free everything we have allocated before
+        packsieve_remove_layer(cetConnB.sieveLayer);
+    }
+
+    return d;
+}
+
+int cet_send(cbd d, uint8_t *dest, const uint8_t *data, uint16_t size) {
+    return cet_send_arg(d, dest, data, size, 0);
+}
+
+int cet_send_arg(cbd d, uint8_t *dest, const uint8_t *data, uint16_t size, uint32_t arg) {
+    ConnBlock connBlock;
+    if (!cbdt_get_connection_block(E.cbdt, d, &connBlock)) {
+        ERROR("Invalid CBD descriptor: '%d'!\n", d);
+        return 0;
+    }
+
+    PcktHeaderElement *ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
+    ethHeader->next = NULL;
+    ethHeader->prev = NULL;
+
+    EthernetProps *ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);
+    ethProps->length_type = ETHERTYPE_FROM_FILTCOND(&connBlock.sieveLayer->filtCond);
+    memcpy(ethProps->destAddr, dest, ETH_HW_ADDR_LEN);
+    memcpy(ethProps->sourceAddr, connBlock.sieve->intf->mac, ETH_HW_ADDR_LEN);
+
+    ethProps->hdrInsFn = insert_ethernet_header;
+    ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
+
+    Pckt cooked;
+    cooked.payload = data;
+    cooked.payloadSize = size;
+    cooked.header = ethHeader;
+
+    // NOT FILLED FIELDS
+    cooked.headerSize = 0;
+    cooked.time_s = 0;
+    cooked.time_ns = 0;
+
+    RawPckt raw;
+    pckt_assemble(&raw, &cooked, connBlock.sieve->intf);
+    raw.ext.tx.arg = arg; // assign argument
+    raw.ext.tx.txTsCb = connBlock.sieveLayer->txTsCb; // assign with TX timestamp callback
+
+    ethinf_transmit(connBlock.sieve->intf, &raw);
+
+    release_resources:
+    // free headers
+    dynmem_free(ethHeader);
+
+    return size;
+}
+
+void cet_print_report(const ConnBlock* connBlock) {
+    const PcktSieveLayer * sl = connBlock->sieveLayer;
+    INFO("EtherType: 0x%X", ETHERTYPE_FROM_FILTCOND(&sl->filtCond));
+}

prefab/conn_blocks/custom_ethertype_connblock.h

@@ -0,0 +1,21 @@
+#ifndef ETHERLIB_CUSTOM_ETHERTYPE_CONNBLOCK_H
+#define ETHERLIB_CUSTOM_ETHERTYPE_CONNBLOCK_H
+
+#include "../../connection_block.h"
+#include "../../cbd_table.h"
+#include "../packet_parsers/ethernet_frame.h"
+
+#define ETHERTYPE_FROM_FILTCOND(fc) ((fc)->uw[0])
+#define ETHERTYPE_TO_FILTCOND(fc,et) (((fc)->uw[0]) = (et))
+
+struct EthInterface_;
+
+cbd cet_new_connblock(struct EthInterface_ * intf, uint16_t etherType, SieveCallBackFn cbFn);
+
+int cet_send_arg(cbd d, uint8_t *dest, const uint8_t *data, uint16_t size, uint32_t arg);
+
+int cet_send(cbd d, EthernetAddress dest, const uint8_t * data, uint16_t size);
+
+void cet_print_report(const ConnBlock* connBlock);
+
+#endif //ETHERLIB_CUSTOM_ETHERTYPE_CONNBLOCK_H

prefab/conn_blocks/ethernet_connblock.c

@@ -2,7 +2,7 @@
 // Created by epagris on 2023.01.30..
 //
 
-#include "ethernet_info.h"
+#include "ethernet_connblock.h"
 #include "../../utils.h"
 
 #include "../../eth_interface.h"

prefab/conn_blocks/ethernet_connblock.h

@@ -1,5 +1,5 @@
-#ifndef ETHERLIB_ETHERNET_INFO_H
+#ifndef ETHERLIB_ETHERNET_CONNBLOCK_H
-#define ETHERLIB_ETHERNET_INFO_H
+#define ETHERLIB_ETHERNET_CONNBLOCK_H
 
 
 #include "../../connection_block.h"
@@ -10,4 +10,4 @@
  */
 void ethernet_print_report(const ConnBlock* connBlock);
 
-#endif //ETHERLIB_ETHERNET_INFO_H
+#endif //ETHERLIB_ETHERNET_CONNBLOCK_H

prefab/conn_blocks/icmp_connblock.c

@@ -21,31 +21,66 @@ static int icmp_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) {
     IcmpProps * icmpProps = HEADER_FETCH_PROPS(IcmpProps, pckt->header);
     EthInterface * intf = (EthInterface *) tag.p; // icmp_new_connblock() puts pointer to intf into tag field
 
+    static PcktHeaderElement * txIcmpHdr = NULL;
+    static PcktHeaderElement * txIpHdr = NULL;
+    static PcktHeaderElement * txEthHdr = NULL;
+
+    static IcmpProps * txIcmpProps = NULL;
+    static IPv4Props * txIpProps = NULL;
+    static EthernetProps * txEthProps = NULL;
+
+    if (txIcmpHdr == NULL) {
+        txIcmpHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(IcmpProps));
+        txIcmpHdr->next = NULL;
+        txIcmpProps = HEADER_FETCH_PROPS(IcmpProps, txIcmpHdr);
+    }
+    if (txIpHdr == NULL) {
+        txIpHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(IPv4Props));
+        txIcmpHdr->prev = txIpHdr;
+        txIpHdr->next = txIcmpHdr;
+        txIpProps = HEADER_FETCH_PROPS(IPv4Props, txIpHdr);
+    }
+    if (txEthHdr == NULL) {
+        txEthHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(EthernetProps));
+        txIpHdr->prev = txEthHdr;
+        txEthHdr->next = txIpHdr;
+        txEthHdr->prev = NULL;
+        txEthProps = HEADER_FETCH_PROPS(EthernetProps , txEthHdr);
+    }
+
     switch (icmpProps->type) {
         case ICMP_MT_ECHO_REQUEST: {
-            icmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply
+            *txIcmpProps = *icmpProps;
-            icmpProps->hdrInsFn = insert_icmp_header;
+            txIcmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply
+            txIcmpProps->hdrInsFn = insert_icmp_header;
 
             // swap source an destination IP addresses
             IPv4Props * iPv4Props = HEADER_FETCH_PROPS(IPv4Props, pckt->header->prev);
-            ip4_addr tmpIp = iPv4Props->SourceIPAddr;
+            *txIpProps = *iPv4Props;
-            iPv4Props->SourceIPAddr = iPv4Props->DestIPAddr;
+            ip4_addr tmpIp = txIpProps->SourceIPAddr;
-            iPv4Props->DestIPAddr = tmpIp;
+            txIpProps->SourceIPAddr = txIpProps->DestIPAddr;
-            iPv4Props->hdrInsFn = insert_ipv4_header;
+            txIpProps->DestIPAddr = tmpIp;
+            txIpProps->hdrInsFn = insert_ipv4_header;
 
             // swap Ethernet fields
             EthernetAddress tmpEth;
             EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pckt->header->prev->prev);
-            HWACPY(tmpEth, ethProps->sourceAddr);
+            *txEthProps = *ethProps;
-            HWACPY(ethProps->sourceAddr, ethProps->destAddr);
+            HWACPY(tmpEth, txEthProps->sourceAddr);
-            HWACPY(ethProps->destAddr, tmpEth);
+            HWACPY(txEthProps->sourceAddr, txEthProps->destAddr);
-            ethProps->hdrInsFn = insert_ethernet_header;
+            HWACPY(txEthProps->destAddr, tmpEth);
+            txEthProps->hdrInsFn = insert_ethernet_header;
 
             // payload is the same...
+            Pckt reply;
+            reply.header = txEthHdr;
+            reply.payload = pckt->payload;
+            reply.payloadSize = pckt->payloadSize;
+            reply.headerSize = pckt->headerSize;
 
             // assemble packet
             RawPckt raw;
-            pckt_assemble(&raw, pckt);
+            pckt_assemble(&raw, &reply, intf);
 
             // release headers
             //pckthdr_chain_free(pckt->header);
@@ -62,6 +97,8 @@ static int icmp_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) {
 
 ConnBlock icmp_new_connblock(EthInterface * intf) {
     ConnBlock icmpConnB;
+    connb_init_defaults(&icmpConnB);
+
     ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block
 
     PcktSieveFilterCondition filtCond;

prefab/conn_blocks/igmp_connblock.c

@@ -19,6 +19,8 @@ static bool filtIgmp(const PcktSieveFilterCondition * filtCond, const PcktProps
 
 ConnBlock igmp_new_connblock(struct EthInterface_ *intf) {
     ConnBlock igmpConnB;
+    connb_init_defaults(&igmpConnB);
+
     ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block
 
     PcktSieveFilterCondition filtCond;
@@ -87,7 +89,7 @@ static void igmp_send(ConnBlock * connBlock, ip4_addr ga, int mt) {
     cooked.time_ns = 0;
 
     RawPckt raw;
-    pckt_assemble(&raw, &cooked);
+    pckt_assemble(&raw, &cooked, connBlock->sieve->intf);
 
     ethinf_transmit(connBlock->sieve->intf, &raw);
 

prefab/conn_blocks/ipv4/ip_assembler.c

@@ -177,17 +177,17 @@ bool ipra_try_reassemble(IPv4Assembler *ipra, uint16_t id, uint8_t **payload, ui
             iter = iter->next;
         }
 
-        // release chain
-        ipra_remove_chain(ipra, id);
-
         // insert headers and change fields to obfuscate defragmentation
         IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, pcktHdrLe);
         ipProps->Flags = 0;
         ipProps->FragmentOffset = 0;
         ipProps->TotalLength = chainDesc->fullSize + ETH_IPv4_HEADER_SIZE;
 
-        insert_ethernet_header(p, pcktHdrLe->prev);
+        // release chain
-        insert_ipv4_header(p + ETH_ETHERNET_HEADER_SIZE, pcktHdrLe);
+        ipra_remove_chain(ipra, id);
+
+        insert_ethernet_header(p, pcktHdrLe->prev, NULL);
+        insert_ipv4_header(p + ETH_ETHERNET_HEADER_SIZE, pcktHdrLe, NULL);
 
         return true;
     }

prefab/conn_blocks/ipv4_connblock.c

@@ -19,6 +19,8 @@ static bool filtIPv4(const PcktSieveFilterCondition *filtCond, const PcktProps *
 
 ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackFn cbFn) {
     ConnBlock connb;
+    connb_init_defaults(&connb);
+
     PcktSieveFilterCondition filtCond;
     packfiltcond_zero(&filtCond);
     IP_ADDR_TO_FILTCOND(&filtCond, ipAddr);
@@ -48,3 +50,22 @@ void ipv4_print_report(const ConnBlock* connBlock) {
         INFO("IP: ANY");
     }
 }
+
+bool ipv4_in_range(ip4_addr a, ip4_addr l, ip4_addr u) {
+    l = ntohl(l); // this way IP addresses are mathematically sortable (by changing byte order)
+    u = ntohl(u);
+    a = ntohl(a);
+    return (l <= a) && (a <= u);
+}
+
+bool ipv4_is_multicast_address(ip4_addr addr) {
+    return ipv4_in_range(addr, IPv4(224, 0, 0, 0), IPv4(224, 0, 255, 255)) ||
+            ipv4_in_range(addr, IPv4(224, 1, 0, 0), IPv4(224, 1, 255, 255)) ||
+            ipv4_in_range(addr, IPv4(224, 3, 0, 0), IPv4(224, 4, 255, 255)) ||
+            ipv4_in_range(addr, IPv4(225, 0, 0, 0), IPv4(239, 255, 255, 255));
+//            ipv4_in_range(addr, IPv4(225, 0, 0, 0), IPv4(231, 255, 255, 255)) ||
+//            ipv4_in_range(addr, IPv4(232, 0, 0, 0), IPv4(232, 255, 255, 255)) ||
+//            ipv4_in_range(addr, IPv4(233, 0, 0, 0), IPv4(233, 251, 255, 255)) ||
+//            ipv4_in_range(addr, IPv4(233, 252, 0, 0), IPv4(233, 255, 255, 255)) ||
+//            ipv4_in_range(addr, IPv4(234, 252, 0, 0), IPv4(239, 255, 255, 255)) ||
+}

prefab/conn_blocks/ipv4_connblock.h

@@ -34,4 +34,11 @@ void ipv4_send(EthInterface * intf, ip4_addr addr, const uint8_t * data, uint32_
  */
 void ipv4_print_report(const ConnBlock* connBlock);
 
+/**
+ * Is the given address a multicast one?
+ * @param addr IP address to check
+ * @return ...
+ */
+bool ipv4_is_multicast_address(ip4_addr addr);
+
 #endif //ETHERLIB_IPV4_CONNBLOCK_H

prefab/conn_blocks/tcp_connblock.c

@@ -233,6 +233,8 @@ void tcp_debug(cbd d, bool debug) {
 
 cbd tcp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) {
     ConnBlock tcpConnB;
+    connb_init_defaults(&tcpConnB);
+
     ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block
 
     PcktSieveFilterCondition filtCond;
@@ -335,7 +337,7 @@ int tcp_send_segment(const struct ConnBlock_ *connBlock, TcpFlag flags, TcpOptio
     cooked.time_ns = 0;
 
     RawPckt raw;
-    pckt_assemble(&raw, &cooked);
+    pckt_assemble(&raw, &cooked, connBlock->sieve->intf);
 
     ethinf_transmit(connBlock->sieve->intf, &raw);
 

prefab/conn_blocks/udp_connblock.c

@@ -20,6 +20,8 @@ static bool filtUdp(const PcktSieveFilterCondition *filtCond, const PcktProps *c
 
 cbd udp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) {
     ConnBlock udpConnB;
+    connb_init_defaults(&udpConnB);
+
     ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block
 
     PcktSieveFilterCondition filtCond;
@@ -42,6 +44,10 @@ cbd udp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveC
 }
 
 int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port) {
+    return udp_sendto_arg(d, data, size, addr, port, 0);
+}
+
+int udp_sendto_arg(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port, uint32_t arg) {
     ConnBlock connBlock;
     if (!cbdt_get_connection_block(E.cbdt, d, &connBlock)) {
         ERROR("Invalid CBD descriptor: '%d'!\n", d);
@@ -82,7 +88,8 @@ int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_
 
     // Ethernet
     layer = layer->parent;
-    if (addr != 0xFFFFFFFF) {
+    bool isMulticast = ipv4_is_multicast_address(addr);
+    if ((!isMulticast) && (addr != 0xFFFFFFFF)) { // unicast
         ArpCache *arpc = connBlock.sieve->intf->arpc;
         const ArpEntry *entry = arpc_get_ask(arpc, addr);
         if (entry == NULL) {
@@ -90,8 +97,10 @@ int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_
             goto release_resources; // YEAH, goto HERE!
         }
         memcpy(ethProps->destAddr, entry, ETH_HW_ADDR_LEN);
-    } else {
+    } else if (isMulticast) { // multicast
-        memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN);
+        ethmc_from_ipmc(ethProps->destAddr, addr);
+    } else { // unicast
+        memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN); // broadcast
     }
     memcpy(ethProps->sourceAddr, connBlock.sieve->intf->mac, ETH_HW_ADDR_LEN);
     ethProps->length_type = ETH_IPv4_PACKET_CLASS;
@@ -111,7 +120,9 @@ int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_
     cooked.time_ns = 0;
 
     RawPckt raw;
-    pckt_assemble(&raw, &cooked);
+    pckt_assemble(&raw, &cooked, connBlock.sieve->intf);
+    raw.ext.tx.arg = arg; // assign argument
+    raw.ext.tx.txTsCb = connBlock.sieveLayer->txTsCb; // assign with TX timestamp callback
 
     ethinf_transmit(connBlock.sieve->intf, &raw);
 

prefab/conn_blocks/udp_connblock.h

@@ -21,7 +21,7 @@
 cbd udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn);
 
 /**
- * UDP transmit callback.
+ * UDP transmit function.
  * @param connBlock UDP connection block
  * @param data pointer to data buffer
  * @param size data size
@@ -30,6 +30,17 @@ cbd udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, Sieve
  */
 int udp_sendto(cbd connBlock, const uint8_t * data, uint32_t size, ip4_addr addr, uint16_t port);
 
+/**
+ * UDP transmit function with additional argument.
+ * @param connBlock UDP connection block
+ * @param data pointer to data buffer
+ * @param size data size
+ * @param addr remote address to send datagram to
+ * @param addr remote port
+ * @param arg user-defined parameter
+ */
+int udp_sendto_arg(cbd connBlock, const uint8_t * data, uint32_t size, ip4_addr addr, uint16_t port, uint32_t arg);
+
 /**
  * Print UDP connblock report.
  * @param connBlock UDP connblock

prefab/packet_parsers/arp_packet.c

@@ -47,7 +47,7 @@ int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
     return arpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT;
 }
 
-void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) {
     ArpProps *arpProps = HEADER_FETCH_PROPS(ArpProps, headers);
 
     FILL_WORD_H2N_ADVANCE(hdr, arpProps->HTYPE); // hardware type

prefab/packet_parsers/arp_packet.h

@@ -58,7 +58,7 @@ int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
  * @param hdr pointer to the packet buffer where the ARP header is to be written
  * @param headers linked list of packet headers
  */
-void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers);
+void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 #endif //ETHERLIB_ARP_PACKET_H
 

prefab/packet_parsers/ethernet_frame.c

@@ -5,6 +5,7 @@
 #include <memory.h>
 #include <stdbool.h>
 #include "ethernet_frame.h"
+#include "../../eth_interface.h"
 #include "../../utils.h"
 
 #define ETH_ETHERTYPE_LENGTH_THRESHOLD (1500)
@@ -24,10 +25,10 @@ int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdr
             frameProps->length_type <= ETH_ETHERTYPE_LENGTH_THRESHOLD ? 0 : frameProps->length_type;
     frameProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
 
-    return frameProps->containedPacketClass;
+    return PROC_FN_RET_OK;
 }
 
-void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) {
     EthernetProps * ethProps = (EthernetProps *) &headers->props;
     FILL_ADVANCE(hdr, ethProps->destAddr, ETH_HW_ADDR_LEN);
     FILL_ADVANCE(hdr, ethProps->sourceAddr, ETH_HW_ADDR_LEN);

prefab/packet_parsers/ethernet_frame.h

@@ -49,6 +49,6 @@ int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdr
  * @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);
+void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 #endif //ETHERLIB_ETHERNET_FRAME_H

prefab/packet_parsers/icmp_packet.c

@@ -26,7 +26,7 @@ int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
     return icmpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT;
 }
 
-void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf) {
     uint8_t * hdrStart = hdr;
     IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, headers);
     FILL_BYTE_ADVANCE(hdr, &icmpProps->type);

prefab/packet_parsers/icmp_packet.h

@@ -41,6 +41,6 @@ int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
  * @param hdr pointer to message header
  * @param headers linked list of recursively encapsulated packet headers
  */
-void insert_icmp_header(uint8_t * hdr, const PcktHeaderElement * headers);
+void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 #endif //ETHERLIB_ICMP_PACKET_H

prefab/packet_parsers/igmp_packet.c

@@ -25,7 +25,7 @@
 //    return sum16;
 //}
 
-void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf) {
     IgmpProps *igmpProps = HEADER_FETCH_PROPS(IgmpProps, headers);
     uint8_t * hdrBeg = hdr;
 

prefab/packet_parsers/igmp_packet.h

@@ -33,7 +33,7 @@ typedef struct {
  * @param hdr pointer to message header
  * @param headers linked list of recursively encapsulated packet headers
  */
-void insert_igmp_header(uint8_t * hdr, const PcktHeaderElement * headers);
+void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 /**
  * Parse IGMP packet.

prefab/packet_parsers/ipv4_packet.c

@@ -11,11 +11,11 @@
 
 #define ETH_IP_HEADER_LENGTH (20)
 
-static bool check_ipv4_validity(const uint8_t *hdr, const IPv4Props *ipProps) {
+static bool check_ipv4_validity(const uint8_t *hdr, const IPv4Props *ipProps, bool dontVerifyChecksum) {
     bool valid =
             (ipProps->Version == 4) &&
             (ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) &&
-            (chksum(hdr, ETH_IPv4_HEADER_SIZE, false) == 0); /*&&
+            (dontVerifyChecksum || (chksum(hdr, ETH_IPv4_HEADER_SIZE, false) == 0)); /*&&
             (ipProps->FragmentOffset == 0) && !(ipProps->Flags & 0x02 << 4); // TODO: discard if fragmented*/
     return valid;
 }
@@ -41,7 +41,7 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
     FETCH_ADVANCE(&ipProps->DestIPAddr, hdr, 4);
 
     // fill-in common packet header fields
-    ipProps->validityOK = check_ipv4_validity(hdrBegin, ipProps);
+    ipProps->validityOK = check_ipv4_validity(hdrBegin, ipProps, (intf->capabilities & ETHINF_CAP_RX_IPCHKSUM_OFFLOAD)); // don't verify checksum if done in hardware
     ipProps->containedPacketClass = ipProps->Protocol;
     ipProps->headerSize = ETH_IP_HEADER_LENGTH;
 
@@ -78,7 +78,7 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
 
 static uint16_t nextIpIdentification = 0;
 
-void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) {
     uint8_t *hdrOrig = hdr;
     IPv4Props *ipProps = (IPv4Props *) &headers->props;
     ipProps->Identification = nextIpIdentification++; // auto-insert identification
@@ -96,9 +96,11 @@ void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
     FILL_ADVANCE(hdr, &ipProps->SourceIPAddr, 4);
     FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);
 
-    // calculate checksum after filling header
+    // calculate checksum after filling header if needed
+    if (!(intf->capabilities & ETHINF_CAP_TX_IPCHKSUM_OFFLOAD)) {
         ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE, false);
         memcpy(ChkSumPtr, &ipProps->HeaderChecksum, 2);
+    }
 }
 
 void ethmc_from_ipmc(uint8_t *hwa, ip4_addr ipa) {

prefab/packet_parsers/ipv4_packet.h

@@ -57,7 +57,7 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
  * @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);
+void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 /**
  * Calculate Ethernet IPv4 multicast address from multicast

prefab/packet_parsers/tcp_segment.c

@@ -6,6 +6,7 @@
 #include "../../utils.h"
 #include "ethernet_frame.h"
 #include "../../dynmem.h"
+#include "../../eth_interface.h"
 
 #include "tcp_udp_common.h"
 #include "ipv4_packet.h"
@@ -140,20 +141,24 @@ int parse_tcp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
     }
 
     // verify checksum
+    if (!(intf->capabilities & ETHINF_CAP_RX_TCPUDPCHKSUM_OFFLOAD)) { // TODO
         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);
-    uint16_t chkSum = 0;
+        uint16_t chkSum = 0; // TODO ....
+        tcpProps->validityOK = (chkSum == 0);
+    } else {
+        tcpProps->validityOK = true;
+    }
 
     tcpProps->headerSize = fullHeaderSize;
-    tcpProps->validityOK = (chkSum == 0);
     tcpProps->containedPacketClass = 0;
 
     return tcpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT;
 }
 
-void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) {
     uint8_t * hdrBegin = hdr;
     TcpProps * tcpProps = HEADER_FETCH_PROPS(TcpProps, headers); // fetch header
 
@@ -181,11 +186,13 @@ void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
     tcp_insert_options(hdr, optSize, tcpProps->options);
 
     // calculate checksum
+    if (!(intf->capabilities & ETHINF_CAP_TX_TCPUDPCHKSUM_OFFLOAD)) {
         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);
+    }
 }
 
 // ----------------------------------

prefab/packet_parsers/tcp_segment.h

@@ -101,7 +101,7 @@ int parse_tcp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
  * @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);
+void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 /**
  * Get the flattened size of the option chain.

prefab/packet_parsers/udp_packet.c

@@ -5,6 +5,7 @@
 #include "ethernet_frame.h"
 
 #include "tcp_udp_common.h"
+#include "etherlib/eth_interface.h"
 
 #define ETH_UDP_HEADER_SIZE (8)
 
@@ -27,13 +28,17 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
 
     // common fields...
     udpProps->headerSize = ETH_UDP_HEADER_SIZE;
+    if (!(intf->capabilities & ETHINF_CAP_RX_TCPUDPCHKSUM_OFFLOAD)) {
         udpProps->validityOK = check_udp_validity(hdrBegin, pcktHdrLe);
+    } else {
+        udpProps->validityOK = true;
+    }
     udpProps->containedPacketClass = 0;
 
     return udpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT;
 }
 
-void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) {
     uint8_t *hdrBegin = hdr;
     UdpProps *udpProps = (UdpProps *) &headers->props;
     FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort);
@@ -43,11 +48,13 @@ void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
     udpProps->Checksum = 0;
     FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
 
-    // calculate checksum
+    // calculate checksum if needed
+    if (!(intf->capabilities & ETHINF_CAP_TX_TCPUDPCHKSUM_OFFLOAD)) {
         const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props;
         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);
+    }
 }
 
 

prefab/packet_parsers/udp_packet.h

@@ -37,6 +37,6 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
  * @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);
+void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf);
 
 #endif //ETHERLIB_UDP_PACKET_H

timestamping.c

@@ -0,0 +1,19 @@
+//
+// Created by epagris on 2023.04.22..
+//
+
+#include "timestamping.h"
+
+#include "global_state.h"
+#include "utils.h"
+
+void ts_set_tx_callback(cbd d, void (*cb)(uint32_t, uint32_t, uint32_t)) {
+    ConnBlock connb;
+    if (!cbdt_get_connection_block(E.cbdt, d, &connb)) { // try to fetch connection block
+        ERROR("Unknown CBD: %u\n", d); // if not found, then do nothing
+        return;
+    }
+
+    // if found...
+    connb.sieveLayer->txTsCb = cb;
+}

timestamping.h

@@ -0,0 +1,13 @@
+#ifndef ETHERLIB_TIMESTAMPING_H
+#define ETHERLIB_TIMESTAMPING_H
+
+#include "cbd_table.h"
+
+/**
+ * Assign callback funtion to specific connection block.
+ * @param d CBD to existing connection
+ * @param cb pointer to callback function
+ */
+void ts_set_tx_callback(cbd d, void(*cb)(uint32_t ts_s, uint32_t ts_ns, uint32_t tag));
+
+#endif //ETHERLIB_TIMESTAMPING_H

utils.h

@@ -5,6 +5,8 @@
 #ifndef ETHERLIB_UTILS_H
 #define ETHERLIB_UTILS_H
 
+#include <etherlib_options.h>
+
 #include <stdio.h>
 #include <stdint.h>
 #include <memory.h>
@@ -35,8 +37,6 @@
 #define WARNING(...) MSG(__VA_ARGS__)
 #define INFO(...) MSG(__VA_ARGS__)
 
-#define SNPRINTF(s,n,...) snprintf(s,n,__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 PRINT_HWADDR(hwaddr) MSG("%02x:%02x:%02x:%02x:%02x:%02x", (hwaddr)[0], (hwaddr)[1], (hwaddr)[2], (hwaddr)[3], (hwaddr)[4], (hwaddr)[5]);
@@ -66,8 +66,12 @@
 #define FILL_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du = htonl(dw); memcpy(dst, &du, 4); (dst) += 4; }
 #define FILL_DWORD_ADVANCE(dst,dw) { memcpy(dst, &(dw), 4); (dst) += 4; }
 
+#ifndef MIN
 #define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#endif
+#ifndef MAX
 #define MAX(a,b) (((a) > (b)) ? (a) : (b))
+#endif
 
 // ------------------