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)
This commit is contained in:
parent
30af96f4c9
commit
ab8d45932f
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
9
dynmem.h
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
14
etherlib.h
Normal 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
|
||||
@ -92,3 +92,7 @@ void close_connection(cbd d) {
|
||||
|
||||
connb_remove(&connBlock); // remove connection block
|
||||
}
|
||||
|
||||
EthInterface *get_default_interface() {
|
||||
return E.ethIntf;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
20
msg_queue.c
@ -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
14
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.
|
||||
|
||||
@ -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) */ \
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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
|
||||
|
||||
@ -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
92
prefab/conn_blocks/custom_ethertype_connblock.c
Normal 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
21
prefab/conn_blocks/custom_ethertype_connblock.h
Normal 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,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"
|
||||
@ -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
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
@ -48,3 +50,22 @@ void ipv4_print_report(const ConnBlock* connBlock) {
|
||||
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)) ||
|
||||
}
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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,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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
// ----------------------------------
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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
19
timestamping.c
Normal 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
13
timestamping.h
Normal 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
8
utils.h
@ -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
|
||||
|
||||
// ------------------
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user