DHCP initially working; ARP basics OK; memory leaks handled

arp_cache.c

@@ -6,11 +6,34 @@
 #include "arp_cache.h"
 #include "dynmem.h"
 #include "utils.h"
+#include "eth_interface.h"
+#include "prefab/conn_blocks/arp_connblock.h"
+#include "prefab/packet_parsers/arp_packet.h"
+#include "etherlib_options.h"
 
-ArpCache *arpc_new(uint16_t size) {
+static int arpc_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) {
-    ArpCache * arcp = (ArpCache *) dynmem_alloc(2 * sizeof(uint16_t) + size * sizeof(ArpEntry));
+    ArpProps * arpProps = HEADER_FETCH_PROPS(ArpProps, pckt->header);
+    EthInterface * intf = (EthInterface *) tag.p; // arp_new_connblock() puts pointer to intf into tag field
+    bool weAreCalled = (arpProps->OPER == ARPOP_REQ) && (arpProps->PTYPE == ETH_IPv4_PACKET_CLASS) &&
+            (arpProps->TPA == intf->ip) && (intf->ip != 0);
+
+    ArpCache * arpc = intf->arpc;
+
+    if (weAreCalled) {
+        arpc_respond(arpc, arpProps->SHA, arpProps->SPA);
+    }
+
+    return 0;
+}
+
+ArpCache *arpc_new(EthInterface * intf, uint16_t size) {
+    // allocate table
+    ArpCache * arcp = (ArpCache *) dynmem_alloc(2 * sizeof(uint16_t) + sizeof(ConnBlock) + size * sizeof(ArpEntry));
     arcp->size = size;
     arcp->fill = 0;
+
+    // create connblock
+    arcp->cb = arp_new_connblock(intf, arpc_recv_cb);
     return arcp;
 }
 
@@ -32,6 +55,7 @@ void arpc_learn(ArpCache *arpc, const ArpEntry *newEntry) {
 }
 
 const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip) {
+    // search in the cache
     for (uint16_t i = 0; i < arpc->fill; i++) {
         ArpEntry * entry = arpc->entries + i;
         if (entry->ip == ip) {
@@ -42,6 +66,23 @@ const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip) {
     return NULL;
 }
 
+const ArpEntry *arpc_get_ask(ArpCache *arpc, ip4_addr ip) {
+    const ArpEntry * entry = arpc_get(arpc, ip);
+    if (entry != NULL) {
+        return entry;
+    }
+
+    // not found in the cache, probe the network
+    uint32_t attemptN = 0;
+    while (((entry = arpc_get(arpc, ip)) == NULL) && (attemptN < ETHLIB_ARP_RETRY_COUNT)) {
+        arpc_ask(arpc, ip);
+        ETHLIB_SLEEP(20);
+        //attemptN++;
+    }
+
+    return entry;
+}
+
 void arpc_dump(ArpCache *arpc) {
     for (uint16_t i = 0; i < arpc->fill; i++) {
         ArpEntry * entry = arpc->entries + i;
@@ -50,3 +91,37 @@ void arpc_dump(ArpCache *arpc) {
     }
     MSG("\n");
 }
+
+void arpc_ask(ArpCache *arpc, ip4_addr addr) {
+    ArpProps arpProps;
+
+    arpProps.HTYPE = 1;
+    arpProps.PTYPE = ETH_IPv4_PACKET_CLASS;
+    arpProps.HLEN = ETH_HW_ADDR_LEN;
+    arpProps.PLEN = sizeof(ip4_addr);
+    arpProps.OPER = ARPOP_REQ;
+
+    memcpy(arpProps.SHA, arpc->cb.intf->mac, ETH_HW_ADDR_LEN);
+    arpProps.SPA = arpc->cb.intf->ip;
+    memset(arpProps.THA, 0x00, ETH_HW_ADDR_LEN);
+    arpProps.TPA = addr;
+
+    arp_send(&arpc->cb, &arpProps);
+}
+
+void arpc_respond(ArpCache *arpc, const EthernetAddress hwAddr, ip4_addr ipAddr) {
+    ArpProps arpProps;
+
+    arpProps.HTYPE = 1;
+    arpProps.PTYPE = ETH_IPv4_PACKET_CLASS;
+    arpProps.HLEN = ETH_HW_ADDR_LEN;
+    arpProps.PLEN = sizeof(ip4_addr);
+    arpProps.OPER = ARPOP_REP;
+
+    memcpy(arpProps.SHA, arpc->cb.intf->mac, ETH_HW_ADDR_LEN);
+    arpProps.SPA = arpc->cb.intf->ip;
+    memcpy(arpProps.THA, hwAddr, ETH_HW_ADDR_LEN);
+    arpProps.TPA = ipAddr;
+
+    arp_send(&arpc->cb, &arpProps);
+}

arp_cache.h

@@ -3,9 +3,13 @@
 
 #include "prefab/packet_parsers/ethernet_frame.h"
 #include "prefab/packet_parsers/ipv4_types.h"
+#include "connection_block.h"
 
 #define ETH_ARP_PACKET_CLASS (0x0806)
 
+struct EthInterface_;
+
+
 /**
  * ARP cache entry record.
  */
@@ -20,15 +24,17 @@ typedef struct {
 typedef struct {
     uint16_t size; ///< Number of cache entries
     uint16_t fill; ///< Fill level
+    ConnBlock cb; ///< Connection block for sending and receiving ARP requests
     ArpEntry entries[]; ///< Cache entries
 } ArpCache;
 
 /**
  * Create new ARP cache table.
+ * @param intf Related Ethernet interface
  * @param size Number of available entries
  * @return Pointer to newly allocated ARP cache
  */
-ArpCache * arpc_new(uint16_t size);
+ArpCache * arpc_new(struct EthInterface_ * intf, uint16_t size);
 
 /**
  * Learn new assignment.
@@ -50,6 +56,12 @@ const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip);
  */
 void arpc_free(ArpCache * aprc);
 
+void arpc_ask(ArpCache *arpc, ip4_addr addr);
+
+void arpc_respond(ArpCache *arpc, const EthernetAddress hwAddr, ip4_addr ipAddr);
+
+const ArpEntry *arpc_get_ask(ArpCache *arpc, ip4_addr ip);
+
 /**
  * Dump ARP cache contents.
  * @param arpc

connection_block.c

@@ -3,6 +3,7 @@
 //
 
 #include "connection_block.h"
+#include "eth_interface.h"
 
 #include <stddef.h>
 

connection_block.h

@@ -2,19 +2,18 @@
 #define ETHERLIB_CONNECTION_BLOCK_H
 
 #include "packet_sieve.h"
-#include "eth_interface.h"
 
 struct ConnBlock_;
 
-typedef int (*ConnBlockTransmitFn)(EthInterface * intf, const uint8_t * data, uint32_t size, const struct ConnBlock_ * connBlock);
+typedef int (*ConnBlockTransmitFn)(struct EthInterface_ * intf, const uint8_t * data, uint32_t size, const struct ConnBlock_ * connBlock);
 
 /**
  * Connection block.
  */
 typedef struct ConnBlock_ {
-    EthInterface * intf; ///< Ethernet interface
+    struct EthInterface_ * intf; ///< Ethernet interface
     PcktSieveLayer * sieveLayer; ///< Sieve layer
-    ConnBlockTransmitFn transmitFn; ///< Conn-block related transmit function
+    void * tag; ///< Arbitrary tagging
 } ConnBlock;
 
 /**

dynmem.c

@@ -13,10 +13,10 @@ void dynmem_init() {
     ASSERT_NULL(E.mp);
 }
 
-void * dynmem_alloc(uint32_t size) {
+void * dynmem_alloc_(uint32_t size) {
     return mp_alloc(E.mp, size);
 }
 
-void dynmem_free(void * ptr) {
+void dynmem_free_(void * ptr) {
     mp_free(E.mp, ptr);
 }

dynmem.h

@@ -14,12 +14,24 @@ void dynmem_init();
  * @param size requested size
  * @return pointer to allocated area or NULL on failure
  */
-void * dynmem_alloc(uint32_t size);
+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__)
+#else
+#define dynmem_alloc(size) dynmem_alloc_(size)
+#endif
 
 /**
  * Release allocated block.
  * @param ptr pointer to allocated area
  */
-void dynmem_free(void * ptr);
+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)
+#else
+#define dynmem_free(ptr) dynmem_free_(ptr)
+#endif
 
 #endif //ETHERLIB_DYNMEM_H

eth_interface.c

@@ -23,7 +23,7 @@ EthInterface *ethintf_new(EthIODef * io) {
     memset(&ethIntf->sieve.layer0, 0, sizeof(PcktSieveLayer));
     ethIntf->ioDef = io;
     ethIntf->ip = 0;
-    ethIntf->arpc = arpc_new(ETHLIB_ARPCACHE_SIZE);
+    ethIntf->arpc = arpc_new(ethIntf, ETHLIB_ARPCACHE_SIZE);
     ASSERT_NULL(ethIntf->arpc);
     ethintf_register(ethIntf);
     return ethIntf;

eth_interface.h

@@ -5,6 +5,7 @@
 #include "prefab/packet_parsers/packet_parsers.h"
 #include "prefab/packet_parsers/ipv4_types.h"
 #include "arp_cache.h"
+#include "connection_block.h"
 
 /**
  * Ethernet interface low level definition.
@@ -27,7 +28,11 @@ typedef struct EthInterface_ {
     EthIODef * ioDef; ///< Low-level IO definitions
     EthernetAddress mac; ///< Ethernet address
     ip4_addr ip; ///< IP address
+    ip4_addr router; ///< Router IP address
+    ip4_addr netmask; ///< Subnet mask
+    ip4_addr dns; ///< Domain Name Server
     ArpCache * arpc; ///< ARP cache
+    ConnBlock arpCb; ///< ARP connection block
 } EthInterface;
 
 /**

memory_pool.c

@@ -65,7 +65,7 @@ uint8_t *mp_alloc(MP *mp, uint32_t size) {
             rec++;
         }
 
-        // store information on allocated block
+        // store information on allocated
         MPAllocRecord *allocated = bestBlock - 1;
         allocated->type = MPRT_ALLOCATED;
         allocated->size = size;
@@ -105,6 +105,7 @@ static void mp_join_free_blocks(MP *mp) {
                 joinIter--;
             }
             mp->freeSpace += sizeof(MPAllocRecord);
+            mp->blockRecCnt--;
         } else {
             recIter--;
         }
@@ -119,6 +120,7 @@ void mp_free(MP *mp, const uint8_t *p) {
         if ((recIter->type == MPRT_ALLOCATED) &&
             ((recIter->addrStart <= p) && ((recIter->addrStart + recIter->size) > p))) { // ...block found
             recIter->type = MPRT_FREE;
+            mp->freeSpace += recIter->size;
             success = true;
             break;
         }

packet_sieve.c

@@ -41,7 +41,9 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
         }
 
         // allocate property object
-        PcktHeaderElement *header = (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize);
+        uint32_t hdrSize = ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize;
+        PcktHeaderElement *header = (PcktHeaderElement *) dynmem_alloc(hdrSize);
+        memset(header, 0, hdrSize);
         header->props.ownPacketClass = ownClass;
         header->props.propSize = cdesc->propertySize;
         header->prev = lastHeader;
@@ -49,7 +51,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
             lastHeader->next = header;
         }
         if (outermostHeader == NULL) {
-            outermostHeader = lastHeader;
+            outermostHeader = header;
         }
 
         // call parsing function
@@ -90,7 +92,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
                     packet.payload = data + offset;
                     packet.headerSize = offset;
                     packet.payloadSize = size - offset;
-                    layer->cbFn(&packet);
+                    layer->cbFn(&packet, layer->tag);
                 }
                 headerIter = containedHeader;
             } else {
@@ -105,7 +107,13 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthI
 //        INFO("Packet headers not fully processed!\n");
 //    }
 
-    return;
+    // release header chain blocks
+    PcktHeaderElement * iter = outermostHeader;
+    while (iter != NULL) {
+        PcktHeaderElement * next = iter->next;
+        dynmem_free(iter);
+        iter = next;
+    }
 }
 
 PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass) {

packet_sieve.h

@@ -51,16 +51,16 @@ typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const P
 
 struct PcktSieveLayer_;
 
-/**
- * Callback function type for packet sieve match.
- */
-typedef int (*SieveCallBackFn)(const Pckt * pckt);
-
 typedef union {
     void * p;
     uint32_t u;
 } PcktSieveLayerTag;
 
+/**
+ * Callback function type for packet sieve match.
+ */
+typedef int (*SieveCallBackFn)(const Pckt * pckt, PcktSieveLayerTag tag);
+
 #define PCKT_SIEVE_INFOTAG_LEN (24)
 
 /**

prefab/conn_blocks/arp_connblock.c

@@ -14,14 +14,14 @@ static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps *
     return ethProps->length_type == ETH_ARP_PACKET_CLASS;
 }
 
-ConnBlock arp_new_connblock(EthInterface * intf) {
+ConnBlock arp_new_connblock(EthInterface * intf, SieveCallBackFn cb) {
     ConnBlock arpConnB; // create ARP connblock
 
     PcktSieveFilterCondition filtCond;
     packfiltcond_zero(&filtCond);
     PcktSieveLayerTag tag;
-    tag.u = 0;
+    tag.p = intf;
-    arpConnB.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, false, filtArp, NULL, tag, ETH_ARP_PACKET_CLASS);
+    arpConnB.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, false, filtArp, cb, tag, ETH_ARP_PACKET_CLASS);
     ASSERT_NULL(arpConnB.sieveLayer);
 
     arpConnB.intf = intf;
@@ -29,4 +29,56 @@ ConnBlock arp_new_connblock(EthInterface * intf) {
     SNPRINTF(arpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "ARP");
 
     return arpConnB;
+}
+
+void arp_send(const ConnBlock * connBlock, const ArpProps * props) {
+    // allocate header chain
+    PcktHeaderElement * arpHeader = ALLOC_HEADER_ELEMENT(ArpProps);
+    PcktHeaderElement * ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps);
+    arpHeader->next = NULL;
+    arpHeader->prev = ethHeader;
+    ethHeader->next = arpHeader;
+    ethHeader->prev = NULL;
+
+    // fetch props
+    ArpProps * arpProps = HEADER_FETCH_PROPS(ArpProps, arpHeader);
+    EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);
+
+    // ARP
+    *arpProps = *props;
+
+    // Ethernet
+    ethProps->length_type = ETH_ARP_PACKET_CLASS;
+    memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN); // broadcast destination
+    memcpy(ethProps->sourceAddr, connBlock->intf->mac, ETH_HW_ADDR_LEN); // source
+
+    // allocate transmit buffer
+    uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_ARP_HEADER_SIZE;
+    uint32_t txBufSize = MAX(txHeaderSize + 4, 60);
+    uint8_t * txBuf = dynmem_alloc(txBufSize);
+
+    // insert Ethernet header
+    insert_ethernet_header(txBuf, ethHeader);
+
+    // insert ARP header
+    insert_arp_header(txBuf + ETH_ETHERNET_HEADER_SIZE, arpHeader);
+
+    // release headers
+    dynmem_free(arpHeader);
+    dynmem_free(ethHeader);
+
+    // append CRC at the end
+    uint32_t crc = crc32(txBuf, txBufSize - 4);
+    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);
 }

prefab/conn_blocks/arp_connblock.h

@@ -6,10 +6,14 @@
 #include <stdint.h>
 #include "../../connection_block.h"
 #include "../../eth_interface.h"
+#include "../packet_parsers/arp_packet.h"
 
 /**
  * Create new ARP connection block.
  * @param intf associated Ethernet interface
+ * @param cbFn callback function
  * @return ARP connection block
  */
-ConnBlock arp_new_connblock(EthInterface * intf);
+ConnBlock arp_new_connblock(EthInterface * intf, SieveCallBackFn cb);
+
+void arp_send(const ConnBlock * connBlock, const ArpProps * props);

prefab/conn_blocks/udp_connblock.c

@@ -36,8 +36,6 @@ ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port,
     return udpConnB;
 }
 
-#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
-
 int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port) {
     // allocate headers
     PcktHeaderElement * udpHeader = ALLOC_HEADER_ELEMENT(UdpProps);
@@ -80,7 +78,9 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
     // Ethernet
     layer = layer->parent;
     if (addr != 0xFFFFFFFF) {
-        memset(ethProps->destAddr, 0x00, ETH_HW_ADDR_LEN); // TODO...
+        ArpCache * arpc = connBlock->intf->arpc;
+        const ArpEntry * entry = arpc_get_ask(arpc, addr);
+        memcpy(ethProps->destAddr, entry, ETH_HW_ADDR_LEN);
     } else {
         memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN);
     }
@@ -104,6 +104,11 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
     // insert Ethernet header
     insert_ethernet_header(txBuf, ethHeader);
 
+    // free headers
+    dynmem_free(udpHeader);
+    dynmem_free(ipHeader);
+    dynmem_free(ethHeader);
+
     // append CRC at the end
     uint32_t crc = crc32(txBuf, txBufSize - 4);
     memcpy(txBuf + txBufSize - 4, &crc, 4);
@@ -116,5 +121,8 @@ int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_
     rpckt.time_ns = 0;
     ethinf_transmit(connBlock->intf, &rpckt);
 
+    // release transmit buffer
+    dynmem_free(txBuf);
+
     return 0;
 }

prefab/packet_parsers/arp_packet.c

@@ -12,12 +12,12 @@ static EthernetAddress emptyEthAddr = { 0x00 };
 
 static void arp_fetch_mapping(const ArpProps * arpProps, ArpCache * arpc) {
     ArpEntry entry;
-    if ((memcmp(arpProps->SHA, emptyEthAddr, ETH_HW_ADDR_LEN)) && (arpProps->SPA != 0)) {
+    if ((memcmp(arpProps->SHA, emptyEthAddr, ETH_HW_ADDR_LEN) != 0) && (arpProps->SPA != 0)) {
         memcpy(entry.eth, arpProps->SHA, ETH_HW_ADDR_LEN);
         entry.ip = arpProps->SPA;
         arpc_learn(arpc, &entry);
     }
-    if ((memcmp(arpProps->THA, emptyEthAddr, ETH_HW_ADDR_LEN)) && (arpProps->TPA != 0)) {
+    if ((memcmp(arpProps->THA, emptyEthAddr, ETH_HW_ADDR_LEN) != 0) && (arpProps->TPA != 0)) {
         memcpy(entry.eth, arpProps->THA, ETH_HW_ADDR_LEN);
         entry.ip = arpProps->TPA;
         arpc_learn(arpc, &entry);
@@ -37,6 +37,11 @@ int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s
     FETCH_ADVANCE(&arpProps->THA, hdr, ETH_HW_ADDR_LEN); // target HW address
     FETCH_ADVANCE(&arpProps->TPA, hdr, sizeof(ip4_addr)); // target protocol address
 
+    // fill-in common packet header fields
+    arpProps->validityOK = true;
+    arpProps->containedPacketClass = 0;
+    arpProps->headerSize = ETH_ARP_HEADER_SIZE;
+
     // learn new mapping
     arp_fetch_mapping(arpProps, intf->arpc);
     return 0;

prefab/packet_parsers/arp_packet.h

@@ -12,6 +12,7 @@
 #include "../../packet_sieve.h"
 
 #define ETH_ARP_PACKET_CLASS (0x0806)
+#define ETH_ARP_HEADER_SIZE (28)
 
 /**
  * Arp operations
@@ -29,6 +30,8 @@ typedef enum {
  * ARP properties.
  */
 typedef struct {
+    PcktPropsHeader
+
     uint16_t HTYPE; ///< Hardware type (Ethernet: 1)
     uint16_t PTYPE; ///< Protocol type (IP: 0x0800)
     uint8_t HLEN; ///< Hardware address length
@@ -42,4 +45,7 @@ typedef struct {
 
 int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
 
+void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers);
+
 #endif //ETHERLIB_TEST_ARP_PACKET_H
+

prefab/packet_parsers/dhcp.c

@@ -10,13 +10,14 @@
 #include "../conn_blocks/udp_connblock.h"
 
 static struct {
-   DhcpState state;
+    DhcpState state;
-   void * buf;
+    void *buf;
-   ConnBlock connb;
+    ConnBlock connb;
-   uint32_t tranId;
+    uint32_t tranId;
+    EthInterface *intf;
 } s;
 
-static const uint8_t DHCP_MAGIC_COOKIE[] = { 99, 130, 83, 99 };
+static const uint8_t DHCP_MAGIC_COOKIE[] = {99, 130, 83, 99};
 
 #define SNAME_LEN (64)
 #define FILE_LEN (128)
@@ -32,14 +33,41 @@ typedef enum {
     DHCPRELEASE = 7
 } DhcpMsgType;
 
-static void dhcp_option_insert_msg_type(uint8_t ** bufPtr, int msgType) {
+// DHCP option IDs
+typedef enum {
+    DHCP_OPT_SubnetMask = 0x1,
+    DHCP_OPT_Router = 0x3,
+    DHCP_OPT_DomainNameServer = 0x6,
+    DHCP_OPT_RequestedIpAddress = 0x32,
+    DHCP_OPT_IPAddrLeaseTime = 0x33,
+    DHCP_OPT_MsgType = 0x35,
+    DHCP_OPT_ServerId = 0x36,
+    DHCP_OPT_ParamReqList = 0x37,
+    DHCP_OPT_MaxMsgSize = 0x39,
+    DHCP_OPT_ClientIdentifier = 0x3d,
+    DHCP_OPT_End = 0xFF,
+} DhcpOptionId;
+
+#define MAX_DHCP_OPTION_LENGTH (16)
+
+#define DHCP_HW_TYPE_ETHERNET (1)
+
+// DHCP options
+typedef struct DhcpOption_ {
+    uint8_t id; ///< Options
+    uint8_t length; ///< Option data length
+    struct DhcpOption_ *next; /// next DHCP option
+    uint8_t value[MAX_DHCP_OPTION_LENGTH]; ///< Option value
+} DhcpOption;
+
+static void dhcp_option_insert_msg_type(uint8_t **bufPtr, int msgType) {
     (*bufPtr)[0] = 0x35;
     (*bufPtr)[1] = 1;
     (*bufPtr)[2] = msgType;
     (*bufPtr) += 3;
 }
 
-static void dhcp_option_insert_max_msg_size(uint8_t ** bufPtr, uint16_t maxSize) {
+static void dhcp_option_insert_max_msg_size(uint8_t **bufPtr, uint16_t maxSize) {
     (*bufPtr)[0] = 0x39;
     (*bufPtr)[1] = 2;
     (*bufPtr)[2] = (maxSize >> 8) & 0xFF;
@@ -47,67 +75,253 @@ static void dhcp_option_insert_max_msg_size(uint8_t ** bufPtr, uint16_t maxSize)
     (*bufPtr) += 4;
 }
 
-static void dhcp_option_insert_end(uint8_t ** bufPtr) {
+static void dhcp_option_insert_end(uint8_t **bufPtr) {
     (*bufPtr)[0] = 0xFF;
     (*bufPtr) += 1;
 }
 
-static void dhcp_send(EthInterface * intf, DhcpOps * ops) {
+static void dhcp_read_next_option(const uint8_t **buf, DhcpOption *opt) {
-    // construct message
+    opt->id = (*buf)[0];
-    uint8_t * buf = (uint8_t *)s.buf;
+    if (opt->id != DHCP_OPT_End) {
+        opt->length = (*buf)[1];
+        memcpy(opt->value, (*buf) + 2, MIN(opt->length, MAX_DHCP_OPTION_LENGTH));
+        (*buf) += 2 + opt->length;
+    } else {
+        (*buf) += 1;
+    }
+}
+
+static void dhcp_insert_option(uint8_t **buf, const DhcpOption *opt) {
+    (*buf)[0] = opt->id;
+    if (opt->id != DHCP_OPT_End) {
+        (*buf)[1] = opt->length;
+        memcpy((*buf) + 2, opt->value, MIN(opt->length, MAX_DHCP_OPTION_LENGTH));
+        (*buf) += 2 + opt->length;
+    } else {
+        (*buf) += 1;
+    }
+}
+
+static void dhcp_free_opt_chain(DhcpOption *opt) {
+    DhcpOption *iter = opt;
+    while (iter) {
+        DhcpOption *next = iter->next;
+        dynmem_free(iter);
+        iter = next;
+    }
+}
+
+static const DhcpOption *dhcp_get_option(const DhcpOption *opts, DhcpOptionId id) {
+    const DhcpOption *iter = opts;
+    while (iter) {
+        if (iter->id == id) {
+            return iter;
+        }
+        iter = iter->next;
+    }
+    return NULL;
+}
+
+static void dhcp_send(const DhcpProps *props, const DhcpOption *opts) {
+    // construct body
+    uint8_t *buf = (uint8_t *) s.buf;
     memset(buf, 0, DHCP_MIN_PACKET_SIZE);
-    FILL_BYTE_ADVANCE(buf, &(ops->op));
+    FILL_BYTE_ADVANCE(buf, &(props->op));
-    FILL_BYTE_ADVANCE(buf, &(ops->htype));
+    FILL_BYTE_ADVANCE(buf, &(props->htype));
-    FILL_BYTE_ADVANCE(buf, &(ops->hlen));
+    FILL_BYTE_ADVANCE(buf, &(props->hlen));
-    FILL_BYTE_ADVANCE(buf, &(ops->hops));
+    FILL_BYTE_ADVANCE(buf, &(props->hops));
-    FILL_DWORD_H2N_ADVANCE(buf, ops->xid);
+    FILL_DWORD_H2N_ADVANCE(buf, props->xid);
-    FILL_WORD_H2N_ADVANCE(buf, ops->secs);
+    FILL_WORD_H2N_ADVANCE(buf, props->secs);
-    FILL_WORD_H2N_ADVANCE(buf, ops->flags);
+    FILL_WORD_H2N_ADVANCE(buf, props->flags);
-    FILL_DWORD_H2N_ADVANCE(buf, ops->ciaddr);
+    FILL_ADVANCE(buf, &(props->ciaddr), 4);
-    FILL_DWORD_H2N_ADVANCE(buf, ops->yiaddr);
+    FILL_ADVANCE(buf, &(props->yiaddr), 4);
-    FILL_DWORD_H2N_ADVANCE(buf, ops->siaddr);
+    FILL_ADVANCE(buf, &(props->siaddr), 4);
-    FILL_DWORD_H2N_ADVANCE(buf, ops->giaddr);
+    FILL_ADVANCE(buf, &(props->giaddr), 4);
-    FILL_ADVANCE(buf, ops->chaddr, 16);
+    FILL_ADVANCE(buf, props->chaddr, 16);
     buf += SNAME_LEN + FILE_LEN;
     FILL_ADVANCE(buf, DHCP_MAGIC_COOKIE, 4); // DHCP magic cookie
 
     // insert options
-    dhcp_option_insert_msg_type(&buf, DHCPDISCOVER);
+    const DhcpOption *iter = opts;
-    dhcp_option_insert_max_msg_size(&buf, 1500);
+    while (iter) {
-    dhcp_option_insert_end(&buf);
+        dhcp_insert_option(&buf, iter);
+        iter = iter->next;
+    }
 
+//    dhcp_option_insert_msg_type(&buf, DHCPDISCOVER);
+//    dhcp_option_insert_max_msg_size(&buf, 1500);
+//    dhcp_option_insert_end(&buf);
+
+    // send packet
     udp_sendto(&s.connb, s.buf, DHCP_MIN_PACKET_SIZE, IPv4_ANY_ADDR, DHCP_SERVER_PORT);
 }
 
-static void dhcp_discover(EthInterface * intf) {
+static void dhcp_parse(const uint8_t *buf, DhcpProps *props, DhcpOption **opts) {
-    s.tranId = rand();
+    // parse body
-    DhcpOps ops = { 0 };
+    FETCH_BYTE_ADVANCE(&(props->op), buf);
+    FETCH_BYTE_ADVANCE(&(props->htype), buf);
+    FETCH_BYTE_ADVANCE(&(props->hlen), buf);
+    FETCH_BYTE_ADVANCE(&(props->hops), buf);
+    FETCH_DWORD_H2N_ADVANCE(&(props->xid), buf);
+    FETCH_WORD_H2N_ADVANCE(&(props->secs), buf);
+    FETCH_WORD_H2N_ADVANCE(&(props->flags), buf);
+    FETCH_ADVANCE(&(props->ciaddr), buf, 4);
+    FETCH_ADVANCE(&(props->yiaddr), buf, 4);
+    FETCH_ADVANCE(&(props->siaddr), buf, 4);
+    FETCH_ADVANCE(&(props->giaddr), buf, 4);
+    FETCH_ADVANCE(props->chaddr, buf, 16);
+    buf += SNAME_LEN + FILE_LEN;
+    uint8_t magicCookie[sizeof(DHCP_MAGIC_COOKIE)];
+    FETCH_ADVANCE(magicCookie, buf, 4); // DHCP magic cookie
 
-    ops.op = DHCP_BOOTREQUEST;
+    // parse options
-    ops.htype = 1;
+    *opts = dynmem_alloc(sizeof(DhcpOption));
-    ops.hlen = 6;
+    (*opts)->next = NULL;
-    ops.hops = 0;
+    dhcp_read_next_option(&buf, (*opts));
-    ops.xid = s.tranId;
+    DhcpOption *iter = *opts;
-    ops.secs = 0;
-    ops.flags = 0;
-    ops.ciaddr = 0;
-    ops.yiaddr = 0;
-    ops.siaddr = 0;
-    ops.giaddr = 0;
 
-    memcpy(ops.chaddr, intf->mac, ETH_HW_ADDR_LEN);
+    while (iter->id != DHCP_OPT_End) {
+        iter->next = dynmem_alloc(sizeof(DhcpOption));
+        iter = iter->next;
+        dhcp_read_next_option(&buf, iter);
+        iter->next = NULL;
+    }
 
-    dhcp_send(intf, &ops);
 }
 
-static int dhcp_resp_cb(const Pckt * pckt) {
+#define UINT16_TO_BE_BYTES(u) ((u) >> 8) & 0xFF, ((u) & 0xFF),
+#define UINT32_TO_BE_BYTES(u) ((u) >> 24) & 0xFF, ((u) >> 16) & 0xFF, ((u) >> 8) & 0xFF, ((u) & 0xFF),
+#define IPv4_ADDR_TO_BE_BYTES(addr) ((addr) & 0xFF), (((addr) >> 8) & 0xFF), (((addr) >> 16) & 0xFF), (((addr) >> 24) & 0xFF),
+#define HWADDR_TO_BE_BYTES(hwa) (hwa)[0], (hwa)[1], (hwa)[2], (hwa)[3], (hwa)[4], (hwa)[5],
+
+static void dhcp_discover() {
+    s.tranId = rand();
+    DhcpProps props = {0};
+
+    props.op = DHCP_BOOTREQUEST;
+    props.htype = DHCP_HW_TYPE_ETHERNET;
+    props.hlen = 6;
+    props.hops = 0;
+    props.xid = s.tranId;
+    props.secs = 0;
+    props.flags = 0;
+    props.ciaddr = 0;
+    props.yiaddr = 0;
+    props.siaddr = 0;
+    props.giaddr = 0;
+
+    memcpy(props.chaddr, s.intf->mac, ETH_HW_ADDR_LEN);
+
+    DhcpOption optEnd = {DHCP_OPT_End, 0, NULL};
+    uint16_t maxSize = 1500;
+    DhcpOption maxMsgSize = {DHCP_OPT_MaxMsgSize, 2, &optEnd, {UINT16_TO_BE_BYTES(maxSize)}};
+    DhcpOption msgType = {DHCP_OPT_MsgType, 1, &maxMsgSize, {DHCPDISCOVER}};
+
+    dhcp_send(&props, &msgType);
+}
+
+void dhcp_request(ip4_addr reqAddr, ip4_addr dhcpServerAddr) {
+    DhcpProps props = {0};
+
+    props.op = DHCP_BOOTREQUEST;
+    props.htype = DHCP_HW_TYPE_ETHERNET;
+    props.hlen = 6;
+    props.hops = 0;
+    props.xid = s.tranId;
+    props.secs = 0;
+    props.flags = 0;
+    props.ciaddr = 0;
+    props.yiaddr = 0;
+    props.siaddr = 0;
+    props.giaddr = 0;
+
+    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 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}};
+
+    dhcp_send(&props, &msgType);
+}
+
+static void dhcp_process(DhcpProps *props, DhcpOption *opts) {
+    switch (s.state) {
+        case DHCP_INIT:
+            dhcp_discover(); // send discover message
+            s.state = DHCP_SELECTING;
+            break;
+        case DHCP_SELECTING: {
+            const DhcpOption *msgType = dhcp_get_option(opts, DHCP_OPT_MsgType);
+            if (msgType->value[0] == DHCPOFFER) {
+                ip4_addr addrOffer = props->yiaddr;
+                ip4_addr serverAddr = props->siaddr;
+
+                dhcp_request(addrOffer, serverAddr);
+
+                s.state = DHCP_REQUESTING;
+            }
+        }
+            break;
+        case DHCP_REQUESTING: {
+            const DhcpOption *opt = dhcp_get_option(opts, DHCP_OPT_MsgType);
+            uint8_t msgType = opt->value[0];
+            if (msgType == DHCPNAK) {
+                //dhcp_discover();
+                //s.state = DHCP_SELECTING;
+            } else if (msgType == DHCPACK) {
+                s.intf->ip = props->yiaddr; // fetch ip address
+
+                opt = dhcp_get_option(opts, DHCP_OPT_Router); // get gateway/router address
+                FETCH_DWORD(&(s.intf->router), opt->value);
+
+                opt = dhcp_get_option(opts, DHCP_OPT_SubnetMask); // get subnet mask
+                FETCH_DWORD(&(s.intf->netmask), opt->value);
+
+                opt = dhcp_get_option(opts, DHCP_OPT_DomainNameServer); // get DNS
+                FETCH_DWORD(&(s.intf->dns), opt->value);
+
+                MSG("DHCP done!\n");
+                MSG("IP: ");
+                PRINT_IPv4(s.intf->ip);
+                MSG("\nRouter: ");
+                PRINT_IPv4(s.intf->router);
+                MSG("\nNetmask: ");
+                PRINT_IPv4(s.intf->netmask);
+                MSG("\nDNS: ");
+                PRINT_IPv4(s.intf->dns);
+                MSG("\n");
+
+                s.state = DHCP_BOUND;
+            }
+        }
+            break;
+        default:
+            break;
+    }
+}
+
+static int dhcp_resp_cb(const Pckt *pckt, PcktSieveLayerTag tag) {
+    DhcpProps props;
+    DhcpOption *opts = NULL;
+    dhcp_parse(pckt->payload, &props, &opts);
+
+    dhcp_process(&props, opts);
+
+    dhcp_free_opt_chain(opts);
+
     return 0;
 }
 
+void dhcp_start() {
+    s.state = DHCP_INIT;
+    dhcp_process(NULL, NULL);
+}
+
 void dhcp_initiate(EthInterface *intf) {
-    s.state = DHCP_INIT_REBOOT;
+    s.state = DHCP_INIT;
     s.buf = dynmem_alloc(DHCP_MIN_PACKET_SIZE);
     s.connb = udp_new_connblock(intf, IPv4_ANY_ADDR, DHCP_CLIENT_PORT, dhcp_resp_cb);
-
+    s.intf = intf;
-    dhcp_discover(intf);
 }

prefab/packet_parsers/dhcp.h

@@ -14,12 +14,12 @@ typedef struct {
     uint16_t flags; ///< Flags
     uint32_t ciaddr; ///< Client IP address (filled in DHCPREQUEST)
     uint32_t yiaddr; ///< 'Your' client IP address
-    uint32_t siaddr; ///< ...
+    uint32_t siaddr; ///< DHCP server IP address
-    uint32_t giaddr; ///< ...
+    uint32_t giaddr; ///< Relay agent IP address
     uint8_t chaddr[16]; ///< Client hardware address
     char * sname; ///< Optional server host name
     char * file; ///< Boot file name
-} DhcpOps;
+} DhcpProps;
 
 /**
  * DHCP state.
@@ -36,7 +36,7 @@ typedef enum {
 } DhcpState;
 
 /**
- * DHCP op codes.
+ * DHCP id codes.
  */
 typedef enum {
     DHCP_BOOTREQUEST = 1,
@@ -53,4 +53,6 @@ typedef enum {
  */
 void dhcp_initiate(EthInterface * intf);
 
+void dhcp_start();
+
 #endif //ETHERLIB_TEST_DHCP_H

prefab/packet_parsers/ethernet_frame.h

@@ -14,6 +14,8 @@
 
 #define HEADER_FETCH_PROPS(T,h) (T *)(&((h)->props))
 
+#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
+
 typedef uint8_t EthernetAddress[ETH_HW_ADDR_LEN];
 
 /**

utils.h

@@ -38,6 +38,7 @@
 
 #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]);
 
 #define ASSERT_BAD_ALIGN(p) if ((size_t)(p) & 0b11) ERROR("Bad memory alignment in function '%s' in file '%s' on line %d!\n", __func__, __FILE__, __LINE__)
 #define ASSERT_NULL(p) if ((p) == NULL) ERROR("NULL in function '%s' in file '%s' on line %d!\n", __func__, __FILE__, __LINE__)
@@ -49,10 +50,18 @@
 #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_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 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 MIN(a,b) (((a) < (b)) ? (a) : (b))
+#define MAX(a,b) (((a) > (b)) ? (a) : (b))
 
 // ------------------