From 05288d7a3c7dfbbe696b94316ed02d066ca7d400 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Wiesner=20Andr=C3=A1s?= Date: Sat, 14 Jan 2023 14:24:56 +0100 Subject: [PATCH] - ARP cache auto lookup feature added - IGMPv2 capabilities added (report membership, leave group) - ICMP capabilities added (ping) - Tx Message Queue added --- arp_cache.c | 2 +- eth_interface.c | 7 +- eth_interface.h | 4 +- global_state.c | 23 ++++ global_state.h | 2 + msg_queue.c | 51 +++++++ msg_queue.h | 55 +++++++- packet_registry.h | 20 ++- packet_sieve.c | 21 ++- packet_sieve.h | 18 ++- pckt_assembler.c | 62 +++++++++ pckt_assembler.h | 18 ++- prefab/conn_blocks/arp_connblock.c | 2 +- prefab/conn_blocks/icmp_connblock.c | 82 ++++++++++- prefab/conn_blocks/icmp_connblock.h | 15 +- prefab/conn_blocks/igmp_connblock.c | 123 ++++++++++++++++- prefab/conn_blocks/igmp_connblock.h | 30 +++- prefab/conn_blocks/ipv4_connblock.c | 17 ++- prefab/conn_blocks/udp_connblock.c | 53 ++++---- prefab/packet_parsers/arp_packet.h | 13 ++ prefab/packet_parsers/dhcp.c | 11 +- prefab/packet_parsers/dhcp.h | 3 + prefab/packet_parsers/ethernet_frame.c | 2 + prefab/packet_parsers/ethernet_frame.h | 8 +- prefab/packet_parsers/icmp_packet.c | 37 +++++ prefab/packet_parsers/icmp_packet.h | 46 ++++++- prefab/packet_parsers/igmp_packet.c | 53 ++++++++ prefab/packet_parsers/igmp_packet.h | 48 ++++++- prefab/packet_parsers/ipv4_packet.c | 34 ++--- prefab/packet_parsers/ipv4_packet.h | 8 ++ prefab/packet_parsers/ipv4_types.h | 1 + prefab/packet_parsers/udp_packet.c | 37 ++--- timer.c | 181 ++++++++++++++++++++++++- timer.h | 91 ++++++++++++- utils.c | 22 +++ utils.h | 14 +- 36 files changed, 1072 insertions(+), 142 deletions(-) diff --git a/arp_cache.c b/arp_cache.c index f08da10..48c1b84 100644 --- a/arp_cache.c +++ b/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; diff --git a/eth_interface.c b/eth_interface.c index 6b22a03..59bc0e9 100644 --- a/eth_interface.c +++ b/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); } diff --git a/eth_interface.h b/eth_interface.h index e48aaed..a905652 100644 --- a/eth_interface.h +++ b/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; /** diff --git a/global_state.c b/global_state.c index 0fcd6b7..d8c61fa 100644 --- a/global_state.c +++ b/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 diff --git a/global_state.h b/global_state.h index 68c3f58..9cdd3d9 100644 --- a/global_state.h +++ b/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; diff --git a/msg_queue.c b/msg_queue.c index 229f88f..5326280 100644 --- a/msg_queue.c +++ b/msg_queue.c @@ -2,4 +2,55 @@ // Created by epagris on 2023.01.13.. // +#include +#include #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); + } +} + + diff --git a/msg_queue.h b/msg_queue.h index d4d7be1..dcdc163 100644 --- a/msg_queue.h +++ b/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 +#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 diff --git a/packet_registry.h b/packet_registry.h index 2d0cb2a..a67bb06 100644 --- a/packet_registry.h +++ b/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; diff --git a/packet_sieve.c b/packet_sieve.c index bf74aca..8b83443 100644 --- a/packet_sieve.c +++ b/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; + } +} diff --git a/packet_sieve.h b/packet_sieve.h index 7e9ff87..afaf6aa 100644 --- a/packet_sieve.h +++ b/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); diff --git a/pckt_assembler.c b/pckt_assembler.c index 2f9cc55..28f9c30 100644 --- a/pckt_assembler.c +++ b/pckt_assembler.c @@ -2,4 +2,66 @@ // Created by epagris on 2023.01.11.. // +#include #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; +} \ No newline at end of file diff --git a/pckt_assembler.h b/pckt_assembler.h index c189ca0..78852b0 100644 --- a/pckt_assembler.h +++ b/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 diff --git a/prefab/conn_blocks/arp_connblock.c b/prefab/conn_blocks/arp_connblock.c index 7020522..8d01b7b 100644 --- a/prefab/conn_blocks/arp_connblock.c +++ b/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; diff --git a/prefab/conn_blocks/icmp_connblock.c b/prefab/conn_blocks/icmp_connblock.c index 72b30cb..f352d37 100644 --- a/prefab/conn_blocks/icmp_connblock.c +++ b/prefab/conn_blocks/icmp_connblock.c @@ -1,5 +1,79 @@ -// -// Created by epagris on 2023.01.04.. -// - #include "icmp_connblock.h" + +#include + +#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; +} \ No newline at end of file diff --git a/prefab/conn_blocks/icmp_connblock.h b/prefab/conn_blocks/icmp_connblock.h index 81819f3..f276020 100644 --- a/prefab/conn_blocks/icmp_connblock.h +++ b/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 diff --git a/prefab/conn_blocks/igmp_connblock.c b/prefab/conn_blocks/igmp_connblock.c index b25c627..d91bcee 100644 --- a/prefab/conn_blocks/igmp_connblock.c +++ b/prefab/conn_blocks/igmp_connblock.c @@ -1,5 +1,120 @@ -// -// Created by epagris on 2023.01.14.. -// - #include "igmp_connblock.h" + +#include + +#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); +} diff --git a/prefab/conn_blocks/igmp_connblock.h b/prefab/conn_blocks/igmp_connblock.h index bd133de..e91d834 100644 --- a/prefab/conn_blocks/igmp_connblock.h +++ b/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 diff --git a/prefab/conn_blocks/ipv4_connblock.c b/prefab/conn_blocks/ipv4_connblock.c index bd3895c..6c5499c 100644 --- a/prefab/conn_blocks/ipv4_connblock.c +++ b/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) { - EthernetProps * etherProps = (EthernetProps *) contProps; - IPv4Props * ipProps = (IPv4Props *) ownProps; +static bool filtIPv4(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, EthInterface * intf) { + EthernetProps *etherProps = (EthernetProps *) contProps; + 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", - (ipAddr & 0xFF), ((ipAddr >> 8) & 0xFF), ((ipAddr >> 16) & 0xFF), ((ipAddr >> 24) & 0xFF)); + if (ipAddr != IPv4_IF_ADDR) { + 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"); } diff --git a/prefab/conn_blocks/udp_connblock.c b/prefab/conn_blocks/udp_connblock.c index 8573885..a872511 100644 --- a/prefab/conn_blocks/udp_connblock.c +++ b/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 - uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE + ETH_UDP_HEADER_SIZE; - uint32_t txBufSize = txHeaderSize + size + 4; - uint8_t * txBuf = dynmem_alloc(txBufSize); + // common fields for packet assembly + ethProps->hdrInsFn = insert_ethernet_header; + ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE; - // copy payload - memcpy(txBuf + txHeaderSize, data, size); + Pckt cooked; + cooked.payload = data; + cooked.payloadSize = size; + cooked.header = ethHeader; - // insert UDP header - insert_udp_header(txBuf + ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE, udpHeader); + // NOT FILLED FIELDS + cooked.headerSize = 0; + cooked.time_s = 0; + cooked.time_ns = 0; - // insert IPv4 header - insert_ipv4_header(txBuf + ETH_ETHERNET_HEADER_SIZE, ipHeader); + RawPckt raw; + pckt_assemble(&raw, &cooked); - // insert Ethernet header - insert_ethernet_header(txBuf, ethHeader); + ethinf_transmit(connBlock->intf, &raw); // 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); - - // 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); + // release payload + // dynmem_free(raw.payload); return 0; } diff --git a/prefab/packet_parsers/arp_packet.h b/prefab/packet_parsers/arp_packet.h index fe6a423..e108789 100644 --- a/prefab/packet_parsers/arp_packet.h +++ b/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 diff --git a/prefab/packet_parsers/dhcp.c b/prefab/packet_parsers/dhcp.c index 04fa4f3..0ac468d 100644 --- a/prefab/packet_parsers/dhcp.c +++ b/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, ¶mReq, {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; diff --git a/prefab/packet_parsers/dhcp.h b/prefab/packet_parsers/dhcp.h index 9bc697e..957b878 100644 --- a/prefab/packet_parsers/dhcp.h +++ b/prefab/packet_parsers/dhcp.h @@ -53,6 +53,9 @@ typedef enum { */ void dhcp_initiate(EthInterface * intf); +/** + * + */ void dhcp_start(); #endif //ETHERLIB_TEST_DHCP_H diff --git a/prefab/packet_parsers/ethernet_frame.c b/prefab/packet_parsers/ethernet_frame.c index 0bb23bf..fb5a46b 100644 --- a/prefab/packet_parsers/ethernet_frame.c +++ b/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 diff --git a/prefab/packet_parsers/ethernet_frame.h b/prefab/packet_parsers/ethernet_frame.h index 144f6ca..abc7051 100644 --- a/prefab/packet_parsers/ethernet_frame.h +++ b/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 sourceAddr[ETH_HW_ADDR_LEN]; ///< source address + EthernetAddress destAddr; ///< destination address + EthernetAddress sourceAddr; ///< source address uint16_t length_type; ///< frame length_type } EthernetProps; diff --git a/prefab/packet_parsers/icmp_packet.c b/prefab/packet_parsers/icmp_packet.c index 1316548..34bb337 100644 --- a/prefab/packet_parsers/icmp_packet.c +++ b/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); +} + + diff --git a/prefab/packet_parsers/icmp_packet.h b/prefab/packet_parsers/icmp_packet.h index 289a676..f1dbc16 100644 --- a/prefab/packet_parsers/icmp_packet.h +++ b/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 +#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 diff --git a/prefab/packet_parsers/igmp_packet.c b/prefab/packet_parsers/igmp_packet.c index c5461d2..0ca7041 100644 --- a/prefab/packet_parsers/igmp_packet.c +++ b/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; +} diff --git a/prefab/packet_parsers/igmp_packet.h b/prefab/packet_parsers/igmp_packet.h index c89e1ed..3e9c6ea 100644 --- a/prefab/packet_parsers/igmp_packet.h +++ b/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 +#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 diff --git a/prefab/packet_parsers/ipv4_packet.c b/prefab/packet_parsers/ipv4_packet.c index 7c196d9..2eb9d72 100644 --- a/prefab/packet_parsers/ipv4_packet.c +++ b/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); - memcpy(ChkSumPtr, &checksum, 2); + ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE); + 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]; } diff --git a/prefab/packet_parsers/ipv4_packet.h b/prefab/packet_parsers/ipv4_packet.h index 4523c0a..12c7d5a 100644 --- a/prefab/packet_parsers/ipv4_packet.h +++ b/prefab/packet_parsers/ipv4_packet.h @@ -4,6 +4,7 @@ #include #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 diff --git a/prefab/packet_parsers/ipv4_types.h b/prefab/packet_parsers/ipv4_types.h index a639a6a..9b8c063 100644 --- a/prefab/packet_parsers/ipv4_types.h +++ b/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 diff --git a/prefab/packet_parsers/udp_packet.c b/prefab/packet_parsers/udp_packet.c index 31cb876..d95b164 100644 --- a/prefab/packet_parsers/udp_packet.c +++ b/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; - - // pseudoheader - for (uint16_t i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) { - uint16_t field = ((uint16_t *) pseudoHeader)[i]; - field = htons(field); - chksum += field; + uint32_t sum = chksum((const uint8_t *)pseudoHeader, sizeof(UdpPseudoHeader)) + chksum(hdr, size); + while (sum >> 16) { + sum = (sum & 0xFFFF) + (sum >> 16); } - - // 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; + return sum; } 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); } diff --git a/timer.c b/timer.c index 28262c8..52cc114 100644 --- a/timer.c +++ b/timer.c @@ -2,21 +2,196 @@ // Created by epagris on 2022.12.21.. // +#include #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(tmrHdrSize + maxSched * sizeof(AlarmAssignment)); + Timer * tmr = (Timer *) dynmem_alloc(sizeof(Timer) + 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); +} diff --git a/timer.h b/timer.h index e609b73..0e3f507 100644 --- a/timer.h +++ b/timer.h @@ -2,18 +2,44 @@ #define ETHERLIB_TEST_TIMER_H #include +#include /** * 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 diff --git a/utils.c b/utils.c index 72ace8d..e46f933 100644 --- a/utils.c +++ b/utils.c @@ -58,4 +58,26 @@ 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; } \ No newline at end of file diff --git a/utils.h b/utils.h index 0711386..7fd5bd1 100644 --- a/utils.h +++ b/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