epagris/EtherLib
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Timestamping and bunch of bugfix and optimization

Browse Source 
- 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)
This commit is contained in:
Wiesner András 2023-04-27 09:38:26 +02:00
parent 30af96f4c9
commit ab8d45932f
47 changed files with 537 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
arp_cache.c
View File

@ -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;

4
connection_block.c
View File

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

7
connection_block.h
View File

@ -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

9
dynmem.h
View File

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

31
eth_interface.c
View File

@ -6,16 +6,22 @@
#include "dynmem.h"
#include "utils.h"
#include "etherlib_options.h"
#include "etherlib/prefab/conn_blocks/ethernet_info.h"
#include "etherlib/prefab/conn_blocks/ethernet_connblock.h"
static int ethintf_llrecv(EthIODef * io, const RawPckt * pckt) {
ethinf_receive((EthInterface *) io->tag, pckt);
return 0;
}
static int ethintf_llrxnotify(EthIODef * io) {
ethinf_notify((EthInterface *) io->tag);
return 0;
}
static void ethintf_register(EthInterface * intf) {
intf->ioDef->tag = intf;
intf->ioDef->llRxDone = ethintf_llrecv;
intf->ioDef->llRxStore = ethintf_llrecv;
intf->ioDef->llRxNotify = ethintf_llrxnotify;
}
// interface processing thread
@ -36,11 +42,13 @@ EthInterface *ethintf_new(EthIODef * io) {
ethIntf->txQ = mq_create(ETHLIB_DEF_MQ_SIZE);
ethIntf->rxQ = mq_create(ETHLIB_DEF_MQ_SIZE);
ETHLIB_OS_SEM_CREATE(&ethIntf->rxSem, ETHLIB_DEF_MQ_SIZE);
ETHLIB_OS_THREAD_DEFINE(ethinf_proc_thread, ipp, osPriorityHigh, 4096, ethIntf);
ETHLIB_OS_THREAD_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;
}

32
eth_interface.h
View File

@ -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

14
etherlib.h Normal file
View File

@ -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

4
global_state.c
View File

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

6
global_state.h
View File

@ -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

4
memory_pool.c
View File

@ -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;

20
msg_queue.c
View File

@ -10,7 +10,7 @@
#include <stdio.h>
MsgQueue *mq_create(uint32_t size) {
MsgQueue * mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
MsgQueue *mq = (MsgQueue *) dynmem_alloc(sizeof(MsgQueue) + size * sizeof(RawPckt));
mq->size = size;
mq->readIdx = 0;
mq->writeIdx = 0;
@ -19,19 +19,23 @@ MsgQueue *mq_create(uint32_t size) {
return mq;
}
void mq_clear(MsgQueue * mq) {
void mq_clear(MsgQueue *mq) {
mq->readIdx = 0;
mq->writeIdx = 0;
memset(mq->pckts, 0, mq->size * sizeof(RawPckt));
}
uint32_t mq_avail(const MsgQueue * mq) {
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);
}

14
packet.h
View File

@ -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.

2
packet_registry.h
View File

@ -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) */ \

86
packet_sieve.c
View File

@ -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);

34
packet_sieve.h
View File

@ -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

23
pckt_assembler.c
View File

@ -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;
}
}

2
pckt_assembler.h
View File

@ -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

9
prefab/conn_blocks/arp_connblock.c
View File

@ -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

92
prefab/conn_blocks/custom_ethertype_connblock.c Normal file
View File

@ -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));
}

21
prefab/conn_blocks/custom_ethertype_connblock.h Normal file
View File

@ -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

2
prefab/conn_blocks/ethernet_info.c → prefab/conn_blocks/ethernet_connblock.c
View File

@ -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"

6
prefab/conn_blocks/ethernet_info.h → prefab/conn_blocks/ethernet_connblock.h
View File

@ -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

59
prefab/conn_blocks/icmp_connblock.c
View File

@ -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;

4
prefab/conn_blocks/igmp_connblock.c
View File

@ -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);

10
prefab/conn_blocks/ipv4/ip_assembler.c
View File

@ -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;
}

21
prefab/conn_blocks/ipv4_connblock.c
View File

@ -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)) ||
}

7
prefab/conn_blocks/ipv4_connblock.h
View File

@ -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

4
prefab/conn_blocks/tcp_connblock.c
View File

@ -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);

19
prefab/conn_blocks/udp_connblock.c
View File

@ -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);

13
prefab/conn_blocks/udp_connblock.h
View File

@ -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

2
prefab/packet_parsers/arp_packet.c
View File

@ -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

2
prefab/packet_parsers/arp_packet.h
View File

@ -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

5
prefab/packet_parsers/ethernet_frame.c
View File

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

2
prefab/packet_parsers/ethernet_frame.h
View File

@ -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

2
prefab/packet_parsers/icmp_packet.c
View File

@ -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);

2
prefab/packet_parsers/icmp_packet.h
View File

@ -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

2
prefab/packet_parsers/igmp_packet.c
View File

@ -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;

2
prefab/packet_parsers/igmp_packet.h
View File

@ -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.

16
prefab/packet_parsers/ipv4_packet.c
View File

@ -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) {

2
prefab/packet_parsers/ipv4_packet.h
View File

@ -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

31
prefab/packet_parsers/tcp_segment.c
View File

@ -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);
}
}
// ----------------------------------

2
prefab/packet_parsers/tcp_segment.h
View File

@ -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.

21
prefab/packet_parsers/udp_packet.c
View File

@ -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);
}
}

2
prefab/packet_parsers/udp_packet.h
View File

@ -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

19
timestamping.c Normal file
View File

@ -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;
}

13
timestamping.h Normal file
View File

@ -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

8
utils.h
View File

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