From ab8d45932f81abfcdc9bd973e3dcabcf2bce9cc5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Wiesner=20Andr=C3=A1s?= Date: Thu, 27 Apr 2023 09:38:26 +0200 Subject: [PATCH] Timestamping and bunch of bugfix and optimization - Timestamping management added - Errors due to reading uninitialized data in ARP fixed - EthInterface reworked, incoming packet notification and payload readout separated (through which fixing concurrent access problems) - RX and TX offloads added - Capability to add a packet sieve layer without prior registration of specific packet class added (this makes it possible to register arbitrary EtherType connection blocks, for example) --- arp_cache.c | 2 +- connection_block.c | 4 + connection_block.h | 7 +- dynmem.h | 9 ++ eth_interface.c | 31 +++++-- eth_interface.h | 32 ++++++- etherlib.h | 14 +++ global_state.c | 4 + global_state.h | 6 ++ memory_pool.c | 4 + msg_queue.c | 20 ++-- packet.h | 14 ++- packet_registry.h | 2 +- packet_sieve.c | 86 +++++++++-------- packet_sieve.h | 34 +++---- pckt_assembler.c | 23 +++-- pckt_assembler.h | 2 +- prefab/conn_blocks/arp_connblock.c | 9 +- .../conn_blocks/custom_ethertype_connblock.c | 92 +++++++++++++++++++ .../conn_blocks/custom_ethertype_connblock.h | 21 +++++ .../{ethernet_info.c => ethernet_connblock.c} | 2 +- .../{ethernet_info.h => ethernet_connblock.h} | 6 +- prefab/conn_blocks/icmp_connblock.c | 59 +++++++++--- prefab/conn_blocks/igmp_connblock.c | 4 +- prefab/conn_blocks/ipv4/ip_assembler.c | 10 +- prefab/conn_blocks/ipv4_connblock.c | 21 +++++ prefab/conn_blocks/ipv4_connblock.h | 7 ++ prefab/conn_blocks/tcp_connblock.c | 4 +- prefab/conn_blocks/udp_connblock.c | 19 +++- prefab/conn_blocks/udp_connblock.h | 13 ++- prefab/packet_parsers/arp_packet.c | 2 +- prefab/packet_parsers/arp_packet.h | 2 +- prefab/packet_parsers/ethernet_frame.c | 5 +- prefab/packet_parsers/ethernet_frame.h | 2 +- prefab/packet_parsers/icmp_packet.c | 2 +- prefab/packet_parsers/icmp_packet.h | 2 +- prefab/packet_parsers/igmp_packet.c | 2 +- prefab/packet_parsers/igmp_packet.h | 2 +- prefab/packet_parsers/ipv4_packet.c | 16 ++-- prefab/packet_parsers/ipv4_packet.h | 2 +- prefab/packet_parsers/tcp_segment.c | 31 ++++--- prefab/packet_parsers/tcp_segment.h | 2 +- prefab/packet_parsers/udp_packet.c | 21 +++-- prefab/packet_parsers/udp_packet.h | 2 +- timestamping.c | 19 ++++ timestamping.h | 13 +++ utils.h | 8 +- 47 files changed, 537 insertions(+), 157 deletions(-) create mode 100644 etherlib.h create mode 100644 prefab/conn_blocks/custom_ethertype_connblock.c create mode 100644 prefab/conn_blocks/custom_ethertype_connblock.h rename prefab/conn_blocks/{ethernet_info.c => ethernet_connblock.c} (86%) rename prefab/conn_blocks/{ethernet_info.h => ethernet_connblock.h} (62%) create mode 100644 timestamping.c create mode 100644 timestamping.h diff --git a/arp_cache.c b/arp_cache.c index 63f0bd8..17ed51a 100644 --- a/arp_cache.c +++ b/arp_cache.c @@ -114,7 +114,7 @@ void arpc_ask(ArpCache *arpc, ip4_addr addr) { } void arpc_respond(ArpCache *arpc, const EthernetAddress hwAddr, ip4_addr ipAddr) { - ArpProps arpProps; + ArpProps arpProps = { 0 }; arpProps.HTYPE = 1; arpProps.PTYPE = ETH_IPv4_PACKET_CLASS; diff --git a/connection_block.c b/connection_block.c index 79321b7..833dc15 100644 --- a/connection_block.c +++ b/connection_block.c @@ -7,6 +7,10 @@ #include +void connb_init_defaults(ConnBlock *connb) { + // ... +} + void connb_remove(ConnBlock *connb) { if (connb != NULL) { packsieve_remove_layer(connb->sieveLayer); diff --git a/connection_block.h b/connection_block.h index 2b506ca..d38c05b 100644 --- a/connection_block.h +++ b/connection_block.h @@ -13,9 +13,14 @@ typedef int (*ConnBlockTransmitFn)(struct EthInterface_ * intf, const uint8_t * typedef struct ConnBlock_ { PcktSieve * sieve; ///< Ethernet interface PcktSieveLayer * sieveLayer; ///< Sieve layer - void * tag; ///< Arbitrary tagging } ConnBlock; +/** + * Initialize non-mandatory fields to default values. + * @param connb + */ +void connb_init_defaults(ConnBlock * connb); + /** * Remove connection block. * @param connb pointer to existing connection block diff --git a/dynmem.h b/dynmem.h index b4b4d79..0ca62d3 100644 --- a/dynmem.h +++ b/dynmem.h @@ -7,6 +7,11 @@ #include #endif +#ifdef VALGRIND_DEBUG +#include +#include +#endif + /** * Initialize EtherLib dynamic memory management subsystem, * based on heap pointer and size given in etherlib_options.h @@ -22,6 +27,8 @@ void * dynmem_alloc_(uint32_t size); #ifdef DYNMEM_DEBUG #define dynmem_alloc(size) dynmem_alloc_(size); MSG("ALLOC: %d %s() in %s:%d\n", size, __FUNCTION__, __FILE__, __LINE__) +#elif defined(VALGRIND_DEBUG) +#define dynmem_alloc(size) malloc(size) #else #define dynmem_alloc(size) dynmem_alloc_(size) #endif @@ -34,6 +41,8 @@ void dynmem_free_(void * ptr); #ifdef DYNMEM_DEBUG #define dynmem_free(ptr) MSG("FREE: %s() in %s:%d\n", __FUNCTION__, __FILE__, __LINE__), dynmem_free_(ptr) +#elif defined(VALGRIND_DEBUG) +#define dynmem_free(p) free(p) #else #define dynmem_free(ptr) dynmem_free_(ptr) #endif diff --git a/eth_interface.c b/eth_interface.c index b6883d9..365d97f 100644 --- a/eth_interface.c +++ b/eth_interface.c @@ -6,16 +6,22 @@ #include "dynmem.h" #include "utils.h" #include "etherlib_options.h" -#include "etherlib/prefab/conn_blocks/ethernet_info.h" +#include "etherlib/prefab/conn_blocks/ethernet_connblock.h" static int ethintf_llrecv(EthIODef * io, const RawPckt * pckt) { ethinf_receive((EthInterface *) io->tag, pckt); return 0; } +static int ethintf_llrxnotify(EthIODef * io) { + ethinf_notify((EthInterface *) io->tag); + return 0; +} + static void ethintf_register(EthInterface * intf) { intf->ioDef->tag = intf; - intf->ioDef->llRxDone = ethintf_llrecv; + intf->ioDef->llRxStore = ethintf_llrecv; + intf->ioDef->llRxNotify = ethintf_llrxnotify; } // interface processing thread @@ -36,11 +42,13 @@ EthInterface *ethintf_new(EthIODef * io) { ethIntf->txQ = mq_create(ETHLIB_DEF_MQ_SIZE); ethIntf->rxQ = mq_create(ETHLIB_DEF_MQ_SIZE); ETHLIB_OS_SEM_CREATE(ðIntf->rxSem, ETHLIB_DEF_MQ_SIZE); - ETHLIB_OS_THREAD_DEFINE(ethinf_proc_thread, ipp, osPriorityHigh, 4096, ethIntf); - ETHLIB_OS_THREAD_CREATE(ipp, ethIntf); + ETHLIB_OS_THREAD_DEFINE(ethinf_proc_thread, ethrx, osPriorityHigh, 4096, ethIntf); + ETHLIB_OS_THREAD_CREATE(ethrx, ethIntf); ethIntf->ipra = ipra_new(); + ethIntf->capabilities = 0; + return ethIntf; } @@ -48,7 +56,8 @@ static void * ethinf_proc_thread(void * param) { EthInterface * intf = (EthInterface *) param; while (true) { ETHLIB_OS_SEM_WAIT(&intf->rxSem); - if (mq_avail(intf->rxQ) > 0) { + intf->ioDef->llRxRead(intf->ioDef, intf->rxQ); // read packets + while (mq_avail(intf->rxQ) > 0) { RawPckt rawPckt = mq_top(intf->rxQ); mq_pop(intf->rxQ); packsieve_input(&intf->sieve, &rawPckt); @@ -59,9 +68,7 @@ static void * ethinf_proc_thread(void * param) { void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt) { bool pushOK = mq_push(intf->rxQ, rawPckt); - if (pushOK) { - ETHLIB_OS_SEM_POST(&intf->rxSem); - } else { + if (!pushOK) { dynmem_free(rawPckt->payload); ERROR("Input queue full, packet dropped!\n"); } @@ -72,3 +79,11 @@ void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt) { intf->ioDef->llTxTrigger(intf->ioDef, intf->txQ); } + +void ethinf_notify(EthInterface *intf) { + ETHLIB_OS_SEM_POST(&intf->rxSem); +} + +void ethinf_set_capabilities(EthInterface *intf, uint32_t cap) { + intf->capabilities = cap; +} diff --git a/eth_interface.h b/eth_interface.h index 7a7ccdc..1620dfb 100644 --- a/eth_interface.h +++ b/eth_interface.h @@ -17,12 +17,27 @@ typedef struct EthIODef_ { int (*llTxTrigger)(struct EthIODef_ * io, MsgQueue * mq); ///< Function pointer to low-level transmit function trigger int (*llTxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Transmission done (interrupt) callback int (*llLinkChg)(struct EthIODef_ * io, int linkState); ///< Link change interrupt - int (*llRxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Receive done callback + int (*llRxStore)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Receive done callback int (*llError)(struct EthIODef_ * io, int error); ///< Low-level error interrupt + int (*llRxRead)(struct EthIODef_ * io, MsgQueue * mq); ///< Read received packets + int (*llRxNotify)(struct EthIODef_ * io); ///< Notify of received packets void * tag; ///< Some arbitrary tagging } EthIODef; +/** + * Ethernet interface capabilities + */ +typedef enum { + ETHINF_CAP_NONE = 0, + ETHINF_CAP_TX_CRC_OFFLOAD = 1, + ETHINF_CAP_TX_IPCHKSUM_OFFLOAD = 2, + ETHINF_CAP_TX_TCPUDPCHKSUM_OFFLOAD = 4, + ETHINF_CAP_RX_IPCHKSUM_OFFLOAD = 8, + ETHINF_CAP_RX_TCPUDPCHKSUM_OFFLOAD = 16, + ETHINF_CAP_ALL_RX_TX_CHECKSUM_OFFLOADS = 0b11111, +} EthIntfCap; + /** * Ethernet interface representation. */ @@ -30,6 +45,7 @@ typedef struct EthInterface_ { PcktSieve sieve; ///< Packet sieve EthIODef * ioDef; ///< Low-level IO definitions EthernetAddress mac; ///< Ethernet address + uint32_t capabilities; ///< Ethernet interface capabilities ip4_addr ip; ///< IP address ip4_addr router; ///< Router IP address ip4_addr netmask; ///< Subnet mask @@ -55,9 +71,23 @@ EthInterface *ethintf_new(EthIODef * io); */ void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt); +/** + * Notify interface of a received, buffered packet, which needs transferred + * from the low level Ethernet-driver. + * @param intf Pointer to Ethernet-interface + */ +void ethinf_notify(EthInterface *intf); + /** * Transmit packet. */ void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt); +/** + * Set interface capabilities. + * @param intf pointer to Ethernet interface + * @param cap OR-ed bitfield of capabilities + */ +void ethinf_set_capabilities(EthInterface *intf, uint32_t cap); + #endif //ETHERLIB_ETH_INTERFACE_H diff --git a/etherlib.h b/etherlib.h new file mode 100644 index 0000000..3422910 --- /dev/null +++ b/etherlib.h @@ -0,0 +1,14 @@ +#ifndef ETHERLIB_ETHERLIB_H +#define ETHERLIB_ETHERLIB_H + +#include "prefab/prefab.h" +#include "global_state.h" +#include "timestamping.h" +#include "utils.h" + +// TODO group these includes.. +#include "prefab/conn_blocks/igmp_connblock.h" +#include "prefab/conn_blocks/udp_connblock.h" +#include "prefab/conn_blocks/custom_ethertype_connblock.h" + +#endif //ETHERLIB_ETHERLIB_H diff --git a/global_state.c b/global_state.c index 3365c2a..ab33041 100644 --- a/global_state.c +++ b/global_state.c @@ -92,3 +92,7 @@ void close_connection(cbd d) { connb_remove(&connBlock); // remove connection block } + +EthInterface *get_default_interface() { + return E.ethIntf; +} diff --git a/global_state.h b/global_state.h index c4e8209..2d99e70 100644 --- a/global_state.h +++ b/global_state.h @@ -35,4 +35,10 @@ void process_raw_packet(const uint8_t * data, uint32_t size); */ void close_connection(cbd d); +/** + * Get default Ethernet interface + * @return pointer to default Ethernet interface + */ +EthInterface * get_default_interface(); + #endif //ETHERLIB_GLOBAL_STATE_H diff --git a/memory_pool.c b/memory_pool.c index ac9030f..c402324 100644 --- a/memory_pool.c +++ b/memory_pool.c @@ -137,6 +137,10 @@ static void mp_join_free_blocks(MP *mp) { } void mp_free(MP *mp, const uint8_t *p) { + if (p == NULL) { // don't do anything with a NULL pointer + return; + } + // look for registry record bool success = false; MPAllocRecord *recIter = mp->blockRegistry; diff --git a/msg_queue.c b/msg_queue.c index f33592a..4fa468e 100644 --- a/msg_queue.c +++ b/msg_queue.c @@ -10,7 +10,7 @@ #include MsgQueue *mq_create(uint32_t size) { - MsgQueue * mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt)); + MsgQueue *mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt)); mq->size = size; mq->readIdx = 0; mq->writeIdx = 0; @@ -19,19 +19,23 @@ MsgQueue *mq_create(uint32_t size) { return mq; } -void mq_clear(MsgQueue * mq) { +void mq_clear(MsgQueue *mq) { mq->readIdx = 0; mq->writeIdx = 0; memset(mq->pckts, 0, mq->size * sizeof(RawPckt)); } -uint32_t mq_avail(const MsgQueue * mq) { - return mq->writeIdx - mq->readIdx; +uint32_t mq_avail(const MsgQueue *mq) { + int32_t delta = (int32_t) (mq->writeIdx - mq->readIdx); + if (delta < 0) { // modulo... + delta += (int32_t) mq->size; + } + return delta; } -#define MQ_NEXT(size,current) (((current)+1)%(size)) +#define MQ_NEXT(size, current) (((current)+1)%(size)) -bool mq_push(MsgQueue * mq, const RawPckt * raw) { +bool mq_push(MsgQueue *mq, const RawPckt *raw) { if (MQ_NEXT(mq->size, mq->writeIdx) == mq->readIdx) { // cannot push, queue is full return false; } @@ -45,11 +49,11 @@ bool mq_push(MsgQueue * mq, const RawPckt * raw) { return true; } -RawPckt mq_top(MsgQueue * mq) { +RawPckt mq_top(MsgQueue *mq) { return mq->pckts[mq->readIdx]; } -void mq_pop(MsgQueue * mq) { +void mq_pop(MsgQueue *mq) { if (mq_avail(mq) > 0) { // if there's anything to pop mq->readIdx = MQ_NEXT(mq->size, mq->readIdx); } diff --git a/packet.h b/packet.h index beacbff..3b82765 100644 --- a/packet.h +++ b/packet.h @@ -10,8 +10,16 @@ typedef struct { uint8_t * payload; ///> Pointer to raw packet payload. uint32_t size; ///> Raw packet size. - uint32_t time_s; ///> Timestamp seconds field - uint32_t time_ns; ///> Timestamp nanoseconds field + union { + struct { + uint32_t time_s; ///> Timestamp seconds field + uint32_t time_ns; ///> Timestamp nanoseconds field + } rx; + struct { + void(*txTsCb)(uint32_t ts_s, uint32_t ts_ns, uint32_t tag); ///> Transmit timestamp callback pointer + uint32_t arg; ///> User-defined argument + } tx; + } ext; ///> Extensions } RawPckt; struct PcktHeaderElement_; @@ -23,7 +31,7 @@ struct PcktHeaderElement_; * Fields have been reordered so that fields are aligned. */ typedef struct { - uint64_t time_s; ///< Timestamp seconds part. + uint64_t time_s; ///< Timestamp seconds part uint32_t time_ns; ///< Timestamp nanoseconds part. struct PcktHeaderElement_ * header; ///< Pointer to packet header. Points to the innermost header uint8_t * payload; ///< Pointer to (innermost) payload. diff --git a/packet_registry.h b/packet_registry.h index 391040b..e889cb4 100644 --- a/packet_registry.h +++ b/packet_registry.h @@ -28,7 +28,7 @@ struct PcktHeaderElement_; * @param hdr buffer to insert header into * @param pcktHdrLe linked list of packet headers */ -typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe); +typedef void (*PcktHeaderInsertFn)(uint8_t * hdr, const struct PcktHeaderElement_ * pcktHdrLe, struct EthInterface_ * intf); #define PcktPropsHeader \ PcktHeaderInsertFn hdrInsFn; /**< Header insert function pointer (used only on transmission) */ \ diff --git a/packet_sieve.c b/packet_sieve.c index 2cac3d1..491987e 100644 --- a/packet_sieve.c +++ b/packet_sieve.c @@ -16,8 +16,8 @@ void packfiltcond_zero(PcktSieveFilterCondition *cond) { memset(cond, 0, sizeof(PcktSieveFilterCondition)); } -PcktSieve *packsieve_new(EthInterface * intf) { - PcktSieve *sieve = (PcktSieve *) dynmem_alloc(sizeof(PcktSieve)); +PcktSieve* packsieve_new(EthInterface *intf) { + PcktSieve *sieve = (PcktSieve*) dynmem_alloc(sizeof(PcktSieve)); ASSERT_NULL(sieve); memset(&sieve->layer0, 0, sizeof(PcktSieveLayer)); // clear layer0 data sieve->intf = intf; @@ -30,7 +30,7 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { uint32_t size = rawPckt->size; bool mrd = true; // 'Must Release Data': at the end of processing not only release headers but data buffer as well - restart:; + restart: ; // process payload, fetch packet class etc. uint16_t ownClass = 0, containerClass = 0; // Ethernet... uint16_t offset = 0; @@ -48,16 +48,14 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { uint32_t hdrSize = ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + cdesc->propertySize; // look for possible former allocated cache area PcktHeaderElement *header = NULL; - if (cdesc->cacheSize <= hdrSize) { // allocate cache area - if (cdesc->cacheArea != NULL) { - dynmem_free(cdesc->cacheArea); - } - cdesc->cacheArea = (uint8_t *)dynmem_alloc(hdrSize); + if (cdesc->cacheSize < hdrSize) { // allocate cache area if previously allocated area is too small + dynmem_free(cdesc->cacheArea); // does nothing if cacheArea is NULL + cdesc->cacheArea = (uint8_t*) dynmem_alloc(hdrSize); ASSERT_NULL(cdesc->cacheArea); - cdesc->cacheSize = hdrSize; + cdesc->cacheSize = hdrSize; // retain cache size } - header = (PcktHeaderElement *)cdesc->cacheArea; + header = (PcktHeaderElement*) cdesc->cacheArea; memset(header, 0, hdrSize); header->props.ownPacketClass = ownClass; @@ -74,18 +72,19 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { PcktProcFnPassbackData pb; procRet = cdesc->procFun(data + offset, size - offset, header, sieve->intf, &pb); switch (procRet) { - case PROC_FN_RET_REPRST: - dynmem_free(data); // release previous packet data - data = pb.p; // store new packet data - size = pb.u; - mrd = pb.b; - // NO BREAK! - case PROC_FN_RET_ABORT: - goto header_release; // GOTO :D! - break; - case PROC_FN_RET_OK: + case PROC_FN_RET_REPRST: + dynmem_free(data); // release previous packet data + data = pb.p; // store new packet data + size = pb.u; + mrd = pb.b; + // NO BREAK! + case PROC_FN_RET_ABORT: + goto header_release; + // GOTO :D! + break; + case PROC_FN_RET_OK: default: - break; + break; } uint16_t containedClass = header->props.containedPacketClass; @@ -102,17 +101,22 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { lastHeader = header; } while ((ownClass != 0) && lastHeader->props.validityOK); + if (lastHeader == NULL) { // if found nothing, don't attempt to process + goto data_release; + } + lastHeader->next = NULL; // ------------------------------------ if (!lastHeader->props.validityOK) { // if packet is not valid, then drop - goto header_release; // GOTO here! + goto header_release; + // GOTO here! } Pckt packet; - packet.time_s = rawPckt->time_s; - packet.time_ns = rawPckt->time_ns; + packet.time_s = rawPckt->ext.rx.time_s; + packet.time_ns = rawPckt->ext.rx.time_ns; // lookup headers in the sieve const PcktHeaderElement *headerIter = outermostHeader; @@ -125,7 +129,10 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props, sieve->intf); // specific or general match if (found) { layer = nodeIter; // advance in the sieve tree - const PcktHeaderElement *containedHeader = headerIter->next; // advance on headers + PcktHeaderElement *containedHeader = headerIter; + if (headerIter->next != NULL) { + containedHeader = containedHeader->next; // advance on headers + } if (layer->cbFn != NULL) { // if defined, invoke layer callback function offset = containedHeader->props.accumulatedOffset; // accumulated offset + own header size packet.header = containedHeader; @@ -136,11 +143,11 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { // execute special return action switch (action) { - case SIEVE_LAYER_REMOVE_THIS: - packsieve_remove_layer(layer); - break; - default: - break; + case SIEVE_LAYER_REMOVE_THIS: + packsieve_remove_layer(layer); + break; + default: + break; } } headerIter = containedHeader; @@ -156,9 +163,8 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { // INFO("Packet headers not fully processed!\n"); // } - // release header chain blocks - header_release:; // empty line, solely for label placement + header_release: ; // empty line, solely for label placement // PcktHeaderElement *iter = outermostHeader; // while (iter != NULL) { // PcktHeaderElement *next = iter->next; @@ -168,13 +174,14 @@ void packsieve_input(PcktSieve *sieve, const RawPckt *rawPckt) { if (procRet == PROC_FN_RET_REPRST) { // if a restart was requested, then run everything again! goto restart; } - data_release:; + data_release: ; if (mrd) { dynmem_free(data); } } -PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, uint16_t pcktClass) { +PcktSieveLayer* packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, bool matchAny, SieveFilterFn filtFn, SieveCallBackFn cbFn, PcktSieveLayerTag tag, + uint16_t pcktClass) { // search for matching layer PcktSieveLayer *nodeIter = parent->nodes; bool alreadyExists = false; @@ -190,7 +197,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte if (alreadyExists) { return nodeIter; // OK } else { // if allocation of a new layer is required - PcktSieveLayer *layer = (PcktSieveLayer *) dynmem_alloc(sizeof(PcktSieveLayer)); + PcktSieveLayer *layer = (PcktSieveLayer*) dynmem_alloc(sizeof(PcktSieveLayer)); ASSERT_NULL(layer); PcktSieveLayer *oldListFirst = parent->nodes; @@ -221,6 +228,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte layer->cbFn = cbFn; layer->tag = tag; layer->connBReportFn = NULL; + layer->txTsCb = NULL; return layer; } } @@ -260,10 +268,10 @@ bool packsieve_remove_layer(PcktSieveLayer *layer) { #define ETH_SIEVE_LAYER_INDENT_PER_LEVEL (4) -void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint32_t indent) { +void packsieve_report(const PcktSieve *sieve, const PcktSieveLayer *layer, uint32_t indent) { if (layer->connBReportFn != NULL) { INFO("%*c└─┤", indent, ' '); - const ConnBlock connBlock = { sieve, layer, NULL }; // tag not required + ConnBlock connBlock = { sieve, layer }; // tag not required layer->connBReportFn(&connBlock); INFO("├───\n"); } else { @@ -276,11 +284,11 @@ void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint } } -void packsieve_layer_info(const PcktSieve * sieve, const PcktSieveLayer * layer) { +void packsieve_layer_info(const PcktSieve *sieve, const PcktSieveLayer *layer) { const PcktSieveLayer *iter = layer; while (iter != NULL) { // climb up to the top in the sieve tree if (iter->connBReportFn != NULL) { - const ConnBlock connBlock = {sieve, iter, NULL}; // tag not required + ConnBlock connBlock = { sieve, iter }; // tag not required iter->connBReportFn(&connBlock); } else { INFO("[%d]", layer->packetClass); diff --git a/packet_sieve.h b/packet_sieve.h index 1559c4e..87998f3 100644 --- a/packet_sieve.h +++ b/packet_sieve.h @@ -10,7 +10,7 @@ * Packet header information. */ typedef struct PcktHeaderElement_ { - struct PcktHeaderElement_ * next, * prev; ///< Next and previous header in the linked list + struct PcktHeaderElement_ *next, *prev; ///< Next and previous header in the linked list PcktProps props; ///< Properties (allocated to appropriate size) } PcktHeaderElement; @@ -18,7 +18,7 @@ typedef struct PcktHeaderElement_ { * Free chain of packet header. List is auto-rewound. * @param hdr pointer to header chain element */ -void pckthdr_chain_free(PcktHeaderElement * hdr); +void pckthdr_chain_free(PcktHeaderElement *hdr); #define ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE (2 * sizeof(PcktHeaderElement *)) @@ -42,25 +42,25 @@ typedef union { * @param c2 condition 2 * @return true if cond1 matches cond2, otherwise false */ -bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilterCondition * c2); +bool packfiltcond_cmp(const PcktSieveFilterCondition *c1, const PcktSieveFilterCondition *c2); /** * Clear packet filter condition structure. * @param cond pointer to existing filter condition structure */ -void packfiltcond_zero(PcktSieveFilterCondition * cond); +void packfiltcond_zero(PcktSieveFilterCondition *cond); struct EthInterface_; /** * Sieve filter function type. */ -typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps, struct EthInterface_ * intf); +typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, struct EthInterface_ *intf); struct PcktSieveLayer_; typedef union { - void * p; + void *p; uint32_t u; } PcktSieveLayerTag; @@ -70,14 +70,15 @@ typedef union { /** * Callback function type for packet sieve match. */ -typedef int (*SieveCallBackFn)(const Pckt * pckt, PcktSieveLayerTag tag); +typedef int (*SieveCallBackFn)(const Pckt *pckt, PcktSieveLayerTag tag); /** * Callback function for printing connblock report. */ struct ConnBlock_; -typedef void (*ConnBlockReportFn)(const struct ConnBlock_ * connBlock); +typedef void (*ConnBlockReportFn)(const struct ConnBlock_ *connBlock); +typedef void(*TxTimeStampCBFn)(uint32_t ts_s, uint32_t ts_ns, uint32_t tag); #define PCKT_SIEVE_INFOTAG_LEN (24) @@ -91,9 +92,10 @@ typedef struct PcktSieveLayer_ { SieveFilterFn filtFn; ///< Filter function pointer SieveCallBackFn cbFn; ///< Associated callback function PcktSieveLayerTag tag; ///< Layer tag (arbitrary information) - struct PcktSieveLayer_ * parent; ///< Pointer to parent node in the sieve tree - struct PcktSieveLayer_ * next, * prev; ///< Next and previous sieve layer on the same level - struct PcktSieveLayer_ * nodes; ///< Subnodes in the sieve tree + TxTimeStampCBFn txTsCb; ///< Transmit timestamp callback pointer + struct PcktSieveLayer_ *parent; ///< Pointer to parent node in the sieve tree + struct PcktSieveLayer_ *next, *prev; ///< Next and previous sieve layer on the same level + struct PcktSieveLayer_ *nodes; ///< Subnodes in the sieve tree ConnBlockReportFn connBReportFn; ///< Connection block report function pointer } PcktSieveLayer; @@ -102,14 +104,14 @@ typedef struct PcktSieveLayer_ { */ typedef struct PcktSieve_ { PcktSieveLayer layer0; ///< Top of sieve tree - struct EthInterface_ * intf; ///< Ethernet interface + struct EthInterface_ *intf; ///< Ethernet interface } PcktSieve; /** * Create new packet sieve. * @return pointer to packet sieve object or NULL on failure */ -PcktSieve * packsieve_new(); +PcktSieve *packsieve_new(); /** * Process packet with packet sieve. @@ -136,7 +138,7 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte * Remove sieve layer from packet sieve. * @param layer Layer to remove and deallocate */ -bool packsieve_remove_layer(PcktSieveLayer * layer); +bool packsieve_remove_layer(PcktSieveLayer *layer); /** * Recursively report layer structure on terminal. @@ -144,7 +146,7 @@ bool packsieve_remove_layer(PcktSieveLayer * layer); * @param layer pointer to existing layer * @param indent sub-tree indentation */ -void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint32_t indent); +void packsieve_report(const PcktSieve *sieve, const PcktSieveLayer *layer, uint32_t indent); #define packsieve_report_full(sieve) packsieve_report((sieve), &(sieve)->layer0, 0) @@ -153,6 +155,6 @@ void packsieve_report(const PcktSieve * sieve, const PcktSieveLayer *layer, uint * @param sieve pointer to packet sieve structure * @param layer pointer to an existing layer */ -void packsieve_layer_info(const PcktSieve * sieve, const PcktSieveLayer * layer); +void packsieve_layer_info(const PcktSieve *sieve, const PcktSieveLayer *layer); #endif //ETHERLIB_PACKET_SIEVE_H diff --git a/pckt_assembler.c b/pckt_assembler.c index 91e76a4..dc4d776 100644 --- a/pckt_assembler.c +++ b/pckt_assembler.c @@ -10,7 +10,7 @@ #include "global_state.h" #include "utils.h" -int pckt_assemble(RawPckt *raw, Pckt *cooked) { +int pckt_assemble(RawPckt *raw, Pckt *cooked, EthInterface *intf) { // calculate frame size uint16_t frameSize = 0; uint16_t headerSize = 0; @@ -27,7 +27,11 @@ int pckt_assemble(RawPckt *raw, Pckt *cooked) { lastHdr = hdrIter; hdrIter = hdrIter->next; } - frameSize = headerSize + cooked->payloadSize + 4; // header + payload + CRC32 checksum area + frameSize = headerSize + cooked->payloadSize;// + 4; // header + payload + bool computeCRC = !(intf->capabilities & ETHINF_CAP_TX_CRC_OFFLOAD); + if (computeCRC) { // + CRC32 checksum area if needed + frameSize += 4; + } // calculate padding size uint16_t padding = (frameSize < ETH_FRAME_MIN_SIZE) ? (ETH_FRAME_MIN_SIZE - frameSize) : 0; @@ -58,7 +62,7 @@ int pckt_assemble(RawPckt *raw, Pckt *cooked) { } while (hdrIter != NULL) { - hdrIter->props.hdrInsFn(headerInsPtr, hdrIter); // insert header + hdrIter->props.hdrInsFn(headerInsPtr, hdrIter, intf); // insert header hdrIter = hdrIter->prev; // step to previous header if (hdrIter != NULL) { headerInsPtr -= hdrIter->props.headerSize; // advance data pointer @@ -69,9 +73,14 @@ int pckt_assemble(RawPckt *raw, Pckt *cooked) { } } - // insert CRC32 - uint32_t crc = crc32(raw->payload, frameSize - 4); - memcpy(raw->payload + frameSize - 4, (const uint8_t *)&crc, 4); + // insert CRC32 if interface is not capable of inserting CRC + if (computeCRC) { + uint32_t crc = crc32(raw->payload, frameSize - 4); + memcpy(raw->payload + frameSize - 4, (const uint8_t *)&crc, 4); + } + + // turn off TX timestamping by default + raw->ext.tx.txTsCb = NULL; return 0; -} \ No newline at end of file +} diff --git a/pckt_assembler.h b/pckt_assembler.h index 0e4711d..e40221a 100644 --- a/pckt_assembler.h +++ b/pckt_assembler.h @@ -13,6 +13,6 @@ struct EthInterface_; * @param cooked packet information and headers * @return 0 on success OR -1 on failure */ -int pckt_assemble(RawPckt *raw, Pckt *cooked); +int pckt_assemble(RawPckt *raw, Pckt *cooked, struct EthInterface_ *intf); #endif //ETHERLIB_PCKT_ASSEMBLER_H diff --git a/prefab/conn_blocks/arp_connblock.c b/prefab/conn_blocks/arp_connblock.c index ab0916b..ac4ce45 100644 --- a/prefab/conn_blocks/arp_connblock.c +++ b/prefab/conn_blocks/arp_connblock.c @@ -14,6 +14,7 @@ static bool filtArp(const PcktSieveFilterCondition * filtCond, const PcktProps * ConnBlock arp_new_connblock(EthInterface * intf, SieveCallBackFn cb) { ConnBlock arpConnB; // create ARP connblock + connb_init_defaults(&arpConnB); PcktSieveFilterCondition filtCond; packfiltcond_zero(&filtCond); @@ -53,12 +54,13 @@ void arp_send(const ConnBlock * connBlock, const ArpProps * props) { uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_ARP_HEADER_SIZE; uint32_t txBufSize = MAX(txHeaderSize + 4, 60); uint8_t * txBuf = dynmem_alloc(txBufSize); + memset(txBuf, 0, txBufSize); // insert Ethernet header - insert_ethernet_header(txBuf, ethHeader); + insert_ethernet_header(txBuf, ethHeader, NULL); // insert ARP header - insert_arp_header(txBuf + ETH_ETHERNET_HEADER_SIZE, arpHeader); + insert_arp_header(txBuf + ETH_ETHERNET_HEADER_SIZE, arpHeader, NULL); // release headers dynmem_free(arpHeader); @@ -72,8 +74,7 @@ void arp_send(const ConnBlock * connBlock, const ArpProps * props) { RawPckt rpckt; rpckt.size = txBufSize; rpckt.payload = txBuf; - rpckt.time_s = 0; - rpckt.time_ns = 0; + rpckt.ext.tx.txTsCb = NULL; ethinf_transmit(connBlock->sieve->intf, &rpckt); // release transmit buffer diff --git a/prefab/conn_blocks/custom_ethertype_connblock.c b/prefab/conn_blocks/custom_ethertype_connblock.c new file mode 100644 index 0000000..f2e6cd5 --- /dev/null +++ b/prefab/conn_blocks/custom_ethertype_connblock.c @@ -0,0 +1,92 @@ +// +// Created by epagris on 2023.04.25.. +// + +#include "custom_ethertype_connblock.h" +#include "../../eth_interface.h" +#include "../../utils.h" +#include "../../dynmem.h" +#include "../../global_state.h" +#include "etherlib/pckt_assembler.h" + +static bool filtCET(const PcktSieveFilterCondition *filtCond, const PcktProps *contProps, const PcktProps *ownProps, EthInterface *intf) { + EthernetProps *ethProps = (EthernetProps *) contProps; + //MSG("FILT! %X %X\n", ethProps->length_type, ETHERTYPE_FROM_FILTCOND(filtCond)); + return ethProps->length_type == ETHERTYPE_FROM_FILTCOND(filtCond); +} + +cbd cet_new_connblock(struct EthInterface_ *intf, uint16_t etherType, SieveCallBackFn cbFn) { + ConnBlock cetConnB; + connb_init_defaults(&cetConnB); + + PcktSieveFilterCondition filtCond; + packfiltcond_zero(&filtCond); + ETHERTYPE_TO_FILTCOND(&filtCond, etherType); + PcktSieveLayerTag tag; + tag.u = 0; + cetConnB.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, false, filtCET, cbFn, tag, etherType); + ASSERT_NULL(cetConnB.sieveLayer); + cetConnB.sieveLayer->connBReportFn = cet_print_report; + + cetConnB.sieve = &intf->sieve; + + // store connection block to CBDT + cbd d = cbdt_alloc_new(E.cbdt, &cetConnB); + if (d == CBDT_ERR) { // on error free everything we have allocated before + packsieve_remove_layer(cetConnB.sieveLayer); + } + + return d; +} + +int cet_send(cbd d, uint8_t *dest, const uint8_t *data, uint16_t size) { + return cet_send_arg(d, dest, data, size, 0); +} + +int cet_send_arg(cbd d, uint8_t *dest, const uint8_t *data, uint16_t size, uint32_t arg) { + ConnBlock connBlock; + if (!cbdt_get_connection_block(E.cbdt, d, &connBlock)) { + ERROR("Invalid CBD descriptor: '%d'!\n", d); + return 0; + } + + PcktHeaderElement *ethHeader = ALLOC_HEADER_ELEMENT(EthernetProps); + ethHeader->next = NULL; + ethHeader->prev = NULL; + + EthernetProps *ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader); + ethProps->length_type = ETHERTYPE_FROM_FILTCOND(&connBlock.sieveLayer->filtCond); + memcpy(ethProps->destAddr, dest, ETH_HW_ADDR_LEN); + memcpy(ethProps->sourceAddr, connBlock.sieve->intf->mac, ETH_HW_ADDR_LEN); + + ethProps->hdrInsFn = insert_ethernet_header; + ethProps->headerSize = ETH_ETHERNET_HEADER_SIZE; + + Pckt cooked; + cooked.payload = data; + cooked.payloadSize = size; + cooked.header = ethHeader; + + // NOT FILLED FIELDS + cooked.headerSize = 0; + cooked.time_s = 0; + cooked.time_ns = 0; + + RawPckt raw; + pckt_assemble(&raw, &cooked, connBlock.sieve->intf); + raw.ext.tx.arg = arg; // assign argument + raw.ext.tx.txTsCb = connBlock.sieveLayer->txTsCb; // assign with TX timestamp callback + + ethinf_transmit(connBlock.sieve->intf, &raw); + + release_resources: + // free headers + dynmem_free(ethHeader); + + return size; +} + +void cet_print_report(const ConnBlock* connBlock) { + const PcktSieveLayer * sl = connBlock->sieveLayer; + INFO("EtherType: 0x%X", ETHERTYPE_FROM_FILTCOND(&sl->filtCond)); +} diff --git a/prefab/conn_blocks/custom_ethertype_connblock.h b/prefab/conn_blocks/custom_ethertype_connblock.h new file mode 100644 index 0000000..dd413cf --- /dev/null +++ b/prefab/conn_blocks/custom_ethertype_connblock.h @@ -0,0 +1,21 @@ +#ifndef ETHERLIB_CUSTOM_ETHERTYPE_CONNBLOCK_H +#define ETHERLIB_CUSTOM_ETHERTYPE_CONNBLOCK_H + +#include "../../connection_block.h" +#include "../../cbd_table.h" +#include "../packet_parsers/ethernet_frame.h" + +#define ETHERTYPE_FROM_FILTCOND(fc) ((fc)->uw[0]) +#define ETHERTYPE_TO_FILTCOND(fc,et) (((fc)->uw[0]) = (et)) + +struct EthInterface_; + +cbd cet_new_connblock(struct EthInterface_ * intf, uint16_t etherType, SieveCallBackFn cbFn); + +int cet_send_arg(cbd d, uint8_t *dest, const uint8_t *data, uint16_t size, uint32_t arg); + +int cet_send(cbd d, EthernetAddress dest, const uint8_t * data, uint16_t size); + +void cet_print_report(const ConnBlock* connBlock); + +#endif //ETHERLIB_CUSTOM_ETHERTYPE_CONNBLOCK_H diff --git a/prefab/conn_blocks/ethernet_info.c b/prefab/conn_blocks/ethernet_connblock.c similarity index 86% rename from prefab/conn_blocks/ethernet_info.c rename to prefab/conn_blocks/ethernet_connblock.c index 50d7fbc..d35988e 100644 --- a/prefab/conn_blocks/ethernet_info.c +++ b/prefab/conn_blocks/ethernet_connblock.c @@ -2,7 +2,7 @@ // Created by epagris on 2023.01.30.. // -#include "ethernet_info.h" +#include "ethernet_connblock.h" #include "../../utils.h" #include "../../eth_interface.h" diff --git a/prefab/conn_blocks/ethernet_info.h b/prefab/conn_blocks/ethernet_connblock.h similarity index 62% rename from prefab/conn_blocks/ethernet_info.h rename to prefab/conn_blocks/ethernet_connblock.h index 7c9b343..e39d1a6 100644 --- a/prefab/conn_blocks/ethernet_info.h +++ b/prefab/conn_blocks/ethernet_connblock.h @@ -1,5 +1,5 @@ -#ifndef ETHERLIB_ETHERNET_INFO_H -#define ETHERLIB_ETHERNET_INFO_H +#ifndef ETHERLIB_ETHERNET_CONNBLOCK_H +#define ETHERLIB_ETHERNET_CONNBLOCK_H #include "../../connection_block.h" @@ -10,4 +10,4 @@ */ void ethernet_print_report(const ConnBlock* connBlock); -#endif //ETHERLIB_ETHERNET_INFO_H +#endif //ETHERLIB_ETHERNET_CONNBLOCK_H diff --git a/prefab/conn_blocks/icmp_connblock.c b/prefab/conn_blocks/icmp_connblock.c index fa095e3..92608d3 100644 --- a/prefab/conn_blocks/icmp_connblock.c +++ b/prefab/conn_blocks/icmp_connblock.c @@ -21,31 +21,66 @@ static int icmp_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) { IcmpProps * icmpProps = HEADER_FETCH_PROPS(IcmpProps, pckt->header); EthInterface * intf = (EthInterface *) tag.p; // icmp_new_connblock() puts pointer to intf into tag field + static PcktHeaderElement * txIcmpHdr = NULL; + static PcktHeaderElement * txIpHdr = NULL; + static PcktHeaderElement * txEthHdr = NULL; + + static IcmpProps * txIcmpProps = NULL; + static IPv4Props * txIpProps = NULL; + static EthernetProps * txEthProps = NULL; + + if (txIcmpHdr == NULL) { + txIcmpHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(IcmpProps)); + txIcmpHdr->next = NULL; + txIcmpProps = HEADER_FETCH_PROPS(IcmpProps, txIcmpHdr); + } + if (txIpHdr == NULL) { + txIpHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(IPv4Props)); + txIcmpHdr->prev = txIpHdr; + txIpHdr->next = txIcmpHdr; + txIpProps = HEADER_FETCH_PROPS(IPv4Props, txIpHdr); + } + if (txEthHdr == NULL) { + txEthHdr = (PcktHeaderElement *)dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(EthernetProps)); + txIpHdr->prev = txEthHdr; + txEthHdr->next = txIpHdr; + txEthHdr->prev = NULL; + txEthProps = HEADER_FETCH_PROPS(EthernetProps , txEthHdr); + } + switch (icmpProps->type) { case ICMP_MT_ECHO_REQUEST: { - icmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply - icmpProps->hdrInsFn = insert_icmp_header; + *txIcmpProps = *icmpProps; + txIcmpProps->type = ICMP_MT_ECHO_REPLY; // replace request with reply + txIcmpProps->hdrInsFn = insert_icmp_header; // swap source an destination IP addresses IPv4Props * iPv4Props = HEADER_FETCH_PROPS(IPv4Props, pckt->header->prev); - ip4_addr tmpIp = iPv4Props->SourceIPAddr; - iPv4Props->SourceIPAddr = iPv4Props->DestIPAddr; - iPv4Props->DestIPAddr = tmpIp; - iPv4Props->hdrInsFn = insert_ipv4_header; + *txIpProps = *iPv4Props; + ip4_addr tmpIp = txIpProps->SourceIPAddr; + txIpProps->SourceIPAddr = txIpProps->DestIPAddr; + txIpProps->DestIPAddr = tmpIp; + txIpProps->hdrInsFn = insert_ipv4_header; // swap Ethernet fields EthernetAddress tmpEth; EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pckt->header->prev->prev); - HWACPY(tmpEth, ethProps->sourceAddr); - HWACPY(ethProps->sourceAddr, ethProps->destAddr); - HWACPY(ethProps->destAddr, tmpEth); - ethProps->hdrInsFn = insert_ethernet_header; + *txEthProps = *ethProps; + HWACPY(tmpEth, txEthProps->sourceAddr); + HWACPY(txEthProps->sourceAddr, txEthProps->destAddr); + HWACPY(txEthProps->destAddr, tmpEth); + txEthProps->hdrInsFn = insert_ethernet_header; // payload is the same... + Pckt reply; + reply.header = txEthHdr; + reply.payload = pckt->payload; + reply.payloadSize = pckt->payloadSize; + reply.headerSize = pckt->headerSize; // assemble packet RawPckt raw; - pckt_assemble(&raw, pckt); + pckt_assemble(&raw, &reply, intf); // release headers //pckthdr_chain_free(pckt->header); @@ -62,6 +97,8 @@ static int icmp_recv_cb(const Pckt * pckt, PcktSieveLayerTag tag) { ConnBlock icmp_new_connblock(EthInterface * intf) { ConnBlock icmpConnB; + connb_init_defaults(&icmpConnB); + ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block PcktSieveFilterCondition filtCond; diff --git a/prefab/conn_blocks/igmp_connblock.c b/prefab/conn_blocks/igmp_connblock.c index f40fe44..2bc008c 100644 --- a/prefab/conn_blocks/igmp_connblock.c +++ b/prefab/conn_blocks/igmp_connblock.c @@ -19,6 +19,8 @@ static bool filtIgmp(const PcktSieveFilterCondition * filtCond, const PcktProps ConnBlock igmp_new_connblock(struct EthInterface_ *intf) { ConnBlock igmpConnB; + connb_init_defaults(&igmpConnB); + ConnBlock ipConnB = ipv4_new_connblock(intf, IPv4_IF_ADDR, NULL); // create new IPv4 connection block PcktSieveFilterCondition filtCond; @@ -87,7 +89,7 @@ static void igmp_send(ConnBlock * connBlock, ip4_addr ga, int mt) { cooked.time_ns = 0; RawPckt raw; - pckt_assemble(&raw, &cooked); + pckt_assemble(&raw, &cooked, connBlock->sieve->intf); ethinf_transmit(connBlock->sieve->intf, &raw); diff --git a/prefab/conn_blocks/ipv4/ip_assembler.c b/prefab/conn_blocks/ipv4/ip_assembler.c index 48e8299..da1993d 100644 --- a/prefab/conn_blocks/ipv4/ip_assembler.c +++ b/prefab/conn_blocks/ipv4/ip_assembler.c @@ -177,17 +177,17 @@ bool ipra_try_reassemble(IPv4Assembler *ipra, uint16_t id, uint8_t **payload, ui iter = iter->next; } - // release chain - ipra_remove_chain(ipra, id); - // insert headers and change fields to obfuscate defragmentation IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, pcktHdrLe); ipProps->Flags = 0; ipProps->FragmentOffset = 0; ipProps->TotalLength = chainDesc->fullSize + ETH_IPv4_HEADER_SIZE; - insert_ethernet_header(p, pcktHdrLe->prev); - insert_ipv4_header(p + ETH_ETHERNET_HEADER_SIZE, pcktHdrLe); + // release chain + ipra_remove_chain(ipra, id); + + insert_ethernet_header(p, pcktHdrLe->prev, NULL); + insert_ipv4_header(p + ETH_ETHERNET_HEADER_SIZE, pcktHdrLe, NULL); return true; } diff --git a/prefab/conn_blocks/ipv4_connblock.c b/prefab/conn_blocks/ipv4_connblock.c index a607ab9..874e2ca 100644 --- a/prefab/conn_blocks/ipv4_connblock.c +++ b/prefab/conn_blocks/ipv4_connblock.c @@ -19,6 +19,8 @@ static bool filtIPv4(const PcktSieveFilterCondition *filtCond, const PcktProps * ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackFn cbFn) { ConnBlock connb; + connb_init_defaults(&connb); + PcktSieveFilterCondition filtCond; packfiltcond_zero(&filtCond); IP_ADDR_TO_FILTCOND(&filtCond, ipAddr); @@ -47,4 +49,23 @@ void ipv4_print_report(const ConnBlock* connBlock) { } else { INFO("IP: ANY"); } +} + +bool ipv4_in_range(ip4_addr a, ip4_addr l, ip4_addr u) { + l = ntohl(l); // this way IP addresses are mathematically sortable (by changing byte order) + u = ntohl(u); + a = ntohl(a); + return (l <= a) && (a <= u); +} + +bool ipv4_is_multicast_address(ip4_addr addr) { + return ipv4_in_range(addr, IPv4(224, 0, 0, 0), IPv4(224, 0, 255, 255)) || + ipv4_in_range(addr, IPv4(224, 1, 0, 0), IPv4(224, 1, 255, 255)) || + ipv4_in_range(addr, IPv4(224, 3, 0, 0), IPv4(224, 4, 255, 255)) || + ipv4_in_range(addr, IPv4(225, 0, 0, 0), IPv4(239, 255, 255, 255)); +// ipv4_in_range(addr, IPv4(225, 0, 0, 0), IPv4(231, 255, 255, 255)) || +// ipv4_in_range(addr, IPv4(232, 0, 0, 0), IPv4(232, 255, 255, 255)) || +// ipv4_in_range(addr, IPv4(233, 0, 0, 0), IPv4(233, 251, 255, 255)) || +// ipv4_in_range(addr, IPv4(233, 252, 0, 0), IPv4(233, 255, 255, 255)) || +// ipv4_in_range(addr, IPv4(234, 252, 0, 0), IPv4(239, 255, 255, 255)) || } \ No newline at end of file diff --git a/prefab/conn_blocks/ipv4_connblock.h b/prefab/conn_blocks/ipv4_connblock.h index 20ca41a..f3c743d 100644 --- a/prefab/conn_blocks/ipv4_connblock.h +++ b/prefab/conn_blocks/ipv4_connblock.h @@ -34,4 +34,11 @@ void ipv4_send(EthInterface * intf, ip4_addr addr, const uint8_t * data, uint32_ */ void ipv4_print_report(const ConnBlock* connBlock); +/** + * Is the given address a multicast one? + * @param addr IP address to check + * @return ... + */ +bool ipv4_is_multicast_address(ip4_addr addr); + #endif //ETHERLIB_IPV4_CONNBLOCK_H diff --git a/prefab/conn_blocks/tcp_connblock.c b/prefab/conn_blocks/tcp_connblock.c index 3af95e5..e59aefa 100644 --- a/prefab/conn_blocks/tcp_connblock.c +++ b/prefab/conn_blocks/tcp_connblock.c @@ -233,6 +233,8 @@ void tcp_debug(cbd d, bool debug) { cbd tcp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) { ConnBlock tcpConnB; + connb_init_defaults(&tcpConnB); + ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block PcktSieveFilterCondition filtCond; @@ -335,7 +337,7 @@ int tcp_send_segment(const struct ConnBlock_ *connBlock, TcpFlag flags, TcpOptio cooked.time_ns = 0; RawPckt raw; - pckt_assemble(&raw, &cooked); + pckt_assemble(&raw, &cooked, connBlock->sieve->intf); ethinf_transmit(connBlock->sieve->intf, &raw); diff --git a/prefab/conn_blocks/udp_connblock.c b/prefab/conn_blocks/udp_connblock.c index 306bc36..df54d76 100644 --- a/prefab/conn_blocks/udp_connblock.c +++ b/prefab/conn_blocks/udp_connblock.c @@ -20,6 +20,8 @@ static bool filtUdp(const PcktSieveFilterCondition *filtCond, const PcktProps *c cbd udp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) { ConnBlock udpConnB; + connb_init_defaults(&udpConnB); + ConnBlock ipConnB = ipv4_new_connblock(intf, ipAddr, NULL); // create new IPv4 connection block PcktSieveFilterCondition filtCond; @@ -42,6 +44,10 @@ cbd udp_new_connblock(EthInterface *intf, ip4_addr ipAddr, uint16_t port, SieveC } int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port) { + return udp_sendto_arg(d, data, size, addr, port, 0); +} + +int udp_sendto_arg(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port, uint32_t arg) { ConnBlock connBlock; if (!cbdt_get_connection_block(E.cbdt, d, &connBlock)) { ERROR("Invalid CBD descriptor: '%d'!\n", d); @@ -82,7 +88,8 @@ int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_ // Ethernet layer = layer->parent; - if (addr != 0xFFFFFFFF) { + bool isMulticast = ipv4_is_multicast_address(addr); + if ((!isMulticast) && (addr != 0xFFFFFFFF)) { // unicast ArpCache *arpc = connBlock.sieve->intf->arpc; const ArpEntry *entry = arpc_get_ask(arpc, addr); if (entry == NULL) { @@ -90,8 +97,10 @@ int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_ goto release_resources; // YEAH, goto HERE! } memcpy(ethProps->destAddr, entry, ETH_HW_ADDR_LEN); - } else { - memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN); + } else if (isMulticast) { // multicast + ethmc_from_ipmc(ethProps->destAddr, addr); + } else { // unicast + memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN); // broadcast } memcpy(ethProps->sourceAddr, connBlock.sieve->intf->mac, ETH_HW_ADDR_LEN); ethProps->length_type = ETH_IPv4_PACKET_CLASS; @@ -111,7 +120,9 @@ int udp_sendto(cbd d, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_ cooked.time_ns = 0; RawPckt raw; - pckt_assemble(&raw, &cooked); + pckt_assemble(&raw, &cooked, connBlock.sieve->intf); + raw.ext.tx.arg = arg; // assign argument + raw.ext.tx.txTsCb = connBlock.sieveLayer->txTsCb; // assign with TX timestamp callback ethinf_transmit(connBlock.sieve->intf, &raw); diff --git a/prefab/conn_blocks/udp_connblock.h b/prefab/conn_blocks/udp_connblock.h index 0f8724d..471672d 100644 --- a/prefab/conn_blocks/udp_connblock.h +++ b/prefab/conn_blocks/udp_connblock.h @@ -21,7 +21,7 @@ cbd udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn); /** - * UDP transmit callback. + * UDP transmit function. * @param connBlock UDP connection block * @param data pointer to data buffer * @param size data size @@ -30,6 +30,17 @@ cbd udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, Sieve */ int udp_sendto(cbd connBlock, const uint8_t * data, uint32_t size, ip4_addr addr, uint16_t port); +/** + * UDP transmit function with additional argument. + * @param connBlock UDP connection block + * @param data pointer to data buffer + * @param size data size + * @param addr remote address to send datagram to + * @param addr remote port + * @param arg user-defined parameter + */ +int udp_sendto_arg(cbd connBlock, const uint8_t * data, uint32_t size, ip4_addr addr, uint16_t port, uint32_t arg); + /** * Print UDP connblock report. * @param connBlock UDP connblock diff --git a/prefab/packet_parsers/arp_packet.c b/prefab/packet_parsers/arp_packet.c index 2a069ea..a16925a 100644 --- a/prefab/packet_parsers/arp_packet.c +++ b/prefab/packet_parsers/arp_packet.c @@ -47,7 +47,7 @@ int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s return arpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT; } -void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) { ArpProps *arpProps = HEADER_FETCH_PROPS(ArpProps, headers); FILL_WORD_H2N_ADVANCE(hdr, arpProps->HTYPE); // hardware type diff --git a/prefab/packet_parsers/arp_packet.h b/prefab/packet_parsers/arp_packet.h index 74ee4a8..9aa0a7e 100644 --- a/prefab/packet_parsers/arp_packet.h +++ b/prefab/packet_parsers/arp_packet.h @@ -58,7 +58,7 @@ int parse_arp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s * @param hdr pointer to the packet buffer where the ARP header is to be written * @param headers linked list of packet headers */ -void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers); +void insert_arp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); #endif //ETHERLIB_ARP_PACKET_H diff --git a/prefab/packet_parsers/ethernet_frame.c b/prefab/packet_parsers/ethernet_frame.c index aba9627..3553cc4 100644 --- a/prefab/packet_parsers/ethernet_frame.c +++ b/prefab/packet_parsers/ethernet_frame.c @@ -5,6 +5,7 @@ #include #include #include "ethernet_frame.h" +#include "../../eth_interface.h" #include "../../utils.h" #define ETH_ETHERTYPE_LENGTH_THRESHOLD (1500) @@ -24,10 +25,10 @@ int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdr frameProps->length_type <= ETH_ETHERTYPE_LENGTH_THRESHOLD ? 0 : frameProps->length_type; frameProps->headerSize = ETH_ETHERNET_HEADER_SIZE; - return frameProps->containedPacketClass; + return PROC_FN_RET_OK; } -void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) { EthernetProps * ethProps = (EthernetProps *) &headers->props; FILL_ADVANCE(hdr, ethProps->destAddr, ETH_HW_ADDR_LEN); FILL_ADVANCE(hdr, ethProps->sourceAddr, ETH_HW_ADDR_LEN); diff --git a/prefab/packet_parsers/ethernet_frame.h b/prefab/packet_parsers/ethernet_frame.h index 17028a5..919bb91 100644 --- a/prefab/packet_parsers/ethernet_frame.h +++ b/prefab/packet_parsers/ethernet_frame.h @@ -49,6 +49,6 @@ int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdr * @param hdr space where the header is to be inserted * @param headers linked list of header, top is always relevant */ -void insert_ethernet_header(uint8_t * hdr, const PcktHeaderElement * headers); +void insert_ethernet_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); #endif //ETHERLIB_ETHERNET_FRAME_H diff --git a/prefab/packet_parsers/icmp_packet.c b/prefab/packet_parsers/icmp_packet.c index d81d7dd..0c3b789 100644 --- a/prefab/packet_parsers/icmp_packet.c +++ b/prefab/packet_parsers/icmp_packet.c @@ -26,7 +26,7 @@ int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, return icmpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT; } -void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf) { uint8_t * hdrStart = hdr; IcmpProps *icmpProps = HEADER_FETCH_PROPS(IcmpProps, headers); FILL_BYTE_ADVANCE(hdr, &icmpProps->type); diff --git a/prefab/packet_parsers/icmp_packet.h b/prefab/packet_parsers/icmp_packet.h index 4c49d55..77ecfd5 100644 --- a/prefab/packet_parsers/icmp_packet.h +++ b/prefab/packet_parsers/icmp_packet.h @@ -41,6 +41,6 @@ int parse_icmp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, * @param hdr pointer to message header * @param headers linked list of recursively encapsulated packet headers */ -void insert_icmp_header(uint8_t * hdr, const PcktHeaderElement * headers); +void insert_icmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); #endif //ETHERLIB_ICMP_PACKET_H diff --git a/prefab/packet_parsers/igmp_packet.c b/prefab/packet_parsers/igmp_packet.c index a6f189d..c64d1db 100644 --- a/prefab/packet_parsers/igmp_packet.c +++ b/prefab/packet_parsers/igmp_packet.c @@ -25,7 +25,7 @@ // return sum16; //} -void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf) { IgmpProps *igmpProps = HEADER_FETCH_PROPS(IgmpProps, headers); uint8_t * hdrBeg = hdr; diff --git a/prefab/packet_parsers/igmp_packet.h b/prefab/packet_parsers/igmp_packet.h index 562ffe2..6746309 100644 --- a/prefab/packet_parsers/igmp_packet.h +++ b/prefab/packet_parsers/igmp_packet.h @@ -33,7 +33,7 @@ typedef struct { * @param hdr pointer to message header * @param headers linked list of recursively encapsulated packet headers */ -void insert_igmp_header(uint8_t * hdr, const PcktHeaderElement * headers); +void insert_igmp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); /** * Parse IGMP packet. diff --git a/prefab/packet_parsers/ipv4_packet.c b/prefab/packet_parsers/ipv4_packet.c index 96a3fd1..217d7a9 100644 --- a/prefab/packet_parsers/ipv4_packet.c +++ b/prefab/packet_parsers/ipv4_packet.c @@ -11,11 +11,11 @@ #define ETH_IP_HEADER_LENGTH (20) -static bool check_ipv4_validity(const uint8_t *hdr, const IPv4Props *ipProps) { +static bool check_ipv4_validity(const uint8_t *hdr, const IPv4Props *ipProps, bool dontVerifyChecksum) { bool valid = (ipProps->Version == 4) && (ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) && - (chksum(hdr, ETH_IPv4_HEADER_SIZE, false) == 0); /*&& + (dontVerifyChecksum || (chksum(hdr, ETH_IPv4_HEADER_SIZE, false) == 0)); /*&& (ipProps->FragmentOffset == 0) && !(ipProps->Flags & 0x02 << 4); // TODO: discard if fragmented*/ return valid; } @@ -41,7 +41,7 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, FETCH_ADVANCE(&ipProps->DestIPAddr, hdr, 4); // fill-in common packet header fields - ipProps->validityOK = check_ipv4_validity(hdrBegin, ipProps); + ipProps->validityOK = check_ipv4_validity(hdrBegin, ipProps, (intf->capabilities & ETHINF_CAP_RX_IPCHKSUM_OFFLOAD)); // don't verify checksum if done in hardware ipProps->containedPacketClass = ipProps->Protocol; ipProps->headerSize = ETH_IP_HEADER_LENGTH; @@ -78,7 +78,7 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, static uint16_t nextIpIdentification = 0; -void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) { uint8_t *hdrOrig = hdr; IPv4Props *ipProps = (IPv4Props *) &headers->props; ipProps->Identification = nextIpIdentification++; // auto-insert identification @@ -96,9 +96,11 @@ void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) { FILL_ADVANCE(hdr, &ipProps->SourceIPAddr, 4); FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4); - // calculate checksum after filling header - ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE, false); - memcpy(ChkSumPtr, &ipProps->HeaderChecksum, 2); + // calculate checksum after filling header if needed + if (!(intf->capabilities & ETHINF_CAP_TX_IPCHKSUM_OFFLOAD)) { + ipProps->HeaderChecksum = chksum(hdrOrig, ETH_IPv4_HEADER_SIZE, false); + memcpy(ChkSumPtr, &ipProps->HeaderChecksum, 2); + } } void ethmc_from_ipmc(uint8_t *hwa, ip4_addr ipa) { diff --git a/prefab/packet_parsers/ipv4_packet.h b/prefab/packet_parsers/ipv4_packet.h index 7362ed3..d262bf5 100644 --- a/prefab/packet_parsers/ipv4_packet.h +++ b/prefab/packet_parsers/ipv4_packet.h @@ -57,7 +57,7 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, * @param hdr space where the header is to be inserted * @param headers linked list of header, top is always relevant */ -void insert_ipv4_header(uint8_t * hdr, const PcktHeaderElement * headers); +void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); /** * Calculate Ethernet IPv4 multicast address from multicast diff --git a/prefab/packet_parsers/tcp_segment.c b/prefab/packet_parsers/tcp_segment.c index e2f788b..28da392 100644 --- a/prefab/packet_parsers/tcp_segment.c +++ b/prefab/packet_parsers/tcp_segment.c @@ -6,6 +6,7 @@ #include "../../utils.h" #include "ethernet_frame.h" #include "../../dynmem.h" +#include "../../eth_interface.h" #include "tcp_udp_common.h" #include "ipv4_packet.h" @@ -140,20 +141,24 @@ int parse_tcp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s } // verify checksum - const IPv4Props *ipProps = (IPv4Props *) &pcktHdrLe->prev->props; - uint16_t headerAndPayloadLength = ipProps->bytesToEnd; - IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_TCP_PACKET_CLASS, htons(headerAndPayloadLength)}; - //uint16_t chkSum = tcp_udp_checksum(&ph, hdrBegin, headerAndPayloadLength); - uint16_t chkSum = 0; + if (!(intf->capabilities & ETHINF_CAP_RX_TCPUDPCHKSUM_OFFLOAD)) { // TODO + const IPv4Props *ipProps = (IPv4Props *) &pcktHdrLe->prev->props; + uint16_t headerAndPayloadLength = ipProps->bytesToEnd; + IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_TCP_PACKET_CLASS, htons(headerAndPayloadLength)}; + //uint16_t chkSum = tcp_udp_checksum(&ph, hdrBegin, headerAndPayloadLength); + uint16_t chkSum = 0; // TODO .... + tcpProps->validityOK = (chkSum == 0); + } else { + tcpProps->validityOK = true; + } tcpProps->headerSize = fullHeaderSize; - tcpProps->validityOK = (chkSum == 0); tcpProps->containedPacketClass = 0; return tcpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT; } -void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) { uint8_t * hdrBegin = hdr; TcpProps * tcpProps = HEADER_FETCH_PROPS(TcpProps, headers); // fetch header @@ -181,11 +186,13 @@ void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers) { tcp_insert_options(hdr, optSize, tcpProps->options); // calculate checksum - const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props; - uint16_t headerAndPayloadLength = fullHeaderSize + headers->props.bytesToEnd; - IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_TCP_PACKET_CLASS, htons(headerAndPayloadLength)}; - tcpProps->Checksum = tcp_udp_checksum(&ph, hdrBegin, headerAndPayloadLength); - memcpy(ChkSumPtr, &tcpProps->Checksum, 2); + if (!(intf->capabilities & ETHINF_CAP_TX_TCPUDPCHKSUM_OFFLOAD)) { + const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props; + uint16_t headerAndPayloadLength = fullHeaderSize + headers->props.bytesToEnd; + IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_TCP_PACKET_CLASS, htons(headerAndPayloadLength)}; + tcpProps->Checksum = tcp_udp_checksum(&ph, hdrBegin, headerAndPayloadLength); + memcpy(ChkSumPtr, &tcpProps->Checksum, 2); + } } // ---------------------------------- diff --git a/prefab/packet_parsers/tcp_segment.h b/prefab/packet_parsers/tcp_segment.h index ecf853b..827640b 100644 --- a/prefab/packet_parsers/tcp_segment.h +++ b/prefab/packet_parsers/tcp_segment.h @@ -101,7 +101,7 @@ int parse_tcp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s * @param hdr space where the header is to be inserted * @param headers linked list of header, top is always relevant */ -void insert_tcp_header(uint8_t * hdr, const PcktHeaderElement * headers); +void insert_tcp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); /** * Get the flattened size of the option chain. diff --git a/prefab/packet_parsers/udp_packet.c b/prefab/packet_parsers/udp_packet.c index fbfd69b..99edac4 100644 --- a/prefab/packet_parsers/udp_packet.c +++ b/prefab/packet_parsers/udp_packet.c @@ -5,6 +5,7 @@ #include "ethernet_frame.h" #include "tcp_udp_common.h" +#include "etherlib/eth_interface.h" #define ETH_UDP_HEADER_SIZE (8) @@ -27,13 +28,17 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s // common fields... udpProps->headerSize = ETH_UDP_HEADER_SIZE; - udpProps->validityOK = check_udp_validity(hdrBegin, pcktHdrLe); + if (!(intf->capabilities & ETHINF_CAP_RX_TCPUDPCHKSUM_OFFLOAD)) { + udpProps->validityOK = check_udp_validity(hdrBegin, pcktHdrLe); + } else { + udpProps->validityOK = true; + } udpProps->containedPacketClass = 0; return udpProps->validityOK ? PROC_FN_RET_OK : PROC_FN_RET_ABORT; } -void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) { +void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers, EthInterface *intf) { uint8_t *hdrBegin = hdr; UdpProps *udpProps = (UdpProps *) &headers->props; FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort); @@ -43,11 +48,13 @@ void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) { udpProps->Checksum = 0; FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum); - // calculate checksum - const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props; - IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)}; - udpProps->Checksum = tcp_udp_checksum(&ph, hdrBegin, udpProps->Length); - memcpy(ChkSumPtr, &udpProps->Checksum, 2); + // calculate checksum if needed + if (!(intf->capabilities & ETHINF_CAP_TX_TCPUDPCHKSUM_OFFLOAD)) { + const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props; + IPv4PseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)}; + udpProps->Checksum = tcp_udp_checksum(&ph, hdrBegin, udpProps->Length); + memcpy(ChkSumPtr, &udpProps->Checksum, 2); + } } diff --git a/prefab/packet_parsers/udp_packet.h b/prefab/packet_parsers/udp_packet.h index c3b863a..3a39cdb 100644 --- a/prefab/packet_parsers/udp_packet.h +++ b/prefab/packet_parsers/udp_packet.h @@ -37,6 +37,6 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, s * @param hdr space where the header is to be inserted * @param headers linked list of header, top is always relevant */ -void insert_udp_header(uint8_t * hdr, const PcktHeaderElement * headers); +void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers, struct EthInterface_ *intf); #endif //ETHERLIB_UDP_PACKET_H diff --git a/timestamping.c b/timestamping.c new file mode 100644 index 0000000..f7a5bf9 --- /dev/null +++ b/timestamping.c @@ -0,0 +1,19 @@ +// +// Created by epagris on 2023.04.22.. +// + +#include "timestamping.h" + +#include "global_state.h" +#include "utils.h" + +void ts_set_tx_callback(cbd d, void (*cb)(uint32_t, uint32_t, uint32_t)) { + ConnBlock connb; + if (!cbdt_get_connection_block(E.cbdt, d, &connb)) { // try to fetch connection block + ERROR("Unknown CBD: %u\n", d); // if not found, then do nothing + return; + } + + // if found... + connb.sieveLayer->txTsCb = cb; +} diff --git a/timestamping.h b/timestamping.h new file mode 100644 index 0000000..acfb5b2 --- /dev/null +++ b/timestamping.h @@ -0,0 +1,13 @@ +#ifndef ETHERLIB_TIMESTAMPING_H +#define ETHERLIB_TIMESTAMPING_H + +#include "cbd_table.h" + +/** + * Assign callback funtion to specific connection block. + * @param d CBD to existing connection + * @param cb pointer to callback function + */ +void ts_set_tx_callback(cbd d, void(*cb)(uint32_t ts_s, uint32_t ts_ns, uint32_t tag)); + +#endif //ETHERLIB_TIMESTAMPING_H diff --git a/utils.h b/utils.h index 4f87e93..a13e77a 100644 --- a/utils.h +++ b/utils.h @@ -5,6 +5,8 @@ #ifndef ETHERLIB_UTILS_H #define ETHERLIB_UTILS_H +#include + #include #include #include @@ -35,8 +37,6 @@ #define WARNING(...) MSG(__VA_ARGS__) #define INFO(...) MSG(__VA_ARGS__) -#define SNPRINTF(s,n,...) snprintf(s,n,__VA_ARGS__) - #define IPv4(a,b,c,d) ((a) | (b << 8) | (c << 16) | (d << 24)) #define PRINT_IPv4(ip) MSG("%u.%u.%u.%u", (ip & 0xFF), ((ip >> 8) & 0xFF), ((ip >> 16) & 0xFF), ((ip >> 24) & 0xFF)) #define PRINT_HWADDR(hwaddr) MSG("%02x:%02x:%02x:%02x:%02x:%02x", (hwaddr)[0], (hwaddr)[1], (hwaddr)[2], (hwaddr)[3], (hwaddr)[4], (hwaddr)[5]); @@ -66,8 +66,12 @@ #define FILL_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du = htonl(dw); memcpy(dst, &du, 4); (dst) += 4; } #define FILL_DWORD_ADVANCE(dst,dw) { memcpy(dst, &(dw), 4); (dst) += 4; } +#ifndef MIN #define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef MAX #define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#endif // ------------------