- ARP cache auto lookup feature added

- IGMPv2 capabilities added (report membership, leave group)
- ICMP capabilities added (ping)
- Tx Message Queue added

arp_cache.c

@@ -77,7 +77,7 @@ const ArpEntry *arpc_get_ask(ArpCache *arpc, ip4_addr ip) {
     while (((entry = arpc_get(arpc, ip)) == NULL) && (attemptN < ETHLIB_ARP_RETRY_COUNT)) {
         arpc_ask(arpc, ip);
         ETHLIB_SLEEP_MS(20);
-        //attemptN++;
+        attemptN++;
     }
 
     return entry;

eth_interface.c

@@ -26,6 +26,9 @@ EthInterface *ethintf_new(EthIODef * io) {
     ethIntf->arpc = arpc_new(ethIntf, ETHLIB_ARPCACHE_SIZE);
     ASSERT_NULL(ethIntf->arpc);
     ethintf_register(ethIntf);
+
+    ethIntf->txQ = mq_create(ETHLIB_DEF_MQ_SIZE);
+
     return ethIntf;
 }
 
@@ -34,5 +37,7 @@ void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt) {
 }
 
 void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt) {
-    intf->ioDef->llTx(intf->ioDef, rawPckt);
+    mq_push(intf->txQ, rawPckt); // push packet onto the message queue
+
+    intf->ioDef->llTxTrigger(intf->ioDef, intf->txQ);
 }

eth_interface.h

@@ -6,12 +6,13 @@
 #include "prefab/packet_parsers/ipv4_types.h"
 #include "arp_cache.h"
 #include "connection_block.h"
+#include "msg_queue.h"
 
 /**
  * Ethernet interface low level definition.
  */
 typedef struct EthIODef_ {
-    int (*llTx)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Function pointer to low-level transmit function
+    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
@@ -33,6 +34,7 @@ typedef struct EthInterface_ {
     ip4_addr dns; ///< Domain Name Server
     ArpCache * arpc; ///< ARP cache
     ConnBlock arpCb; ///< ARP connection block
+    MsgQueue * txQ; ///< Transmit queue
 } EthInterface;
 
 /**

global_state.c

@@ -5,6 +5,8 @@
 #include "prefab/prefab.h"
 #include "packet_sieve.h"
 #include "prefab/packet_parsers/arp_packet.h"
+#include "prefab/packet_parsers/icmp_packet.h"
+#include "etherlib/prefab/packet_parsers/igmp_packet.h"
 
 EthState gEthState;
 
@@ -14,6 +16,7 @@ static void register_packet_parsers() {
     cdesc.class = 0;
     cdesc.containerClass = 0;
     cdesc.procFun = parse_ethernet;
+    cdesc.hdrInsFn = insert_ethernet_header;
     cdesc.propertySize = sizeof(EthernetProps);
     packreg_add_class(E.pcktReg, &cdesc);
 
@@ -21,6 +24,7 @@ static void register_packet_parsers() {
     cdesc.class = ETH_IPv4_PACKET_CLASS;
     cdesc.containerClass = 0;
     cdesc.procFun = parse_ipv4;
+    cdesc.hdrInsFn = insert_ipv4_header;
     cdesc.propertySize = sizeof(IPv4Props);
     packreg_add_class(E.pcktReg, &cdesc);
 
@@ -28,6 +32,7 @@ static void register_packet_parsers() {
     cdesc.class = ETH_UDP_PACKET_CLASS;
     cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
     cdesc.procFun = parse_udp;
+    cdesc.hdrInsFn = insert_udp_header;
     cdesc.propertySize = sizeof(UdpProps);
     packreg_add_class(E.pcktReg, &cdesc);
 
@@ -35,12 +40,30 @@ static void register_packet_parsers() {
     cdesc.class = ETH_ARP_PACKET_CLASS;
     cdesc.containerClass = 0;
     cdesc.procFun = parse_arp;
+    cdesc.hdrInsFn = insert_arp_header;
     cdesc.propertySize = sizeof(ArpProps);
     packreg_add_class(E.pcktReg, &cdesc);
+
+    // ICMP packet parser
+    cdesc.class = ETH_ICMP_PACKET_CLASS;
+    cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
+    cdesc.procFun = parse_icmp;
+    cdesc.hdrInsFn = insert_icmp_header;
+    cdesc.propertySize = sizeof(IcmpProps);
+    packreg_add_class(E.pcktReg, &cdesc);
+
+    // IGMP packet parser
+    cdesc.class = ETH_IGMP_PACKET_CLASS;
+    cdesc.containerClass = ETH_IPv4_PACKET_CLASS;
+    cdesc.procFun = parse_igmp;
+    cdesc.hdrInsFn = insert_igmp_header;
+    cdesc.propertySize = sizeof(IgmpProps);
+    packreg_add_class(E.pcktReg, &cdesc);
 }
 
 void ethlib_init() {
     dynmem_init(); // initialize dynamic memory subsystem
+    E.tmr = timer_new(ETHLIB_TMR_SCHED_TABLE_SIZE); // create timer
     E.pcktReg = packreg_new(); // create new packet registry
     register_packet_parsers(); // register packet parsers
 

global_state.h

@@ -5,12 +5,14 @@
 #include "packet_registry.h"
 #include "packet_sieve.h"
 #include "eth_interface.h"
+#include "timer.h"
 
 /**
  * Global EtherLib state.
  */
 typedef struct {
     MP * mp; ///< Memory pool for dynamic allocations
+    Timer * tmr; ///< Timer for internal schedule.
     PcktRegistry * pcktReg; ///< Packet registry
     EthInterface * ethIntf; ///< Array of Ethernet interfaces
 } EthState;

msg_queue.c

@@ -2,4 +2,55 @@
 // Created by epagris on 2023.01.13..
 //
 
+#include <memory.h>
+#include <stdbool.h>
 #include "msg_queue.h"
+#include "dynmem.h"
+
+MsgQueue *mq_create(uint32_t size) {
+    MsgQueue * mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
+    mq->size = size;
+    mq->readIdx = 0;
+    mq->writeIdx = 0;
+    memset(mq->pckts, 0, mq->size * sizeof(RawPckt));
+
+    return 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) {
+    return mq->writeIdx - mq->readIdx;
+}
+
+#define MQ_NEXT(size,current) (((current)+1)%(size))
+
+bool mq_push(MsgQueue * mq, const RawPckt * raw) {
+    if (MQ_NEXT(mq->size, mq->writeIdx) == mq->readIdx) { // cannot push, queue is full
+        return false;
+    }
+
+    // can push
+    mq->pckts[mq->writeIdx++] = *raw;
+
+    // advance write pointer
+    mq->writeIdx = MQ_NEXT(mq->size, mq->writeIdx);
+
+    return true;
+}
+
+RawPckt mq_top(MsgQueue * mq) {
+    return mq->pckts[mq->readIdx];
+}
+
+void mq_pop(MsgQueue * mq) {
+    if (mq_avail(mq) > 0) { // if there's something to pop
+        mq->readIdx = MQ_NEXT(mq->size, mq->readIdx);
+    }
+}
+
+

msg_queue.h

@@ -1,8 +1,55 @@
-//
-// Created by epagris on 2023.01.13..
-//
-
 #ifndef ETHERLIB_TEST_MSG_QUEUE_H
 #define ETHERLIB_TEST_MSG_QUEUE_H
 
+#include <stdint.h>
+#include "packet.h"
+
+typedef struct {
+    uint32_t writeIdx; ///< Next block to write
+    uint32_t readIdx; ///< Next block to read
+    uint32_t size; ///< Size of circular buffer
+    RawPckt pckts[]; ///< Array of packets
+} MsgQueue;
+
+/**
+ * Create Message Queue.
+ * @param size size of circular buffer
+ * @return pointer to MQ instance OR NULL on failure
+ */
+MsgQueue * mq_create(uint32_t size);
+
+/**
+ * Clear circular buffer.
+ * @param mq pointer to Message Queue
+ */
+void mq_clear(MsgQueue * mq);
+
+/**
+ * Get number of available elements.
+ * @param mq pointer to Message Queue
+ * @return number of available elements
+ */
+uint32_t mq_avail(const MsgQueue * mq);
+
+/**
+ * Push element to the Message Queue.
+ * @param mq pointer to MQ
+ * @param raw pointer to raw packet
+ * @return true on success, false on failure (e.g.: queue full)
+ */
+bool mq_push(MsgQueue * mq, const RawPckt * raw);
+
+/**
+ * Get top element.
+ * @param mq pointer to MQ
+ * @return top element (COPY, NOT POINTER!)
+ */
+RawPckt mq_top(MsgQueue * mq);
+
+/**
+ * Pop top element.
+ * @param mq pointer to MQ
+ */
+void mq_pop(MsgQueue * mq);
+
 #endif //ETHERLIB_TEST_MSG_QUEUE_H

packet_registry.h

@@ -18,24 +18,31 @@ typedef uint8_t bool8_t;
  * as well. (The size of the packet is divisible by 4.)
  */
 
-struct PcktClassDesc_;
+//struct PcktClassDesc_;
+struct EthInterface_;
+struct PcktHeaderElement_;
+
+/**
+ * @typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe)
+ * @brief Packet header insert function prototype.
+ * @param hdr buffer to insert header into
+ * @param pcktHdrLe linked list of packet headers
+ */
+typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe);
 
 #define PcktPropsHeader \
+    PcktHeaderInsertFn hdrInsFn; /**< Header insert function pointer (used only on transmission)  */ \
     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. */ \
+    bool8_t validityOK; /**< Indicates that checksum is OK. */                                     \
-
 
 typedef struct {
     PcktPropsHeader
 } PcktProps;
 
-struct EthInterface_;
-struct PcktHeaderElement_;
-
 /**
  * @typedef int (*PcktProcFn)(const uint8_t *pHdr, uint32_t size, uint8_t *pPcktProps);
  * @brief Pckt processing function template.
@@ -59,6 +66,7 @@ typedef struct PcktClassDesc_ {
     uint16_t class; ///< Type identification of the packet 'class' (unique!)
     uint16_t containerClass; ///< Type of container packet packet (e.g.: IPv4 in case of UDP)
     PcktProcFn procFun; ///< Pckt processing function
+    PcktHeaderInsertFn hdrInsFn; ///< Header insert function
     uint16_t propertySize; ///< Size of property structure
 } PcktClassDesc;
 

packet_sieve.c

@@ -82,7 +82,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
         const PcktSieveLayer * nodeIter = layer->nodes;
         found = false;
         while (nodeIter && !found) {
-            found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props); // specific or general match
+            found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props, intf); // specific or general match
             if (found) {
                 layer = nodeIter; // advance in the sieve tree
                 const PcktHeaderElement * containedHeader = headerIter->next; // advance on headers
@@ -204,9 +204,9 @@ bool packsieve_remove_layer(PcktSieveLayer * layer) {
 
 void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
     if (*layer->infoTag != '\0') {
-        INFO("%*c\\--|%s|---\n", indent, ' ', layer->infoTag);
+        INFO("%*c└─┤%s├───\n", indent, ' ', layer->infoTag);
     } else {
-        INFO("%*c\\--|%d|---\n", indent, ' ', layer->packetClass);
+        INFO("%*c└─┤%d├───\n", indent, ' ', layer->packetClass);
     }
     const PcktSieveLayer * nodeIter = layer->nodes;
     while(nodeIter) {
@@ -214,3 +214,18 @@ void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
         nodeIter = nodeIter->next;
     }
 }
+
+void pckthdr_chain_free(PcktHeaderElement *hdr) {
+    // rewind
+    while (hdr->prev != NULL) {
+        hdr = hdr->prev;
+    }
+
+    // free
+    PcktHeaderElement * next;
+    while (hdr != NULL) {
+        next = hdr->next;
+        dynmem_free(hdr);
+        hdr = next;
+    }
+}

packet_sieve.h

@@ -14,6 +14,12 @@ typedef struct PcktHeaderElement_ {
     PcktProps props; ///< Properties (allocated to appropriate size)
 } PcktHeaderElement;
 
+/**
+ * Free chain of packet header. List is auto-rewound.
+ * @param hdr pointer to header chain element
+ */
+void pckthdr_chain_free(PcktHeaderElement * hdr);
+
 #define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *))
 
 /**
@@ -44,10 +50,12 @@ bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilter
  */
 void packfiltcond_zero(PcktSieveFilterCondition * cond);
 
+struct EthInterface_;
+
 /**
  * Sieve filter function type.
  */
-typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps);
+typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, struct EthInterface_ * intf);
 
 struct PcktSieveLayer_;
 
@@ -79,8 +87,6 @@ typedef struct PcktSieveLayer_ {
     char infoTag[PCKT_SIEVE_INFOTAG_LEN]; ///< Info tag length
 } PcktSieveLayer;
 
-struct EthInterface_;
-
 /**
  * Packet sieve class.
  */
@@ -105,6 +111,12 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
 /**
  * Create a new sieve filter layer.
  * @param parent parent layer
+ * @param filtCond filter condition passed to the filter callback function
+ * @param matchAny don't test against filter criteria, accept any incoming packet (e.g. pass IP packets with any destination address)
+ * @param filtFn filter callback function, returning boolean
+ * @param cbFn callback function invoked, if packet has passed filter criteria
+ * @param tag arbitrary tag, passed to callbacn function, when invoked
+ * @param pcktClass packet class associated with current sieve layer
  * @return pointer to new layer object or NULL on failure
  */
 PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass);

pckt_assembler.c

@@ -2,4 +2,66 @@
 // Created by epagris on 2023.01.11..
 //
 
+#include <stddef.h>
 #include "pckt_assembler.h"
+#include "packet_sieve.h"
+#include "dynmem.h"
+#include "eth_interface.h"
+#include "global_state.h"
+#include "utils.h"
+
+int pckt_assemble(RawPckt *raw, const Pckt *cooked) {
+    // calculate frame size
+    uint16_t frameSize = 0;
+    uint16_t headerSize = 0;
+    const PcktHeaderElement *hdrIter = cooked->header;
+
+    // rewind headers
+    while (hdrIter->prev != NULL) {
+        hdrIter = hdrIter->prev;
+    }
+
+    const PcktHeaderElement *lastHdr = hdrIter;
+    while (hdrIter != NULL) {
+        headerSize += hdrIter->props.headerSize;
+        lastHdr = hdrIter;
+        hdrIter = hdrIter->next;
+    }
+    frameSize = headerSize + cooked->payloadSize + 4; // header + payload + CRC32 checksum area
+
+    // calculate padding size
+    uint16_t padding = (frameSize < ETH_FRAME_MIN_SIZE) ? (ETH_FRAME_MIN_SIZE - frameSize) : 0;
+
+    // grow frame to minimum length if needed
+    frameSize = (frameSize < ETH_FRAME_MIN_SIZE) ? ETH_FRAME_MIN_SIZE : frameSize;
+
+    // allocate raw packet data
+    raw->payload = dynmem_alloc(frameSize);
+    raw->size = frameSize;
+
+    // insert pointer
+    uint8_t * payloadInsPtr = raw->payload + headerSize;
+    uint8_t * headerInsPtr = payloadInsPtr - lastHdr->props.headerSize;
+
+    // insert payload
+    memcpy(payloadInsPtr, cooked->payload, cooked->payloadSize);
+
+    // insert zero padding
+    memset(payloadInsPtr + cooked->payloadSize, 0, padding);
+
+    // insert reversely headers (e.g.: UDP -> IPv4 -> Ethernet)
+    hdrIter = lastHdr;
+    while (hdrIter != NULL) {
+        hdrIter->props.hdrInsFn(headerInsPtr, hdrIter); // insert header
+        hdrIter = hdrIter->prev; // step to previous header
+        if (hdrIter != NULL) {
+            headerInsPtr -= hdrIter->props.headerSize; // advance data pointer
+        }
+    }
+
+    // insert CRC32
+    uint32_t crc = crc32(raw->payload, frameSize - 4);
+    memcpy(raw->payload + frameSize - 4, (const uint8_t *)&crc, 4);
+
+    return 0;
+}

pckt_assembler.h

@@ -1,8 +1,18 @@
-//
-// Created by epagris on 2023.01.11..
-//
-
 #ifndef ETHERLIB_TEST_PCKT_ASSEMBLER_H
 #define ETHERLIB_TEST_PCKT_ASSEMBLER_H
 
+#include "packet.h"
+
+struct EthInterface_;
+
+#define ETH_FRAME_MIN_SIZE (64)
+
+/**
+ * Assemble packet.
+ * @param raw raw packet
+ * @param cooked packet information and headers
+ * @return 0 on success OR -1 on failure
+ */
+int pckt_assemble(RawPckt *raw, const Pckt *cooked);
+
 #endif //ETHERLIB_TEST_PCKT_ASSEMBLER_H

prefab/conn_blocks/arp_connblock.c

@@ -7,7 +7,7 @@
 #include "../../dynmem.h"
 #include "../packet_parsers/arp_packet.h"
 
-static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
+static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
     EthernetProps * ethProps = (EthernetProps *) contProps;
     ArpProps * arpProps = (ArpProps *) ownProps;
 

prefab/conn_blocks/icmp_connblock.c

@@ -1,5 +1,79 @@
-//
-// Created by epagris on 2023.01.04..
-//
-
 #include "icmp_connblock.h"
+
+#include <stddef.h>
+
+#include "../packet_parsers/packet_parsers.h"
+#include "../../utils.h"
+#include "../../dynmem.h"
+#include "../../pckt_assembler.h"
+#include "../../eth_interface.h"
+#include "../packet_parsers/icmp_packet.h"
+#include "ipv4_connblock.h"
+
+static bool filtIcmp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
+    IPv4Props * ipProps = (IPv4Props *) contProps;
+    IcmpProps * icmpProps = (IcmpProps *) ownProps;
+
+    return ipProps->Protocol == ETH_ICMP_PACKET_CLASS;
+}
+
+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
+
+    switch (icmpProps->type) {
+        case ICMP_MT_ECHO_REQUEST: {
+            icmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply
+            icmpProps->hdrInsFn = insert_icmp_header;
+
+            // swap source an destination IP addresses
+            IPv4Props * iPv4Props = HEADER_FETCH_PROPS(IPv4Props, pckt->header->prev);
+            ip4_addr tmpIp = iPv4Props->SourceIPAddr;
+            iPv4Props->SourceIPAddr = iPv4Props->DestIPAddr;
+            iPv4Props->DestIPAddr = tmpIp;
+            iPv4Props->hdrInsFn = insert_ipv4_header;
+
+            // swap Ethernet fields
+            EthernetAddress tmpEth;
+            EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pckt->header->prev->prev);
+            HWACPY(tmpEth, ethProps->sourceAddr);
+            HWACPY(ethProps->sourceAddr, ethProps->destAddr);
+            HWACPY(ethProps->destAddr, tmpEth);
+            ethProps->hdrInsFn = insert_ethernet_header;
+
+            // payload is the same...
+
+            // assemble packet
+            RawPckt raw;
+            pckt_assemble(&raw, pckt);
+
+            // release headers
+            pckthdr_chain_free(pckt->header);
+
+            ethinf_transmit(intf, &raw);
+        }
+            break;
+        default:
+            break;
+    }
+
+    return 0;
+}
+
+ConnBlock icmp_new_connblock(EthInterface * intf) {
+    ConnBlock icmpConnB;
+    ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block
+
+    PcktSieveFilterCondition filtCond;
+    packfiltcond_zero(&filtCond);
+    PcktSieveLayerTag tag;
+    tag.p = intf; // Ethernet-interface passed in 'tag.p'
+    icmpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtIcmp, icmp_recv_cb, tag, ETH_ICMP_PACKET_CLASS);
+    ASSERT_NULL(icmpConnB.sieveLayer);
+
+    icmpConnB.intf = intf;
+
+    SNPRINTF(icmpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "ICMP (ping)");
+
+    return icmpConnB;
+}

prefab/conn_blocks/icmp_connblock.h

@@ -1,8 +1,15 @@
-//
-// Created by epagris on 2023.01.04..
-//
-
 #ifndef ETHERLIB_TEST_ICMP_CONNBLOCK_H
 #define ETHERLIB_TEST_ICMP_CONNBLOCK_H
 
+#include "../../connection_block.h"
+
+struct EthInterface_;
+
+/**
+ * Create ICMP connection block.
+ * @param intf associated Ethernet interface
+ * @return connection block
+ */
+ConnBlock icmp_new_connblock(struct EthInterface_ * intf);
+
 #endif //ETHERLIB_TEST_ICMP_CONNBLOCK_H

prefab/conn_blocks/igmp_connblock.c

@@ -1,5 +1,120 @@
-//
-// Created by epagris on 2023.01.14..
-//
-
 #include "igmp_connblock.h"
+
+#include <stddef.h>
+
+#include "../packet_parsers/packet_parsers.h"
+#include "../../utils.h"
+#include "../../dynmem.h"
+#include "../../pckt_assembler.h"
+#include "../../eth_interface.h"
+#include "ipv4_connblock.h"
+#include "../packet_parsers/igmp_packet.h"
+
+static bool filtIgmp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
+    IPv4Props * ipProps = (IPv4Props *) contProps;
+    IgmpProps * icmpProps = (IgmpProps *) ownProps;
+
+    return ipProps->Protocol == ETH_IGMP_PACKET_CLASS;
+}
+
+ConnBlock igmp_new_connblock(struct EthInterface_ *intf) {
+    ConnBlock igmpConnB;
+    ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block
+
+    PcktSieveFilterCondition filtCond;
+    packfiltcond_zero(&filtCond);
+    PcktSieveLayerTag tag;
+    tag.p = intf; // Ethernet-interface passed in 'tag.p'
+    igmpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtIgmp, NULL, tag, ETH_IGMP_PACKET_CLASS);
+    ASSERT_NULL(igmpConnB.sieveLayer);
+
+    igmpConnB.intf = intf;
+
+    SNPRINTF(igmpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IGMP");
+
+    return igmpConnB;
+}
+
+static void igmp_send(ConnBlock * connBlock, ip4_addr ga, int mt) {
+    // allocate headers
+    PcktHeaderElement * igmpHeader = ALLOC_HEADER_ELEMENT(IgmpProps);
+    PcktHeaderElement * ipHeader = ALLOC_HEADER_ELEMENT(IPv4Props);
+    PcktHeaderElement * ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
+
+    igmpHeader->next = NULL;
+    igmpHeader->prev = ipHeader;
+    ipHeader->next = igmpHeader;
+    ipHeader->prev = ethHeader;
+    ethHeader->next = ipHeader;
+    ethHeader->prev = NULL;
+
+    // prepare headers
+    IgmpProps * igmpProps = HEADER_FETCH_PROPS(IgmpProps, igmpHeader);
+    IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, ipHeader);
+    EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);
+
+    // fetch sieve layers and fill transmit headers
+    PcktSieveLayer * layer = connBlock->sieveLayer; // UDP layer
+    igmpProps->type = mt;
+    igmpProps->maxRespTime = 0;
+    igmpProps->checksum = 0;
+    igmpProps->groupAddr = ga;
+
+    // common fields for packet assembly
+    igmpProps->headerSize = ETH_IGMP_HEADER_SIZE;
+    igmpProps->hdrInsFn = insert_igmp_header;
+
+    // IP
+    layer = layer->parent;
+    ipProps->IHL = ETH_IPv4_HEADER_SIZE / 4;
+    ipProps->Version = 4;
+    ipProps->DSF = 0x00;
+    ipProps->TotalLength = ETH_IPv4_HEADER_SIZE + ETH_IGMP_HEADER_SIZE;
+    // Identification is filled on header insertion
+    ipProps->Flags = 0x00;
+    ipProps->FragmentOffset = 0x00;
+    ipProps->TTL = 255;
+    ipProps->Protocol = ETH_IGMP_PACKET_CLASS;
+    // HeaderChecksum is calculated on header insertion
+    ipProps->SourceIPAddr = connBlock->intf->ip;
+    ipProps->DestIPAddr = ga;
+
+    // common fields for packet assembly
+    ipProps->hdrInsFn = insert_ipv4_header;
+    ipProps->headerSize = ETH_IPv4_HEADER_SIZE;
+
+    // Ethernet
+    ethmc_from_ipmc(ethProps->destAddr, ga);
+    memcpy(ethProps->sourceAddr, connBlock->intf->mac, ETH_HW_ADDR_LEN);
+    ethProps->length_type = ETH_IPv4_PACKET_CLASS;
+
+    // common fields for packet assembly
+    ethProps->hdrInsFn = insert_ethernet_header;
+    ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
+
+    Pckt cooked;
+    cooked.payload = NULL;
+    cooked.payloadSize = 0;
+    cooked.header = ethHeader;
+
+    // NOT FILLED FIELDS
+    cooked.headerSize = 0;
+    cooked.time_s = 0;
+    cooked.time_ns = 0;
+
+    RawPckt raw;
+    pckt_assemble(&raw, &cooked);
+
+    ethinf_transmit(connBlock->intf, &raw);
+
+    // free headers
+    pckthdr_chain_free(ethHeader);
+}
+
+void igmp_report_membership(ConnBlock * connBlock, ip4_addr ga) {
+    igmp_send(connBlock, ga, IGMP_MT_MEMBERSHIP_REPORT);
+}
+
+void igmp_leave_group(ConnBlock *connBlock, ip4_addr ga) {
+    igmp_send(connBlock, ga, IGMP_MT_LEAVE_GROUP);
+}

prefab/conn_blocks/igmp_connblock.h

@@ -1,8 +1,30 @@
-//
-// Created by epagris on 2023.01.14..
-//
-
 #ifndef ETHERLIB_TEST_IGMP_CONNBLOCK_H
 #define ETHERLIB_TEST_IGMP_CONNBLOCK_H
 
+#include "../../connection_block.h"
+#include "../packet_parsers/ipv4_types.h"
+
+struct EthInterface_;
+
+/**
+ * Create IGMP connection block.
+ * @param intf associated Ethernet interface
+ * @return connection block
+ */
+ConnBlock igmp_new_connblock(struct EthInterface_ * intf);
+
+/**
+ * Report IGMP membership.
+ * @param connBlock pointer to IGMP connblock
+ * @param ga IPv4 group address
+ */
+void igmp_report_membership(ConnBlock * connBlock, ip4_addr ga);
+
+/**
+ * Leave IGMP group.
+ * @param connBlock pointer to IGMP connblock
+ * @param ga IPv4 group address
+ */
+void igmp_leave_group(ConnBlock * connBlock, ip4_addr ga);
+
 #endif //ETHERLIB_TEST_IGMP_CONNBLOCK_H

prefab/conn_blocks/ipv4_connblock.c

@@ -7,11 +7,12 @@
 #include "../packet_parsers/packet_parsers.h"
 #include "../../utils.h"
 
-static bool filtIPv4(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
+static bool filtIPv4(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, EthInterface * intf) {
-    EthernetProps * etherProps = (EthernetProps *) contProps;
+    EthernetProps *etherProps = (EthernetProps *) contProps;
-    IPv4Props * ipProps = (IPv4Props *) ownProps;
+    IPv4Props *ipProps = (IPv4Props *) ownProps;
 
-    return etherProps->length_type == ETH_IPv4_PACKET_CLASS && IP_ADDR_FROM_FILTCOND(filtCond) == ipProps->DestIPAddr;
+    return etherProps->length_type == ETH_IPv4_PACKET_CLASS && ((IP_ADDR_FROM_FILTCOND(filtCond) == ipProps->DestIPAddr) ||
+            ((IP_ADDR_FROM_FILTCOND(filtCond) == IPv4_IF_ADDR) && (ipProps->DestIPAddr == intf->ip) && (intf->ip != 0)));
 }
 
 ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackFn cbFn) {
@@ -28,8 +29,12 @@ ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackF
     connb.intf = intf;
 
     if (!matchAny) {
-        SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: %u.%u.%u.%u",
+        if (ipAddr != IPv4_IF_ADDR) {
-                 (ipAddr & 0xFF), ((ipAddr >> 8) & 0xFF), ((ipAddr >> 16) & 0xFF), ((ipAddr >> 24) & 0xFF));
+            SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: %u.%u.%u.%u",
+                     (ipAddr & 0xFF), ((ipAddr >> 8) & 0xFF), ((ipAddr >> 16) & 0xFF), ((ipAddr >> 24) & 0xFF));
+        } else {
+            SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: INTF. ADDR.");
+        }
     } else {
         SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: ANY");
     }

prefab/conn_blocks/udp_connblock.c

@@ -9,8 +9,9 @@
 #include "../packet_parsers/packet_parsers.h"
 #include "../../utils.h"
 #include "../../dynmem.h"
+#include "etherlib/pckt_assembler.h"
 
-static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps) {
+static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, EthInterface * intf) {
     IPv4Props * ipProps = (IPv4Props *) contProps;
     UdpProps * udpProps = (UdpProps *) ownProps;
 
@@ -60,6 +61,10 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
     udpProps->Length = ETH_UDP_HEADER_SIZE + size;
     udpProps->Checksum = 0;
 
+    // common fields for packet assembly
+    udpProps->hdrInsFn = insert_udp_header;
+    udpProps->headerSize = ETH_UDP_HEADER_SIZE;
+
     // IP
     layer = layer->parent;
     ipProps->IHL = ETH_IPv4_HEADER_SIZE / 4;
@@ -75,6 +80,10 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
     ipProps->SourceIPAddr = connBlock->intf->ip;
     ipProps->DestIPAddr = addr;
 
+    // common fields for packet assembly
+    ipProps->hdrInsFn = insert_ipv4_header;
+    ipProps->headerSize = ETH_IPv4_HEADER_SIZE;
+
     // Ethernet
     layer = layer->parent;
     if (addr != 0xFFFFFFFF) {
@@ -87,42 +96,32 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
     memcpy(ethProps->sourceAddr, connBlock->intf->mac, ETH_HW_ADDR_LEN);
     ethProps->length_type = ETH_IPv4_PACKET_CLASS;
 
-    // allocate transmit buffer
+    // common fields for packet assembly
-    uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE + ETH_UDP_HEADER_SIZE;
+    ethProps->hdrInsFn = insert_ethernet_header;
-    uint32_t txBufSize = txHeaderSize + size + 4;
+    ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
-    uint8_t * txBuf = dynmem_alloc(txBufSize);
 
-    // copy payload
+    Pckt cooked;
-    memcpy(txBuf + txHeaderSize, data, size);
+    cooked.payload = data;
+    cooked.payloadSize = size;
+    cooked.header = ethHeader;
 
-    // insert UDP header
+    // NOT FILLED FIELDS
-    insert_udp_header(txBuf + ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE, udpHeader);
+    cooked.headerSize = 0;
+    cooked.time_s = 0;
+    cooked.time_ns = 0;
 
-    // insert IPv4 header
+    RawPckt raw;
-    insert_ipv4_header(txBuf + ETH_ETHERNET_HEADER_SIZE, ipHeader);
+    pckt_assemble(&raw, &cooked);
 
-    // insert Ethernet header
+    ethinf_transmit(connBlock->intf, &raw);
-    insert_ethernet_header(txBuf, ethHeader);
 
     // free headers
     dynmem_free(udpHeader);
     dynmem_free(ipHeader);
     dynmem_free(ethHeader);
 
-    // append CRC at the end
+    // release payload
-    uint32_t crc = crc32(txBuf, txBufSize - 4);
+    // dynmem_free(raw.payload);
-    memcpy(txBuf + txBufSize - 4, &crc, 4);
-
-    // send packet
-    RawPckt rpckt;
-    rpckt.size = txBufSize;
-    rpckt.payload = txBuf;
-    rpckt.time_s = 0;
-    rpckt.time_ns = 0;
-    ethinf_transmit(connBlock->intf, &rpckt);
-
-    // release transmit buffer
-    dynmem_free(txBuf);
 
     return 0;
 }

prefab/packet_parsers/arp_packet.h

@@ -43,8 +43,21 @@ typedef struct {
     ip4_addr TPA;
 } ArpProps;
 
+/**
+ * Parse ARP packet.
+ * @param hdr pointer to the packet buffer beginning with ARP header
+ * @param size size of processable buffer area
+ * @param pcktHdrLe linked list of packet headers
+ * @param intf Ethernet interface
+ * @return always 0 OR -1 on failure
+ */
 int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
 
+/**
+ * Insert ARP header into packet.
+ * @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);
 
 #endif //ETHERLIB_TEST_ARP_PACKET_H

prefab/packet_parsers/dhcp.c

@@ -8,6 +8,7 @@
 #include "../../utils.h"
 #include "../../connection_block.h"
 #include "../conn_blocks/udp_connblock.h"
+#include "etherlib/global_state.h"
 
 static struct {
     DhcpState state;
@@ -238,7 +239,7 @@ void dhcp_request(ip4_addr reqAddr, ip4_addr dhcpServerAddr) {
     memcpy(props.chaddr, s.intf->mac, ETH_HW_ADDR_LEN);
 
     DhcpOption optEnd = {DHCP_OPT_End, 0, NULL};
-    DhcpOption paramReq = {DHCP_OPT_ParamReqList, 3, &optEnd, {1, 3, 6}}; // TODO...
+    DhcpOption paramReq = {DHCP_OPT_ParamReqList, 4, &optEnd, {1, 3, 6, 51}}; // TODO...
     DhcpOption reqIp = {DHCP_OPT_RequestedIpAddress, 4, &paramReq, {IPv4_ADDR_TO_BE_BYTES(reqAddr)}};
     DhcpOption clId = {DHCP_OPT_ClientIdentifier, 7, &reqIp, {DHCP_HW_TYPE_ETHERNET, HWADDR_TO_BE_BYTES(s.intf->mac)}};
     DhcpOption msgType = {DHCP_OPT_MsgType, 1, &clId, {DHCPREQUEST}};
@@ -282,6 +283,13 @@ static void dhcp_process(DhcpProps *props, DhcpOption *opts) {
                 opt = dhcp_get_option(opts, DHCP_OPT_DomainNameServer); // get DNS
                 FETCH_DWORD(&(s.intf->dns), opt->value);
 
+                opt = dhcp_get_option(opts, DHCP_OPT_IPAddrLeaseTime); // fetch Lease Time
+                uint32_t dhcpLeaseTime_s;
+                FETCH_DWORD_H2N(&dhcpLeaseTime_s, opt->value);
+
+                AlarmUserData params = { 0 };
+                timer_sched_rel(E.tmr, ((int64_t)dhcpLeaseTime_s) * 1000000, NULL, params);
+
                 MSG("DHCP done!\n");
                 MSG("IP: ");
                 PRINT_IPv4(s.intf->ip);
@@ -291,6 +299,7 @@ static void dhcp_process(DhcpProps *props, DhcpOption *opts) {
                 PRINT_IPv4(s.intf->netmask);
                 MSG("\nDNS: ");
                 PRINT_IPv4(s.intf->dns);
+                MSG("\nLease time: %u s\n", dhcpLeaseTime_s);
                 MSG("\n");
 
                 s.state = DHCP_BOUND;

prefab/packet_parsers/dhcp.h

@@ -53,6 +53,9 @@ typedef enum {
  */
 void dhcp_initiate(EthInterface * intf);
 
+/**
+ *
+ */
 void dhcp_start();
 
 #endif //ETHERLIB_TEST_DHCP_H

prefab/packet_parsers/ethernet_frame.c

@@ -10,6 +10,8 @@
 #define ETH_ETHERTYPE_LENGTH_THRESHOLD (1500)
 #define ETH_ETHERNET_HEADER_SIZE (14)
 
+EthernetAddress ETH_ETHERNET_IPMC_HWADDR = { 0x01, 0x00, 0x5E, 0x00, 0x00, 0x00 };
+
 int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
     EthernetProps * frameProps = (EthernetProps *) &pcktHdrLe->props;
     FETCH_ADVANCE(frameProps->destAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address

prefab/packet_parsers/ethernet_frame.h

@@ -16,16 +16,20 @@
 
 #define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
 
+#define HWACPY(dst,src) memcpy((dst),(src), ETH_HW_ADDR_LEN)
+
 typedef uint8_t EthernetAddress[ETH_HW_ADDR_LEN];
 
+extern EthernetAddress ETH_ETHERNET_IPMC_HWADDR;
+
 /**
  * @struct EthernetProps
  * Ethernet frame properties
  */
 typedef struct {
     PcktPropsHeader;
-    EthernetAddress destAddr[ETH_HW_ADDR_LEN]; ///< destination address
+    EthernetAddress destAddr; ///< destination address
-    EthernetAddress sourceAddr[ETH_HW_ADDR_LEN]; ///< source address
+    EthernetAddress sourceAddr; ///< source address
     uint16_t length_type; ///< frame length_type
 } EthernetProps;
 

prefab/packet_parsers/icmp_packet.c

@@ -3,3 +3,40 @@
 //
 
 #include "icmp_packet.h"
+#include "../../utils.h"
+#include "ipv4_packet.h"
+
+int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
+    // parse header
+    IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, pcktHdrLe);
+    FETCH_BYTE_ADVANCE(&icmpProps->type, hdr);
+    FETCH_BYTE_ADVANCE(&icmpProps->code, hdr);
+    FETCH_WORD_ADVANCE(&icmpProps->checksum, hdr);
+    FETCH_WORD_ADVANCE(&icmpProps->identifier, hdr);
+    FETCH_WORD_ADVANCE(&icmpProps->sequenceNumber, hdr);
+
+    // fill-in common fields
+    icmpProps->validityOK = true; // TODO...
+    icmpProps->containedPacketClass = 0;
+    icmpProps->headerSize = ETH_ICMP_HEADER_SIZE;
+
+    return 0;
+}
+
+void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+    uint8_t * hdrStart = hdr;
+    IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, headers);
+    FILL_BYTE_ADVANCE(hdr, &icmpProps->type);
+    FILL_BYTE_ADVANCE(hdr, &icmpProps->code);
+    uint8_t * ChkSumPtr = hdr;
+    icmpProps->checksum = 0;
+    FILL_ADVANCE(hdr, &icmpProps->checksum, 2);
+    FILL_ADVANCE(hdr, &icmpProps->identifier, 2);
+    FILL_ADVANCE(hdr, &icmpProps->sequenceNumber, 2);
+
+    IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, headers->prev);
+    icmpProps->checksum = chksum(hdrStart, ipProps->TotalLength - ETH_IPv4_HEADER_SIZE);
+    FILL_WORD_H2N_ADVANCE(ChkSumPtr, icmpProps->checksum);
+}
+
+

prefab/packet_parsers/icmp_packet.h

@@ -1,8 +1,46 @@
-//
-// Created by epagris on 2022.12.25..
-//
-
 #ifndef ETHERLIB_TEST_ICMP_PACKET_H
 #define ETHERLIB_TEST_ICMP_PACKET_H
 
+#include <stdint.h>
+#include "ipv4_types.h"
+#include "ethernet_frame.h"
+
+#define ETH_ICMP_PACKET_CLASS (0x0001)
+#define ETH_ICMP_HEADER_SIZE (8)
+
+typedef enum {
+    ICMP_MT_ECHO_REPLY = 0x00,
+    ICMP_MT_ECHO_REQUEST = 0x08
+} IcmpMsgType;
+
+/**
+ * ICMP packet properties.
+ */
+typedef struct {
+    PcktPropsHeader
+
+    uint8_t type; ///< ICMP message type
+    uint8_t code; ///< ICMP message code (~subtype)
+    uint16_t checksum; ///< Checksum for the entire packet
+    uint16_t identifier; ///< Identifier
+    uint16_t sequenceNumber; ///< Sequence number
+} IcmpProps;
+
+/**
+ * Parse ICMP packet header
+ * @param hdr pointer to the beginning of ICMP header
+ * @param size packet size
+ * @param pcktHdrLe linked list of packet headers
+ * @param intf Ethernet interface through which transmission and reception is carried out
+ * @return always 0
+ */
+int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
+
+/**
+ * Insert ICMP header.
+ * @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);
+
 #endif //ETHERLIB_TEST_ICMP_PACKET_H

prefab/packet_parsers/igmp_packet.c

@@ -3,3 +3,56 @@
 //
 
 #include "igmp_packet.h"
+#include "../../utils.h"
+
+//static uint16_t igmp_chksum(const uint8_t * hdr) {
+//    // sum fields
+//    uint32_t sum = 0;
+//    const uint16_t *pField = (const uint16_t *) hdr;
+//    for (uint8_t i = 0; i < (ETH_IGMP_HEADER_SIZE / sizeof(uint16_t)); i++) {
+//        uint16_t field = pField[i];
+//        if (i != 1) { // 6. 16-bit long field is the checksum itself, do not count in
+//            sum += field;
+//        }
+//    }
+//
+//    while (sum >> 16) {
+//        sum = (sum & 0xFFFF) + (sum >> 16);
+//    }
+//
+//    // invert result
+//    uint16_t sum16 = sum ^ 0xFFFF;
+//    return sum16;
+//}
+
+void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
+    IgmpProps *igmpProps = HEADER_FETCH_PROPS(IgmpProps, headers);
+    uint8_t * hdrBeg = hdr;
+
+    FILL_ADVANCE(hdr, &igmpProps->type, 1);
+    FILL_ADVANCE(hdr, &igmpProps->maxRespTime, 1);
+    uint8_t * ChkSumPtr = hdr;
+    igmpProps->checksum = 0;
+    FILL_WORD_ADVANCE(hdr, igmpProps->checksum);
+    FILL_DWORD_ADVANCE(hdr, igmpProps->groupAddr);
+
+    igmpProps->checksum = chksum(hdrBeg, ETH_IGMP_HEADER_SIZE);
+    memcpy(ChkSumPtr, &igmpProps->checksum, 2);
+}
+
+int parse_igmp(const uint8_t * hdr, uint32_t size, PcktHeaderElement * pcktHdrLe, struct EthInterface_ * intf) {
+    // parse header
+    IgmpProps *igmpProps = HEADER_FETCH_PROPS(IgmpProps, pcktHdrLe);
+    FETCH_BYTE_ADVANCE(&igmpProps->type, hdr);
+    FETCH_BYTE_ADVANCE(&igmpProps->maxRespTime, hdr);
+    FETCH_WORD_ADVANCE(&igmpProps->checksum, hdr);
+    FETCH_WORD_ADVANCE(&igmpProps->groupAddr, hdr);
+
+    // fill-in common fields
+    uint16_t calcChkSum = chksum(hdr, ETH_IGMP_HEADER_SIZE);
+    igmpProps->validityOK = (calcChkSum == 0);
+    igmpProps->containedPacketClass = 0;
+    igmpProps->headerSize = ETH_IGMP_HEADER_SIZE;
+
+    return 0;
+}

prefab/packet_parsers/igmp_packet.h

@@ -1,8 +1,48 @@
-//
-// Created by epagris on 2022.12.25..
-//
-
 #ifndef ETHERLIB_TEST_IGMP_PACKET_H
 #define ETHERLIB_TEST_IGMP_PACKET_H
 
+#include <stdint.h>
+#include "ipv4_types.h"
+#include "ethernet_frame.h"
+
+#define ETH_IGMP_PACKET_CLASS (0x0002)
+#define ETH_IGMP_HEADER_SIZE (8)
+
+// IGMPv2 is implemented!
+
+typedef enum {
+    IGMP_MT_MEMBERSHIP_QUERY = 0x11,
+    IGMP_MT_MEMBERSHIP_REPORT = 0x16,
+    IGMP_MT_LEAVE_GROUP = 0x17,
+} IgmpMsgType;
+
+/**
+ * IGMP packet properties.
+ */
+typedef struct {
+    PcktPropsHeader
+
+    uint8_t type; ///< IGMP message type
+    uint8_t maxRespTime; ///< Maximum response time in 1/10 seconds
+    uint16_t checksum; ///< Checksum for the entire packet
+    ip4_addr groupAddr; ///< Group address
+} IgmpProps;
+
+/**
+ * Insert IGMP header.
+ * @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);
+
+/**
+ * Parse IGMP packet.
+ * @param hdr pointer to the beginning of the IGMP header
+ * @param size size of processable buffer area
+ * @param pcktHdrLe linket list of packet headers
+ * @param intf Ethernet interface
+ * @return always 0
+ */
+int parse_igmp(const uint8_t * hdr, uint32_t size, PcktHeaderElement * pcktHdrLe, struct EthInterface_ * intf);
+
 #endif //ETHERLIB_TEST_IGMP_PACKET_H

prefab/packet_parsers/ipv4_packet.c

@@ -9,33 +9,13 @@
 
 #include "../../eth_interface.h"
 
-static uint16_t ip_checksum(const uint8_t * hdr) {
-    // sum fields
-    uint32_t sum = 0;
-    const uint16_t *pField = (const uint16_t *) hdr;
-    for (uint8_t i = 0; i < (ETH_IPv4_HEADER_SIZE / sizeof(uint16_t)); i++) {
-        uint16_t field = pField[i];
-        if (i != 5) { // 6. 16-bit long field is the checksum itself, do not count in
-            sum += field;
-        }
-    }
-
-    while (sum >> 16) {
-        sum = (sum & 0xFFFF) + (sum >> 16);
-    }
-
-    // invert result
-    uint16_t sum16 = sum ^ 0xFFFF;
-    return sum16;
-}
-
 #define ETH_IP_HEADER_LENGTH (20)
 
 static bool check_ipv4_validity(const uint8_t * hdr, const IPv4Props *ipProps) {
     bool valid =
             (ipProps->Version == 4) &&
             (ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) &&
-            (ntohs(ipProps->HeaderChecksum) == ip_checksum(hdr));
+            (ntohs(ipProps->HeaderChecksum) == chksum(hdr, ETH_IPv4_HEADER_SIZE));
     return valid;
 }
 
@@ -97,6 +77,14 @@ void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
     FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);
 
     // calculate checksum after filling header
-    uint16_t checksum = ip_checksum(hdrOrig);
+    ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE);
-    memcpy(ChkSumPtr, &checksum, 2);
+    memcpy(ChkSumPtr, &ipProps->HeaderChecksum, 2);
+}
+
+void ethmc_from_ipmc(uint8_t *hwa, ip4_addr ipa) {
+    memcpy(hwa, ETH_ETHERNET_IPMC_HWADDR, ETH_HW_ADDR_LEN);
+    const uint8_t * ipab = (const uint8_t *) &ipa;
+    hwa[3] = (hwa[3] & 0x80) | (ipab[1] & 0x7F);
+    hwa[4] = ipab[2];
+    hwa[5] = ipab[3];
 }

prefab/packet_parsers/ipv4_packet.h

@@ -4,6 +4,7 @@
 #include <stdint.h>
 #include "../../packet_registry.h"
 #include "../../packet_sieve.h"
+#include "ipv4_types.h"
 
 #define ETH_IPv4_PACKET_CLASS (0x0800)
 #define ETH_IPv4_HEADER_SIZE (20)
@@ -53,4 +54,11 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe,
  */
 void insert_ipv4_header(uint8_t * hdr, const PcktHeaderElement * headers);
 
+/**
+ * Calculate Ethernet IPv4 multicast address from multicast
+ * @param hwa
+ * @param ipa
+ */
+void ethmc_from_ipmc(uint8_t * hwa, ip4_addr ipa);
+
 #endif //ETHERLIB_IPV4_PACKET_H

prefab/packet_parsers/ipv4_types.h

@@ -6,5 +6,6 @@
 typedef uint32_t ip4_addr;
 
 #define IPv4_ANY_ADDR (0xFFFFFFFF)
+#define IPv4_IF_ADDR  (0x00000000)
 
 #endif //ETHERLIB_TEST_IPV4_TYPES_H

prefab/packet_parsers/udp_packet.c

@@ -15,35 +15,11 @@ typedef struct {
 } UdpPseudoHeader;
 
 static uint16_t udp_checksum(const UdpPseudoHeader *pseudoHeader, const uint8_t * hdr, uint32_t size) {
-    uint32_t chksum = 0;
+    uint32_t sum = chksum((const uint8_t *)pseudoHeader, sizeof(UdpPseudoHeader)) + chksum(hdr, size);
-
+    while (sum >> 16) {
-    // pseudoheader
+        sum = (sum & 0xFFFF) + (sum >> 16);
-    for (uint16_t i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) {
-        uint16_t field = ((uint16_t *) pseudoHeader)[i];
-        field = htons(field);
-        chksum += field;
     }
-
+    return sum;
-    // UDP header and hdr
-    for (uint16_t i = 0; i < size / 2; i++) {
-        if (i != 3) { // skip Checksum field
-            uint16_t field = ((uint16_t *) hdr)[i];
-            field = htons(field);
-            chksum += field;
-        }
-    }
-
-    // pad if packet size is odd
-    if (size % 2) {
-        chksum += ((uint16_t) (hdr[size - 1] << 8));
-    }
-
-    while (chksum >> 16) {
-        chksum = (chksum & 0xFFFF) + (chksum >> 16);
-    }
-
-    uint16_t sum16 = ~(chksum & 0xFFFF);
-    return sum16;
 }
 
 int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
@@ -67,12 +43,15 @@ void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
     FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort);
     FILL_WORD_H2N_ADVANCE(hdr, udpProps->DestinationPort);
     FILL_WORD_H2N_ADVANCE(hdr, udpProps->Length);
+    uint8_t *ChkSumPtr = hdr;
+    udpProps->Checksum = 0;
+    FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
 
     // calculate checksum
     const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props;
     UdpPseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)};
     udpProps->Checksum = udp_checksum(&ph, hdrBegin, udpProps->Length);
-    FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
+    memcpy(ChkSumPtr, &udpProps->Checksum, 2);
 }
 
 

timer.c

@@ -2,21 +2,196 @@
 // Created by epagris on 2022.12.21..
 //
 
+#include <stdlib.h>
 #include "timer.h"
 #include "dynmem.h"
 #include "utils.h"
 
+int64_t time_to_us(const TimePoint * t) {
+    return (int64_t)t->s * 1000000 + (int64_t)t->us;
+}
+
+void time_from_us(TimePoint * t, int64_t us) {
+    t->s = us / 1000000;
+    t->us = us - ((int64_t)t->s * 1000000);
+}
+
+void time_add_us(TimePoint * t, int64_t us) {
+    time_from_us(t, time_to_us(t) + us);
+}
+
+// ------------------------------
+
 Timer *timer_new(uint32_t maxSched) {
-    uint32_t tmrHdrSize = sizeof(TimePoint) + sizeof(AlarmAssignment *) + 2 * sizeof(uint32_t);
+    Timer * tmr = (Timer *) dynmem_alloc(sizeof(Timer) + maxSched * sizeof(AlarmAssignment));
-    Timer * tmr = (Timer *) dynmem_alloc(tmrHdrSize + maxSched * sizeof(AlarmAssignment));
     ASSERT_NULL(tmr);
 
     tmr->maxSched = maxSched;
     tmr->nSched = 0;
     tmr->nextAlarm = NULL;
     tmr->time.s = 0;
-    tmr->time.ns = 0;
+    tmr->time.us = 0;
     memset(tmr->alarms, 0, maxSched * sizeof(AlarmAssignment));
 
     return tmr;
 }
+
+static AlarmAssignment * timer_get_alarm_by_id(Timer * tmr, uint32_t id) {
+    AlarmAssignment * slot = NULL;
+    for (uint32_t i = 0; (i < tmr->maxSched) && (slot == NULL); i++) {
+        if (tmr->alarms[i].id == id) {
+            slot = (tmr->alarms) + i;
+        }
+    }
+    return slot;
+}
+
+static uint32_t timer_generate_id(Timer * tmr) {
+    uint32_t newId = 0;
+    do {
+        newId = ((uint32_t) rand()) | 0x01; // ID cannot be 0
+
+    } while (timer_get_alarm_by_id(tmr, newId) != NULL);
+    return newId;
+}
+
+static void timer_update_nearest_alarm(Timer * tmr) {
+    if (tmr->nSched == 0) {
+        tmr->nextAlarm = NULL;
+        return;
+    }
+
+    int64_t t_c_us = time_to_us(&tmr->time);
+    int64_t min_delta_t_us = 0;
+
+    AlarmAssignment * nearest = NULL;
+    for (uint32_t i = 0; i < tmr->maxSched; i++) {
+        int64_t t_i_us = time_to_us(&(tmr->alarms[i].time));
+        if ((nearest == NULL) && (tmr->alarms[i].id != 0)) {
+            nearest = tmr->alarms + i;
+            min_delta_t_us = t_i_us - t_c_us;
+        } else {
+            int64_t delta_t_us = t_i_us - t_c_us;
+            if (delta_t_us < min_delta_t_us) {
+                min_delta_t_us = delta_t_us;
+                nearest = tmr->alarms + i;
+            }
+        }
+    }
+
+    tmr->nextAlarm = nearest;
+}
+
+uint32_t timer_sched(Timer * tmr, const TimePoint * t, TimerAlarmCb cb, AlarmUserData params) {
+    if (tmr->nSched == tmr->maxSched) { // if cannot schedule more alarm
+        return TIMER_SCHED_FAILED;
+    }
+
+    // if we can schedule get the first unused block
+    AlarmAssignment * slot = timer_get_alarm_by_id(tmr, 0);
+
+    // allocate on the first free slot
+    slot->id = timer_generate_id(tmr);
+    slot->cb = cb;
+    slot->time = *t;
+    slot->params = params;
+
+    // increase allocation count
+    tmr->nSched++;
+
+    // replace nearest if needed
+    if (tmr->nSched > 1) {
+        timer_update_nearest_alarm(tmr);
+    } else {
+        tmr->nextAlarm = slot;
+    }
+
+    return slot->id;
+}
+
+uint32_t timer_sched_rel(Timer * tmr, int64_t us, TimerAlarmCb cb, AlarmUserData params) {
+    TimePoint t = tmr->time;
+    time_add_us(&t, us);
+    return timer_sched(tmr, &t, cb, params);
+}
+
+void timer_unsched(Timer * tmr, uint32_t id) {
+    AlarmAssignment * alarm = timer_get_alarm_by_id(tmr, id);
+    if (alarm != NULL) {
+        memset(alarm, 0, sizeof(AlarmAssignment));
+        tmr->nSched--;
+    }
+}
+
+void timer_set_time(Timer *tmr, const TimePoint *t) {
+    tmr->time = *t;
+}
+
+int64_t timer_get_time_us(const Timer * tmr) {
+    return time_to_us(&tmr->time);
+}
+
+TimePoint timer_get_time(const Timer * tmr) {
+    return tmr->time;
+}
+
+void timer_tick(Timer * tmr, int64_t us) {
+    // advance time
+    time_add_us(&tmr->time, us);
+    /*t_us += us;
+    time_from_us(&tmr->time, t_us);*/
+
+    int64_t t_us = timer_get_time_us(tmr);
+
+    if ((tmr->nSched > 0) && (tmr->nextAlarm != NULL)) {
+        int64_t t_alarm = time_to_us(&(tmr->nextAlarm->time));
+        while (((t_alarm - t_us) <= 0) && (tmr->nSched > 0)) {
+            // invoke callback
+            if (tmr->nextAlarm->cb != NULL) {
+                tmr->nextAlarm->cb(tmr, tmr->nextAlarm->params);
+            }
+
+            // clear alarm descriptor
+            memset(tmr->nextAlarm, 0, sizeof(AlarmAssignment));
+
+            // decrease scheduled alarm count
+            tmr->nSched--;
+
+            // update nearest alarm
+            timer_update_nearest_alarm(tmr);
+            if (tmr->nextAlarm != NULL) {
+                t_alarm = time_to_us(&(tmr->nextAlarm->time));
+            }
+        }
+    }
+}
+
+void timer_report(const Timer * tmr) {
+    MSG("Time: %u.%06u\n\n", tmr->time.s, tmr->time.us);
+
+    int64_t t_c_us = timer_get_time_us(tmr);
+    if (tmr->nSched > 0) {
+        for (uint32_t i = 0; i < tmr->maxSched; i++) {
+
+            if (tmr->alarms[i].id == 0) {
+                continue;
+            }
+
+            int64_t t_delta_us = time_to_us(&tmr->alarms[i].time) - t_c_us;
+            if (t_delta_us < 1000000) {
+                MSG("─ alarm (#%X) in %li us", tmr->alarms[i].id, t_delta_us);
+            } else {
+                TimePoint dt;
+                time_from_us(&dt, t_delta_us);
+                MSG("─ alarm (#%X) in %i.%06i s", tmr->alarms[i].id, dt.s, dt.us);
+            }
+
+            if ((tmr->alarms + i) == tmr->nextAlarm) {
+                MSG(" <-- NEXT ALARM\n");
+            } else {
+                MSG("\n");
+            }
+        }
+    }
+    MSG("\nSchedules: %u/%u\n", tmr->nSched, tmr->maxSched);
+}

timer.h

@@ -2,18 +2,44 @@
 #define ETHERLIB_TEST_TIMER_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 /**
  * A single point in time
  */
 typedef struct {
     uint32_t s; ///< Seconds
-    uint32_t ns; ///< Nanoseconds
+    uint32_t us; ///< Microseconds
 } TimePoint;
 
+/**
+ * Convert TimePoint to microseconds.
+ * @param t time
+ * @return time in microseconds
+ */
+int64_t time_to_us(const TimePoint * t);
+
+/**
+ * Convert microseconds to TimePoint.
+ * @param t pointer to TimePoint structure
+ * @param us time in microseconds
+ */
+void time_from_us(TimePoint * t, int64_t us);
+
+/**
+ * Add microseconds to TimePoint.
+ * @param t pointer to TimePoint structure
+ * @param us time in microseconds
+ */
+void time_add_us(TimePoint * t, int64_t us);
+
+#define TIMER_SCHED_FAILED (-1)
 
 struct Timer_;
 
+/**
+ * Alarm user data.
+ */
 typedef union {
     void * ptr;
     uint32_t u;
@@ -21,7 +47,11 @@ typedef union {
 
 typedef void (*TimerAlarmCb)(struct Timer_ * timer, AlarmUserData user);
 
+/**
+ * Alarm assignment.
+ */
 typedef struct {
+    uint32_t id; ///< Timing ID
     TimePoint time; ///< Alarm time
     TimerAlarmCb cb; ///< Pointer to callback function
     AlarmUserData params; ///< User data passed to callback function
@@ -45,6 +75,65 @@ typedef struct Timer_ {
  */
 Timer * timer_new(uint32_t maxSched);
 
+/**
+ * Schedule new alarm.
+ * @param tmr pointer to Timer instance
+ * @param t alarm time
+ * @param cb alarm callback function
+ * @param params user-defined params passed to callback function
+ * @return ID of new schedule OR TIMER_SCHED_FAILED
+ */
+uint32_t timer_sched(Timer * tmr, const TimePoint * t, TimerAlarmCb cb, AlarmUserData params);
 
+/**
+ * Schedule new alarm by specifying time relatively from the current time point.
+ * @param tmr pointer to Timer instance
+ * @param us relative alarm time
+ * @param cb alarm callback function
+ * @param params user-defined params passed to callback function
+ * @return ID of new schedule OR TIMER_SCHED_FAILED
+ */
+uint32_t timer_sched_rel(Timer * tmr, int64_t us, TimerAlarmCb cb, AlarmUserData params);
+
+/**
+ * Unschedule alarm.
+ * @param tmr pointer to Timer instance
+ * @param id schedule ID returned by timer_sched() or timer_sched_rel()
+ */
+void timer_unsched(Timer * tmr, uint32_t id);
+
+/**
+ * Set time.
+ * @param tmr pointer to Timer instance
+ * @param t time
+ */
+void timer_set_time(Timer * tmr, const TimePoint * t);
+
+/**
+ * Get current time in microseconds.
+ * @param tmr pointer to Timer instance
+ * @return current time in microseconds
+ */
+int64_t timer_get_time_us(const Timer * tmr);
+
+/**
+ * Get current time.
+ * @param tmr pointer to Timer instance
+ * @return current time
+ */
+TimePoint timer_get_time(const Timer * tmr);
+
+/**
+ * Advance timer.
+ * @param tmr pointer to Timer instance
+ * @param us time advancement in microseconds
+ */
+void timer_tick(Timer * tmr, int64_t us);
+
+/**
+ * Print Timer report.
+ * @param tmr pointer to Timer instance
+ */
+void timer_report(const Timer * tmr);
 
 #endif //ETHERLIB_TEST_TIMER_H

utils.c

@@ -59,3 +59,25 @@ uint32_t crc32(const uint8_t * data, uint32_t size) {
     checksum = ~checksum;
     return checksum;
 }
+
+uint16_t chksum(const uint8_t * data, uint32_t size) {
+    // sum fields
+    uint32_t sum = 0;
+    const uint16_t *pField = (const uint16_t *) data;
+    for (uint8_t i = 0; i < (size / sizeof(uint16_t)); i++) {
+        uint16_t field = pField[i];
+        sum += field;
+    }
+
+    if (size % 2) {
+        sum += ((uint16_t) (data[size - 1] << 8));
+    }
+
+    while (sum >> 16) {
+        sum = (sum & 0xFFFF) + (sum >> 16);
+    }
+
+    // invert result
+    uint16_t sum16 = sum ^ 0xFFFF;
+    return sum16;
+}

utils.h

@@ -50,15 +50,17 @@
 #define FETCH_BYTE_ADVANCE(dst,src) FETCH_ADVANCE(dst,src,1)
 #define FILL_BYTE_ADVANCE(dst,src) FILL_ADVANCE(dst,src,1)
 #define FETCH_WORD_H2N_ADVANCE(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2); (w) += 2; }
+#define FETCH_WORD_ADVANCE(dst,w) { memcpy(dst, w, 2); (w) += 2; }
 #define FETCH_WORD_H2N(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2) }
-#define FETCH_WORD(dst,w) { uint16_t u; memcpy(&u, w, 2); memcpy((dst), &u, 2) }
+#define FETCH_WORD(dst,w) memcpy(dst, w, 2)
+#define FILL_WORD_ADVANCE(dst,w) { memcpy((dst), &(w), 2); (dst) += 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 FETCH_DWORD_H2N(dst,dw) { uint32_t du; memcpy(&du, dw, 4); du = htonl(du); memcpy((dst), &du, 4); }
 #define FETCH_DWORD(dst,dw) { uint32_t du; memcpy(&du, dw, 4); memcpy((dst), &du, 4); }
 #define FETCH_DWORD_ADVANCE(dst,dw) { uint32_t du; memcpy(&du, dw, 4); memcpy((dst), &du, 4); (dw) += 4; }
 #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; }
+#define FILL_DWORD_ADVANCE(dst,dw) { memcpy(dst, &(dw), 4); (dst) += 4; }
 
 #define MIN(a,b) (((a) < (b)) ? (a) : (b))
 #define MAX(a,b) (((a) > (b)) ? (a) : (b))
@@ -73,4 +75,12 @@
  */
 uint32_t crc32(const uint8_t * data, uint32_t size);
 
+/**
+ * Compute IP/UDP/ICMP/TCP etc. 16-bit checksum.
+ * @param data pointer to data block to process
+ * @param size size of data block
+ * @return checksum
+ */
+uint16_t chksum(const uint8_t * data, uint32_t size);
+
 #endif //ETHERLIB_UTILS_H