ARP cache added (initial implementation); DHCP basics
This commit is contained in:
parent
e6c866b018
commit
905b455bb3
42
arp_cache.c
Normal file
42
arp_cache.c
Normal file
@ -0,0 +1,42 @@
|
||||
//
|
||||
// Created by epagris on 2022.12.10..
|
||||
//
|
||||
|
||||
#include <memory.h>
|
||||
#include "arp_cache.h"
|
||||
#include "dynmem.h"
|
||||
|
||||
ArpCache *arpc_new(uint16_t size) {
|
||||
ArpCache * arcp = (ArpCache *) dynmem_alloc(2 * sizeof(uint16_t) + size * sizeof(ArpEntry));
|
||||
arcp->size = size;
|
||||
arcp->fill = 0;
|
||||
return arcp;
|
||||
}
|
||||
|
||||
void arpc_free(ArpCache *aprc) {
|
||||
dynmem_free(aprc);
|
||||
}
|
||||
|
||||
void arpc_learn(ArpCache *arpc, const ArpEntry *newEntry) {
|
||||
// TODO: nagyon dummy...
|
||||
for (uint16_t i = 0; i < arpc->fill; i++) {
|
||||
ArpEntry * entry = arpc->entries + i;
|
||||
if (!memcmp(entry->eth, newEntry->eth, ETH_HW_ADDR_LEN) && (entry->ip == newEntry->ip)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// if not found...
|
||||
arpc->entries[arpc->fill++] = *newEntry;
|
||||
}
|
||||
|
||||
const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip) {
|
||||
for (uint16_t i = 0; i < arpc->fill; i++) {
|
||||
ArpEntry * entry = arpc->entries + i;
|
||||
if (entry->ip == ip) {
|
||||
return entry;
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
51
arp_cache.h
Normal file
51
arp_cache.h
Normal file
@ -0,0 +1,51 @@
|
||||
#ifndef ETHERLIB_TEST_ARP_CACHE_H
|
||||
#define ETHERLIB_TEST_ARP_CACHE_H
|
||||
|
||||
#include "prefab/packet_parsers/ethernet_frame.h"
|
||||
#include "prefab/packet_parsers/ipv4_types.h"
|
||||
|
||||
/**
|
||||
* ARP cache entry record.
|
||||
*/
|
||||
typedef struct {
|
||||
EthernetAddress eth; ///< Ethernet address
|
||||
ip4_addr ip; ///< IPv4 address
|
||||
} ArpEntry;
|
||||
|
||||
/**
|
||||
* ARP cache class.
|
||||
*/
|
||||
typedef struct {
|
||||
uint16_t size; ///< Number of cache entries
|
||||
uint16_t fill; ///< Fill level
|
||||
ArpEntry entries[]; ///< Cache entries
|
||||
} ArpCache;
|
||||
|
||||
/**
|
||||
* Create new ARP cache table.
|
||||
* @param size Number of available entries
|
||||
* @return Pointer to newly allocated ARP cache
|
||||
*/
|
||||
ArpCache * arpc_new(uint16_t size);
|
||||
|
||||
/**
|
||||
* Learn new assignment.
|
||||
* @param arpc Pointer to ARP cache
|
||||
* @param newEntry Pointer to map newEntry
|
||||
*/
|
||||
void arpc_learn(ArpCache * arpc, const ArpEntry * newEntry);
|
||||
|
||||
/**
|
||||
* Get entry by IP-address.
|
||||
* @param ip IP address
|
||||
* @return Pointer to matching entry OR NULL if not found
|
||||
*/
|
||||
const ArpEntry *arpc_get(ArpCache *arpc, ip4_addr ip);
|
||||
|
||||
/**
|
||||
* Free ARP cache.
|
||||
* @param aprc Pointer to existing ARP cache.
|
||||
*/
|
||||
void arpc_free(ArpCache * aprc);
|
||||
|
||||
#endif //ETHERLIB_TEST_ARP_CACHE_H
|
||||
@ -8,6 +8,9 @@ struct ConnBlock_;
|
||||
|
||||
typedef int (*ConnBlockTransmitFn)(EthInterface * intf, const uint8_t * data, uint32_t size, const struct ConnBlock_ * connBlock);
|
||||
|
||||
/**
|
||||
* Connection block.
|
||||
*/
|
||||
typedef struct ConnBlock_ {
|
||||
EthInterface * intf; ///< Ethernet interface
|
||||
PcktSieveLayer * sieveLayer; ///< Sieve layer
|
||||
|
||||
1
docs/.$PacketSieve.bkp
Normal file
1
docs/.$PacketSieve.bkp
Normal file
@ -0,0 +1 @@
|
||||
<mxfile host="Electron" modified="2022-12-09T10:36:11.906Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="y_61wgbl8uiCwZsw6xJH" version="20.3.0" type="device"><diagram id="apy941U9fxJ8yi3WkWR-" name="PacketSieve">7Vpbk6I4FP41PrYFCTcfu217LtUz27tWz8w+bSEEYTYQK8bb/voNEgRMWmlFdBy7qyzOITfOd75zTggd2I+XH6g7Cb8QH+EO0PxlBz52AHB0yH9TxSpTmKaeKcY08jNVSTGM/kNCqQntLPLRtNKQEYJZNKkqPZIkyGMVnUspWVSbBQRXZ524YyQphp6LZe33yGeheCxTK/QfUTQO85l1TdyJ3byxUExD1yeLkgoOOrBPCWHZVbzsI5zaLrdL1u/pjbubhVGUsDod/rj7q0fiAXz+TO694bdXr0eHd7oYZu7imXhisVq2yk2AfG4RIRLKQjImiYsHhfbBm9E5SqfRuUDJLPHXksalosMzIRPR5CdibCWwdmeMcFXIYizuBiRhT24c4dRl+mRGI0T5ir6ihbgpejpclI0gHmjK+3lo15MLZ3LpGLEd7aysXWqE0gTCxB8QiRGjK96AIuyyaF51G1d433jTrgCIXwiM3oOXBNeAhYgm/BG2YVuEEUPDibu2woKT8z1WjjDuE0zoeijou8gJPK6fMkr+RaU7luegUbALiDmiDC13mi6/CwVTRKgAmpnJi4J4tmgSljiXdzvG2NOf375+fg6I9sw0a/DnP7BnvN7p4PLIofTGwxhgyAxQm+GsHm9IGHx6mRsn9XYTOb6h8nYHjKBlNePt0Kh6u5679h5vBw14uzq0yEZt27m5Nenqh+i/Fv5Oha6Zi4/L8s3HlZDekzHQMmI/8vn4dTYDMIVYzJAK+QSHk8yqmWaMc6UZNeUPcIXj0VciY+9G5lCXqThug0G1Lt66ec6gaklBVe+Brm45Xb2bVt4W5mt/GHESWeP0CgCjq/F/2S94tGNVGKtRMyEJ2gqxQuXiaJxwkWax7SENnREvvO+FPo58f+1YqtBedbb38J8Sxg1Mkn3Mrh/JgVS3WF3HqRfMrTTynAbj3uUVL+1U9nZdCl5UZW9LcD1FmCHK7Q20UboOskj4b1Hva1/u+3eu71M0naYrStKW8QynLJrK+4HjmepxKDhgvzBVt5iq9+xuvT2G0QBLlZn2AlnaYDbUzZp7DOuc1MtXefV7DGCAmv7exC5DCbS8pX59fDmlpYMgAJ7y3YVvjSyzsLRkVoXx37S00bs4S8M9NV7j6QGj4Nx13FEYSjtyq96OXD8VgvKrj1PW4dcGH4B1KXgyAPOBf/3s/nbSrpHcQUvJfdciSxBA3b7RR85gUq2gqo1Bq/SRA+A10Sc/e9v/Ah6ek0CKI0JD024FxH4K6bZMINgqgeQa8Cqr7TbPTtSGlneQN47Ug05RZLfLkVuBcOAWV1lft1sgyC9N5QIh8e/T74AKQHx3Gm4KghKgVWMfdEAkG69kGlNhmlxXO5WLGV5IlLACmw2LtgNiPkRWjIhehdX3DuRsjZPVKtI4a/Q2T30EoM4N0CwE2UbX1rXiz6jiax6IL8xLkjPhW+d19++Arwmq+MJm4DXBWeHNg790iNURR1ifXtLF5EdWjafYJg6oGsiUYAuFzTehJd9yFL51sg/ZgLyFq+Iy4Tvh68XD0FvDg4vFF70ZrYrPouHgfw==</diagram></mxfile>
|
||||
1
docs/PacketSieve
Normal file
1
docs/PacketSieve
Normal file
@ -0,0 +1 @@
|
||||
<mxfile host="Electron" modified="2022-12-09T11:34:28.143Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="ODecaFkfpWYIbJyWGB1x" version="20.3.0" type="device"><diagram id="apy941U9fxJ8yi3WkWR-" name="PacketSieve">7Vpbk6I6EP41Po5FEoj4OOM4e6nZ3TnHmr08bSEEYReIFePt/PoTJAxiMsooous6U2XRTW701193h9BCvXjxjjnj4BP1SNSChrdoofsWhF0biN9UscwUGONMMWKhl6lAoRiE/xGpNKR2GnpkUmrIKY14OC4rXZokxOUlncMYnZeb+TQqzzp2RkRRDFwnUrXfQo8Hmda2jEL/noSjIJ8ZGPJO7OSNpWISOB6dr6lQv4V6jFKeXcWLHolS2+V2yfo9vHL3ZWGMJLxKhy83/3Zp3EePH+mtO/j67HbZ4AbIYWZONJVPLFfLl7kJiCcsIkXKeEBHNHGifqG9c6dsRtJpgBAYnSbeSjKEVHR4pHQsm/winC8l1s6UU6EKeBzJuz5N+IMTh1HqMj06ZSFhYkWfyVzelD1tIapGkA80Ef1csu3JpTM5bET4lnbSXVMjrE0gTfyO0JhwthQNGIkcHs7KbuNI7xu9tCsAEhcSo7fgpcDV5wFhiXiETdjmQcjJYOysrDAX5HyLlcMo6tGIstVQyHOI7btCP+GM/iZrd7Brk6G/DYgZYZwstppO3oU5qWSoECzL5HlBvI5sEqxxDhmHG3vy6+vnj48+NR65gfv//ERd8/kGwPMjh9Yb92OAqTJAb4aTerypYPDhaWYe1dstYnumztttOEQif9Xi7Sbe8Pbc+3d4O6zB2/WhRTVq084trMmW32X/lfAjFdpWLt4v1m/eL6X0loxBFiH/ns8nrrMZoCXFYoZUyCfYn2S4YpoxT5Vm9JTfwxUOR1+LTGc7Mvu6TMlxawyqVfEG1imDKlaCKujCNsB2G7TTyhtHYu13Q0EiPEqvRLhqG+Jf9QsR7XgZxnLUTGhCNkKsVDlROEqEyLLYdpeGzlAU3rdSH4eet3IsXWgvO9tb+M8oFwamyS5mV4/kSKlbcNu2qwVznEae42DcPb/ipZnKvlOVgmdV2XcUuB7CiBMm7A2NYboOOk/Eb1HvG59uezeO5zEymaQrStKW8TRKWTRR9wOHM9UVUAjA/mCqgk2qdjrtapsMswaaalPtGdK0xnQIrIqbDHxK7uWrvPhNBoKwor/Xsc3QAq3uqZ/vn45pad/3oat9eeHhIbYKSytm1Rj/VUtjcHaWRjuKvNrzQ0T8UxdyB2GobMnNaltycCwE1XcfxyzELw0+BKpS8GgA5gP/+dn99aRdIbnDhpL7tkWuQYBA50ofhT6WUivoamPYKH3UAHhJ9MkP33a/gUenJJDmjNA0jGsBsZtC0FIJhBolkFoDXmS13eThid7Q6g7yypFq0GmK7GY5ci0Q9tziauvrZgsE9a2pWiAk3m36IVABiOdMgpeCYA3QsrH3OiFSjbdmGktjmlxXOZXLGZ5omPACG2i/EhDzIbJiRPYqrL5zIIA3BsqKFWWgFXwvj30AovYV0Sx9dM12BxjFn1kGePOrl6oAm13rtABXeeH9NwCMzTLAqB58sXlafPP4rxxkteQx1oendDH5sVXtWbaOQ6oakiXahMFWyxxb41xH+5gNqru4Mi5jsRm+XDws1BgeQiy+6s1oVXwajfr/Aw==</diagram></mxfile>
|
||||
@ -5,6 +5,7 @@
|
||||
#include "eth_interface.h"
|
||||
#include "dynmem.h"
|
||||
#include "utils.h"
|
||||
#include "etherlib_options.h"
|
||||
|
||||
static int ethintf_llrecv(EthIODef * io, const RawPckt * pckt) {
|
||||
ethinf_receive((EthInterface *) io->tag, pckt);
|
||||
@ -21,14 +22,17 @@ EthInterface *ethintf_new(EthIODef * io) {
|
||||
ASSERT_NULL(ethIntf);
|
||||
memset(ðIntf->sieve.layer0, 0, sizeof(PcktSieveLayer));
|
||||
ethIntf->ioDef = io;
|
||||
ethIntf->ip = 0;
|
||||
ethIntf->arpc = arpc_new(ETHLIB_ARPCACHE_SIZE);
|
||||
ASSERT_NULL(ethIntf->arpc);
|
||||
ethintf_register(ethIntf);
|
||||
return ethIntf;
|
||||
}
|
||||
|
||||
void ethinf_receive(EthInterface *intf, const RawPckt *rawPckt) {
|
||||
packsieve_input(&intf->sieve, rawPckt);
|
||||
packsieve_input(&intf->sieve, rawPckt, intf);
|
||||
}
|
||||
|
||||
void ethinf_transmit(EthInterface *intf, const RawPckt *rawPckt) {
|
||||
intf->ioDef->llTx(&(intf->ioDef), rawPckt);
|
||||
intf->ioDef->llTx(intf->ioDef, rawPckt);
|
||||
}
|
||||
|
||||
@ -3,7 +3,12 @@
|
||||
|
||||
#include "packet_sieve.h"
|
||||
#include "prefab/packet_parsers/packet_parsers.h"
|
||||
#include "prefab/packet_parsers/ipv4_types.h"
|
||||
#include "arp_cache.h"
|
||||
|
||||
/**
|
||||
* Ethernet interface low level definition.
|
||||
*/
|
||||
typedef struct EthIODef_ {
|
||||
int (*llTx)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Function pointer to low-level transmit function
|
||||
int (*llTxDone)(struct EthIODef_ * io, const RawPckt * rawPckt); ///< Transmission done (interrupt) callback
|
||||
@ -14,10 +19,15 @@ typedef struct EthIODef_ {
|
||||
|
||||
} EthIODef;
|
||||
|
||||
/**
|
||||
* Ethernet interface representation.
|
||||
*/
|
||||
typedef struct EthInterface_ {
|
||||
PcktSieve sieve; ///< Packet sieve
|
||||
EthIODef * ioDef; ///< Low-level IO definitions
|
||||
EthernetAddress mac; ///< Ethernet address
|
||||
ip4_addr ip; ///< IP address
|
||||
ArpCache * arpc; ///< ARP cache
|
||||
} EthInterface;
|
||||
|
||||
/**
|
||||
|
||||
@ -3,6 +3,8 @@
|
||||
#include "dynmem.h"
|
||||
#include "utils.h"
|
||||
|
||||
#include "packet_sieve.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <memory.h>
|
||||
|
||||
|
||||
@ -14,7 +14,7 @@ typedef uint8_t bool8_t;
|
||||
*
|
||||
* Every packet property structure must extend this base structure,
|
||||
* so that, must begin with fields defined here. PcktProps is guaranteed
|
||||
* to be aligned to 32-bit boundary and so must be guaranteed by extending
|
||||
* to be aligned to 32-bit boundary and so must be kept so by extending
|
||||
* as well. (The size of the packet is divisible by 4.)
|
||||
*/
|
||||
|
||||
@ -33,6 +33,9 @@ typedef struct {
|
||||
PcktPropsHeader
|
||||
} PcktProps;
|
||||
|
||||
struct EthInterface_;
|
||||
struct PcktHeaderElement_;
|
||||
|
||||
/**
|
||||
* @typedef int (*PcktProcFn)(const uint8_t *pHdr, uint32_t size, uint8_t *pPcktProps);
|
||||
* @brief Pckt processing function template.
|
||||
@ -40,11 +43,12 @@ typedef struct {
|
||||
* @param size: remaining packet size counted from pHdr
|
||||
* @param props: pointer to existing structure to store
|
||||
* packet properties to (e.g. source address, port etc.)
|
||||
* @param intf associeated Ethernet interface
|
||||
* @return packet class of contained packet, or -1 on failure or
|
||||
* 0 if no more standard packet contained (e.g. UDP packet contains
|
||||
* user data)
|
||||
*/
|
||||
typedef int (*PcktProcFn)(const uint8_t *hdr, uint32_t size, PcktProps *props);
|
||||
typedef int (*PcktProcFn)(const uint8_t *hdr, uint32_t size, struct PcktHeaderElement_ * pcktHdrLe, struct EthInterface_ * intf);
|
||||
|
||||
/**
|
||||
* @struct PcktClassDesc
|
||||
|
||||
@ -23,7 +23,7 @@ PcktSieve *packsieve_new() {
|
||||
return sieve;
|
||||
}
|
||||
|
||||
void packsieve_input(const PcktSieve *sieve, const RawPckt * rawPckt) {
|
||||
void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthInterface_ *intf) {
|
||||
// extract fields
|
||||
uint8_t * data = rawPckt->payload;
|
||||
uint32_t size = rawPckt->size;
|
||||
@ -53,7 +53,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt * rawPckt) {
|
||||
}
|
||||
|
||||
// call parsing function
|
||||
cdesc->procFun(data + offset, size - offset, &header->props);
|
||||
cdesc->procFun(data + offset, size - offset, header, intf);
|
||||
uint16_t containedClass = header->props.containedPacketClass;
|
||||
if (containedClass != 0) {
|
||||
containerClass = ownClass;
|
||||
@ -80,7 +80,7 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt * rawPckt) {
|
||||
const PcktSieveLayer * nodeIter = layer->nodes;
|
||||
found = false;
|
||||
while (nodeIter && !found) {
|
||||
found |= nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props);
|
||||
found |= nodeIter->matchAny || nodeIter->filtFn(&nodeIter->filtCond, &headerIter->props, &headerIter->next->props); // specific or general match
|
||||
if (found) {
|
||||
layer = nodeIter; // advance in the sieve tree
|
||||
const PcktHeaderElement * containedHeader = headerIter->next; // advance on headers
|
||||
@ -108,12 +108,12 @@ void packsieve_input(const PcktSieve *sieve, const RawPckt * rawPckt) {
|
||||
return;
|
||||
}
|
||||
|
||||
PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, 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;
|
||||
while (nodeIter != NULL && !alreadyExists) {
|
||||
if (packfiltcond_cmp(&nodeIter->filtCond, filtCond) && (nodeIter->filtFn == filtFn)) { // if matching...
|
||||
if ((packfiltcond_cmp(&nodeIter->filtCond, filtCond) || (nodeIter->matchAny && matchAny)) && (nodeIter->filtFn == filtFn)) { // if matching... [search for specific match OR any match]
|
||||
alreadyExists = true;
|
||||
} else {
|
||||
nodeIter = nodeIter->next;
|
||||
@ -128,19 +128,33 @@ PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilte
|
||||
ASSERT_NULL(layer);
|
||||
|
||||
PcktSieveLayer *oldListFirst = parent->nodes;
|
||||
parent->nodes = layer; // replace first element (it's the fastest way of new element insertion, since element position does not carry any meaning this case)
|
||||
layer->packetClass = pcktClass;
|
||||
layer->parent = parent;
|
||||
|
||||
if (!matchAny || (oldListFirst == NULL)) { // for specific match or on first node insertion...
|
||||
layer->prev = NULL;
|
||||
parent->nodes = layer; // ...replace first element (it's the fastest way of new element insertion, since element position does not carry any meaning in general case)
|
||||
layer->next = oldListFirst;
|
||||
if (oldListFirst != NULL) {
|
||||
layer->prev = layer;
|
||||
}
|
||||
} else { // for 'any' match if at least a single node is already present
|
||||
PcktSieveLayer * iter = parent->nodes;
|
||||
while (iter->next != NULL) { // find the last node
|
||||
iter = iter->next;
|
||||
}
|
||||
iter->next = layer;
|
||||
layer->prev = iter;
|
||||
layer->next = NULL;
|
||||
}
|
||||
|
||||
layer->nodes = NULL;
|
||||
layer->filtCond = *filtCond;
|
||||
layer->matchAny = matchAny;
|
||||
layer->filtFn = filtFn;
|
||||
layer->cbFn = cbFn;
|
||||
layer->tag = tag;
|
||||
layer->infoTag[0] = '\0';
|
||||
return layer;
|
||||
}
|
||||
}
|
||||
@ -181,7 +195,11 @@ bool packsieve_remove_layer(PcktSieveLayer * layer) {
|
||||
#define ETH_SIEVE_LAYER_INDENT_PER_LEVEL (4)
|
||||
|
||||
void packsieve_report(const PcktSieveLayer *layer, uint32_t indent) {
|
||||
if (*layer->infoTag != '\0') {
|
||||
INFO("%*c\\--|%s|---\n", indent, ' ', layer->infoTag);
|
||||
} else {
|
||||
INFO("%*c\\--|%d|---\n", indent, ' ', layer->packetClass);
|
||||
}
|
||||
const PcktSieveLayer * nodeIter = layer->nodes;
|
||||
while(nodeIter) {
|
||||
packsieve_report(nodeIter, indent + ETH_SIEVE_LAYER_INDENT_PER_LEVEL);
|
||||
|
||||
@ -6,6 +6,9 @@
|
||||
#include "packet_registry.h"
|
||||
#include "packet.h"
|
||||
|
||||
/**
|
||||
* Packet header information.
|
||||
*/
|
||||
typedef struct PcktHeaderElement_ {
|
||||
struct PcktHeaderElement_ * next, * prev; ///< Next and previous header in the linked list
|
||||
PcktProps props; ///< Properties (allocated to appropriate size)
|
||||
@ -41,10 +44,16 @@ bool packfiltcond_cmp(const PcktSieveFilterCondition * c1, const PcktSieveFilter
|
||||
*/
|
||||
void packfiltcond_zero(PcktSieveFilterCondition * cond);
|
||||
|
||||
/**
|
||||
* Sieve filter function type.
|
||||
*/
|
||||
typedef bool (*SieveFilterFn)(const PcktSieveFilterCondition * filtCond, const PcktProps * contProps, const PcktProps * ownProps);
|
||||
|
||||
struct PcktSieveLayer_;
|
||||
|
||||
/**
|
||||
* Callback function type for packet sieve match.
|
||||
*/
|
||||
typedef int (*SieveCallBackFn)(const Pckt * pckt);
|
||||
|
||||
typedef union {
|
||||
@ -52,17 +61,29 @@ typedef union {
|
||||
uint32_t u;
|
||||
} PcktSieveLayerTag;
|
||||
|
||||
#define PCKT_SIEVE_INFOTAG_LEN (24)
|
||||
|
||||
/**
|
||||
* Packet sieve layer structure.
|
||||
*/
|
||||
typedef struct PcktSieveLayer_ {
|
||||
uint16_t packetClass; ///< Packet class (e.g. IP)
|
||||
PcktSieveFilterCondition filtCond; ///< Filter condition, arbitrary type (e.g. destination IP-address)
|
||||
bool matchAny; ///< Match any packet, don't test against filter condition
|
||||
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 on the sieve tree
|
||||
struct PcktSieveLayer_ * nodes; ///< Subnodes in the sieve tree
|
||||
char infoTag[PCKT_SIEVE_INFOTAG_LEN]; ///< Info tag length
|
||||
} PcktSieveLayer;
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Packet sieve class.
|
||||
*/
|
||||
typedef struct {
|
||||
PcktSieveLayer layer0; ///< Top of sieve tree
|
||||
} PcktSieve;
|
||||
@ -79,14 +100,14 @@ PcktSieve * packsieve_new();
|
||||
* @param data
|
||||
* @param size
|
||||
*/
|
||||
void packsieve_input(const PcktSieve * sieve, const RawPckt * rawPckt);
|
||||
void packsieve_input(const PcktSieve *sieve, const RawPckt *rawPckt, struct EthInterface_ *intf);
|
||||
|
||||
/**
|
||||
* Create a new sieve filter layer.
|
||||
* @param parent parent layer
|
||||
* @return pointer to new layer object or NULL on failure
|
||||
*/
|
||||
PcktSieveLayer *packsieve_new_layer(PcktSieveLayer *parent, const PcktSieveFilterCondition *filtCond, 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);
|
||||
|
||||
/**
|
||||
* Remove sieve layer from packet sieve.
|
||||
|
||||
@ -11,17 +11,28 @@ static bool filtIPv4(const PcktSieveFilterCondition * filtCond, const PcktProps
|
||||
EthernetProps * etherProps = (EthernetProps *) contProps;
|
||||
IPv4Props * ipProps = (IPv4Props *) ownProps;
|
||||
|
||||
return etherProps->length_type == ETH_IPv4_PACKET_CLASS && filtCond->u[0] == ipProps->DestIPAddr;
|
||||
return etherProps->length_type == ETH_IPv4_PACKET_CLASS && IP_ADDR_FROM_FILTCOND(filtCond) == ipProps->DestIPAddr;
|
||||
}
|
||||
|
||||
ConnBlock ipv4_new_connblock(EthInterface *intf, ip4_addr ipAddr, SieveCallBackFn cbFn) {
|
||||
ConnBlock connb;
|
||||
PcktSieveFilterCondition filtCond;
|
||||
packfiltcond_zero(&filtCond);
|
||||
filtCond.u[0] = ipAddr;
|
||||
IP_ADDR_TO_FILTCOND(&filtCond, ipAddr);
|
||||
PcktSieveLayerTag tag;
|
||||
tag.u = 0;
|
||||
connb.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, filtIPv4, cbFn, tag, ETH_IPv4_PACKET_CLASS);
|
||||
bool matchAny = ipAddr == IPv4_ANY_ADDR;
|
||||
connb.sieveLayer = packsieve_new_layer(&intf->sieve.layer0, &filtCond, matchAny, filtIPv4, cbFn, tag, ETH_IPv4_PACKET_CLASS);
|
||||
ASSERT_NULL(connb.sieveLayer);
|
||||
|
||||
connb.intf = intf;
|
||||
|
||||
if (!matchAny) {
|
||||
SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: %u.%u.%u.%u",
|
||||
(ipAddr & 0xFF), ((ipAddr >> 8) & 0xFF), ((ipAddr >> 16) & 0xFF), ((ipAddr >> 24) & 0xFF));
|
||||
} else {
|
||||
SNPRINTF(connb.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "IP: ANY");
|
||||
}
|
||||
|
||||
return connb;
|
||||
}
|
||||
|
||||
@ -4,8 +4,10 @@
|
||||
#include <stdint.h>
|
||||
#include "../../connection_block.h"
|
||||
#include "../../eth_interface.h"
|
||||
#include "../packet_parsers/ipv4_types.h"
|
||||
|
||||
typedef uint32_t ip4_addr;
|
||||
#define IP_ADDR_FROM_FILTCOND(fc) ((fc)->u[0])
|
||||
#define IP_ADDR_TO_FILTCOND(fc,addr) (((fc)->u[0]) = (addr))
|
||||
|
||||
/**
|
||||
* Create new IPv4 connection block.
|
||||
|
||||
@ -14,7 +14,7 @@ static bool filtUdp(const PcktSieveFilterCondition * filtCond, const PcktProps *
|
||||
IPv4Props * ipProps = (IPv4Props *) contProps;
|
||||
UdpProps * udpProps = (UdpProps *) ownProps;
|
||||
|
||||
return ipProps->Protocol == ETH_UDP_PACKET_CLASS && filtCond->uw[0] == udpProps->DestinationPort;
|
||||
return ipProps->Protocol == ETH_UDP_PACKET_CLASS && (UDP_PORT_FROM_FILTCOND(filtCond) == udpProps->DestinationPort);
|
||||
}
|
||||
|
||||
ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port, SieveCallBackFn cbFn) {
|
||||
@ -23,18 +23,22 @@ ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port,
|
||||
|
||||
PcktSieveFilterCondition filtCond;
|
||||
packfiltcond_zero(&filtCond);
|
||||
filtCond.uw[0] = port;
|
||||
UDP_PORT_TO_FILTCOND(&filtCond, port);
|
||||
PcktSieveLayerTag tag;
|
||||
tag.u = 0;
|
||||
udpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, filtUdp, cbFn, tag, ETH_UDP_PACKET_CLASS);
|
||||
udpConnB.sieveLayer = packsieve_new_layer(ipConnB.sieveLayer, &filtCond, false, filtUdp, cbFn, tag, ETH_UDP_PACKET_CLASS);
|
||||
ASSERT_NULL(udpConnB.sieveLayer);
|
||||
|
||||
udpConnB.intf = intf;
|
||||
|
||||
SNPRINTF(udpConnB.sieveLayer->infoTag, PCKT_SIEVE_INFOTAG_LEN, "UDP port: %d", port);
|
||||
|
||||
return udpConnB;
|
||||
}
|
||||
|
||||
#define ALLOC_HEADER_ELEMENT(T) (PcktHeaderElement *) dynmem_alloc(ETH_PCKT_HEADER_ELEMENT_HEAD_SIZE + sizeof(T))
|
||||
#define HEADER_FETCH_PROPS(T,h) (T *)(&h->props)
|
||||
|
||||
int udp_send(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_t size) {
|
||||
int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_t size, ip4_addr addr, uint16_t port) {
|
||||
// allocate headers
|
||||
PcktHeaderElement * udpHeader = ALLOC_HEADER_ELEMENT(UdpProps);
|
||||
PcktHeaderElement * ipHeader = ALLOC_HEADER_ELEMENT(IPv4Props);
|
||||
@ -46,12 +50,71 @@ int udp_send(const struct ConnBlock_ * connBlock, const uint8_t *data, uint32_t
|
||||
ethHeader->next = ipHeader;
|
||||
ethHeader->prev = NULL;
|
||||
|
||||
// prepare headers
|
||||
UdpProps * udpProps = HEADER_FETCH_PROPS(UdpProps, udpHeader);
|
||||
IPv4Props * ipProps = HEADER_FETCH_PROPS(IPv4Props, ipHeader);
|
||||
EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, ethHeader);
|
||||
|
||||
udpProps->Length = size;
|
||||
// fetch sieve layers and fill transmit headers
|
||||
PcktSieveLayer * layer = connBlock->sieveLayer; // UDP layer
|
||||
udpProps->SourcePort = UDP_PORT_FROM_FILTCOND(&layer->filtCond);
|
||||
udpProps->DestinationPort = port;
|
||||
udpProps->Length = ETH_UDP_HEADER_SIZE + size;
|
||||
udpProps->Checksum = 0;
|
||||
|
||||
// IP
|
||||
layer = layer->parent;
|
||||
ipProps->IHL = ETH_IPv4_HEADER_SIZE / 4;
|
||||
ipProps->Version = 4;
|
||||
ipProps->DSF = 0x00;
|
||||
ipProps->TotalLength = ETH_IPv4_HEADER_SIZE + ETH_UDP_HEADER_SIZE + size;
|
||||
// Identification is filled on header insertion
|
||||
ipProps->Flags = 0x00;
|
||||
ipProps->FragmentOffset = 0x00;
|
||||
ipProps->TTL = 255;
|
||||
ipProps->Protocol = ETH_UDP_PACKET_CLASS;
|
||||
// HeaderChecksum is calculated on header insertion
|
||||
ipProps->SourceIPAddr = connBlock->intf->ip;
|
||||
ipProps->DestIPAddr = addr;
|
||||
|
||||
// Ethernet
|
||||
layer = layer->parent;
|
||||
if (addr != 0xFFFFFFFF) {
|
||||
memset(ethProps->destAddr, 0x00, ETH_HW_ADDR_LEN); // TODO...
|
||||
} else {
|
||||
memset(ethProps->destAddr, 0xFF, ETH_HW_ADDR_LEN);
|
||||
}
|
||||
memcpy(ethProps->sourceAddr, connBlock->intf->mac, ETH_HW_ADDR_LEN);
|
||||
ethProps->length_type = ETH_IPv4_PACKET_CLASS;
|
||||
|
||||
// allocate transmit buffer
|
||||
uint32_t txHeaderSize = ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE + ETH_UDP_HEADER_SIZE;
|
||||
uint32_t txBufSize = txHeaderSize + size + 4;
|
||||
uint8_t * txBuf = dynmem_alloc(txBufSize);
|
||||
|
||||
// copy payload
|
||||
memcpy(txBuf + txHeaderSize, data, size);
|
||||
|
||||
// insert UDP header
|
||||
insert_udp_header(txBuf + ETH_ETHERNET_HEADER_SIZE + ETH_IPv4_HEADER_SIZE, udpHeader);
|
||||
|
||||
// insert IPv4 header
|
||||
insert_ipv4_header(txBuf + ETH_ETHERNET_HEADER_SIZE, ipHeader);
|
||||
|
||||
// insert Ethernet header
|
||||
insert_ethernet_header(txBuf, ethHeader);
|
||||
|
||||
// append CRC at the end
|
||||
uint32_t crc = crc32(txBuf, txBufSize - 4);
|
||||
memcpy(txBuf + txBufSize - 4, &crc, 4);
|
||||
|
||||
// send packet
|
||||
RawPckt rpckt;
|
||||
rpckt.size = txBufSize;
|
||||
rpckt.payload = txBuf;
|
||||
rpckt.time_s = 0;
|
||||
rpckt.time_ns = 0;
|
||||
ethinf_transmit(connBlock->intf, &rpckt);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -6,6 +6,9 @@
|
||||
#include "../../eth_interface.h"
|
||||
#include "ipv4_connblock.h"
|
||||
|
||||
#define UDP_PORT_FROM_FILTCOND(fc) ((fc)->uw[0])
|
||||
#define UDP_PORT_TO_FILTCOND(fc,port) (((fc)->uw[0]) = (port))
|
||||
|
||||
/**
|
||||
* Create new IPv4 connection block.
|
||||
* @param intf interface to the connection block will be associated
|
||||
@ -18,7 +21,12 @@ ConnBlock udp_new_connblock(EthInterface * intf, ip4_addr ipAddr, uint16_t port,
|
||||
|
||||
/**
|
||||
* UDP transmit callback.
|
||||
* @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
|
||||
*/
|
||||
int udp_send(const struct ConnBlock_ * connBlock, const uint8_t * data, uint32_t size);
|
||||
int udp_sendto(const struct ConnBlock_ * connBlock, const uint8_t * data, uint32_t size, ip4_addr addr, uint16_t port);
|
||||
|
||||
#endif //ETHERLIB_UDP_CONNBLOCK_H
|
||||
|
||||
@ -3,18 +3,10 @@
|
||||
//
|
||||
|
||||
#include "arp_packet.h"
|
||||
#include "../../packet_registry.h"
|
||||
#include "ethernet_frame.h"
|
||||
#include "../../utils.h"
|
||||
|
||||
int parse_arp_packet(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
||||
EthernetProps * frameProps = (EthernetProps *)props;
|
||||
FETCH_ADVANCE(frameProps->destAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
|
||||
FETCH_ADVANCE(frameProps->sourceAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
|
||||
FETCH_WORD_H2N_ADVANCE(&frameProps->length_type, hdr); // copy length/type field
|
||||
|
||||
// fill-in packet property header fields
|
||||
frameProps->validityOK = true;
|
||||
frameProps->containedPacketClass =
|
||||
frameProps->length_type <= ETH_ETHERTYPE_LENGTH_THRESHOLD ? 0 : frameProps->length_type;
|
||||
frameProps->headerSize = ETH_ETHERNET_HEADER_SIZE;
|
||||
|
||||
return frameProps->containedPacketClass;
|
||||
return 0;
|
||||
}
|
||||
113
prefab/packet_parsers/dhcp.c
Normal file
113
prefab/packet_parsers/dhcp.c
Normal file
@ -0,0 +1,113 @@
|
||||
//
|
||||
// Created by epagris on 2022.12.09..
|
||||
//
|
||||
|
||||
#include <stdlib.h>
|
||||
#include "dhcp.h"
|
||||
#include "../../dynmem.h"
|
||||
#include "../../utils.h"
|
||||
#include "../../connection_block.h"
|
||||
#include "../conn_blocks/udp_connblock.h"
|
||||
|
||||
static struct {
|
||||
DhcpState state;
|
||||
void * buf;
|
||||
ConnBlock connb;
|
||||
uint32_t tranId;
|
||||
} s;
|
||||
|
||||
static const uint8_t DHCP_MAGIC_COOKIE[] = { 99, 130, 83, 99 };
|
||||
|
||||
#define SNAME_LEN (64)
|
||||
#define FILE_LEN (128)
|
||||
|
||||
// (Exact copy of the standard.)
|
||||
typedef enum {
|
||||
DHCPDISCOVER = 1,
|
||||
DHCPOFFER = 2,
|
||||
DHCPREQUEST = 3,
|
||||
DHCPDECLINE = 4,
|
||||
DHCPACK = 5,
|
||||
DHCPNAK = 6,
|
||||
DHCPRELEASE = 7
|
||||
} DhcpMsgType;
|
||||
|
||||
static void dhcp_option_insert_msg_type(uint8_t ** bufPtr, int msgType) {
|
||||
(*bufPtr)[0] = 0x35;
|
||||
(*bufPtr)[1] = 1;
|
||||
(*bufPtr)[2] = msgType;
|
||||
(*bufPtr) += 3;
|
||||
}
|
||||
|
||||
static void dhcp_option_insert_max_msg_size(uint8_t ** bufPtr, uint16_t maxSize) {
|
||||
(*bufPtr)[0] = 0x39;
|
||||
(*bufPtr)[1] = 2;
|
||||
(*bufPtr)[2] = (maxSize >> 8) & 0xFF;
|
||||
(*bufPtr)[3] = maxSize & 0xFF;
|
||||
(*bufPtr) += 4;
|
||||
}
|
||||
|
||||
static void dhcp_option_insert_end(uint8_t ** bufPtr) {
|
||||
(*bufPtr)[0] = 0xFF;
|
||||
(*bufPtr) += 1;
|
||||
}
|
||||
|
||||
static void dhcp_send(EthInterface * intf, DhcpOps * ops) {
|
||||
// construct message
|
||||
uint8_t * buf = (uint8_t *)s.buf;
|
||||
memset(buf, 0, DHCP_MIN_PACKET_SIZE);
|
||||
FILL_BYTE_ADVANCE(buf, &(ops->op));
|
||||
FILL_BYTE_ADVANCE(buf, &(ops->htype));
|
||||
FILL_BYTE_ADVANCE(buf, &(ops->hlen));
|
||||
FILL_BYTE_ADVANCE(buf, &(ops->hops));
|
||||
FILL_DWORD_H2N_ADVANCE(buf, ops->xid);
|
||||
FILL_WORD_H2N_ADVANCE(buf, ops->secs);
|
||||
FILL_WORD_H2N_ADVANCE(buf, ops->flags);
|
||||
FILL_DWORD_H2N_ADVANCE(buf, ops->ciaddr);
|
||||
FILL_DWORD_H2N_ADVANCE(buf, ops->yiaddr);
|
||||
FILL_DWORD_H2N_ADVANCE(buf, ops->siaddr);
|
||||
FILL_DWORD_H2N_ADVANCE(buf, ops->giaddr);
|
||||
FILL_ADVANCE(buf, ops->chaddr, 16);
|
||||
buf += SNAME_LEN + FILE_LEN;
|
||||
FILL_ADVANCE(buf, DHCP_MAGIC_COOKIE, 4); // DHCP magic cookie
|
||||
|
||||
// insert options
|
||||
dhcp_option_insert_msg_type(&buf, DHCPDISCOVER);
|
||||
dhcp_option_insert_max_msg_size(&buf, 1500);
|
||||
dhcp_option_insert_end(&buf);
|
||||
|
||||
udp_sendto(&s.connb, s.buf, DHCP_MIN_PACKET_SIZE, IPv4_ANY_ADDR, DHCP_SERVER_PORT);
|
||||
}
|
||||
|
||||
static void dhcp_discover(EthInterface * intf) {
|
||||
s.tranId = rand();
|
||||
DhcpOps ops = { 0 };
|
||||
|
||||
ops.op = DHCP_BOOTREQUEST;
|
||||
ops.htype = 1;
|
||||
ops.hlen = 6;
|
||||
ops.hops = 0;
|
||||
ops.xid = s.tranId;
|
||||
ops.secs = 0;
|
||||
ops.flags = 0;
|
||||
ops.ciaddr = 0;
|
||||
ops.yiaddr = 0;
|
||||
ops.siaddr = 0;
|
||||
ops.giaddr = 0;
|
||||
|
||||
memcpy(ops.chaddr, intf->mac, ETH_HW_ADDR_LEN);
|
||||
|
||||
dhcp_send(intf, &ops);
|
||||
}
|
||||
|
||||
static int dhcp_resp_cb(const Pckt * pckt) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void dhcp_initiate(EthInterface *intf) {
|
||||
s.state = DHCP_INIT_REBOOT;
|
||||
s.buf = dynmem_alloc(DHCP_MIN_PACKET_SIZE);
|
||||
s.connb = udp_new_connblock(intf, IPv4_ANY_ADDR, DHCP_CLIENT_PORT, dhcp_resp_cb);
|
||||
|
||||
dhcp_discover(intf);
|
||||
}
|
||||
56
prefab/packet_parsers/dhcp.h
Normal file
56
prefab/packet_parsers/dhcp.h
Normal file
@ -0,0 +1,56 @@
|
||||
#ifndef ETHERLIB_TEST_DHCP_H
|
||||
#define ETHERLIB_TEST_DHCP_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../../eth_interface.h"
|
||||
|
||||
typedef struct {
|
||||
uint8_t op; ///< Operations
|
||||
uint8_t htype; ///< Hardware type
|
||||
uint8_t hlen; ///< Hardware address length
|
||||
uint8_t hops; ///< Number of network hops
|
||||
uint32_t xid; ///< Transaction ID
|
||||
uint16_t secs; ///< Seconds elapsed since client started trying to boot
|
||||
uint16_t flags; ///< Flags
|
||||
uint32_t ciaddr; ///< Client IP address (filled in DHCPREQUEST)
|
||||
uint32_t yiaddr; ///< 'Your' client IP address
|
||||
uint32_t siaddr; ///< ...
|
||||
uint32_t giaddr; ///< ...
|
||||
uint8_t chaddr[16]; ///< Client hardware address
|
||||
char * sname; ///< Optional server host name
|
||||
char * file; ///< Boot file name
|
||||
} DhcpOps;
|
||||
|
||||
/**
|
||||
* DHCP state.
|
||||
*/
|
||||
typedef enum {
|
||||
DHCP_INIT_REBOOT,
|
||||
DHCP_REBOOTING,
|
||||
DHCP_INIT,
|
||||
DHCP_REQUESTING,
|
||||
DHCP_SELECTING,
|
||||
DHCP_REBINDING,
|
||||
DHCP_BOUND,
|
||||
DHCP_RENEWING
|
||||
} DhcpState;
|
||||
|
||||
/**
|
||||
* DHCP op codes.
|
||||
*/
|
||||
typedef enum {
|
||||
DHCP_BOOTREQUEST = 1,
|
||||
DHCP_BOOTREPLY = 0
|
||||
} DhcpOpCode;
|
||||
|
||||
#define DHCP_MIN_PACKET_SIZE (312)
|
||||
#define DHCP_CLIENT_PORT (68)
|
||||
#define DHCP_SERVER_PORT (67)
|
||||
|
||||
/**
|
||||
* Initiate DHCP on interface
|
||||
* @param intf interface
|
||||
*/
|
||||
void dhcp_initiate(EthInterface * intf);
|
||||
|
||||
#endif //ETHERLIB_TEST_DHCP_H
|
||||
@ -10,8 +10,8 @@
|
||||
#define ETH_ETHERTYPE_LENGTH_THRESHOLD (1500)
|
||||
#define ETH_ETHERNET_HEADER_SIZE (14)
|
||||
|
||||
int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
||||
EthernetProps * frameProps = (EthernetProps *)props;
|
||||
int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
||||
EthernetProps * frameProps = (EthernetProps *) &pcktHdrLe->props;
|
||||
FETCH_ADVANCE(frameProps->destAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
|
||||
FETCH_ADVANCE(frameProps->sourceAddr, hdr, ETH_HW_ADDR_LEN); // copy destination address
|
||||
FETCH_WORD_H2N_ADVANCE(&frameProps->length_type, hdr); // copy length/type field
|
||||
|
||||
@ -12,6 +12,8 @@
|
||||
#define ETH_HW_ADDR_LEN (6)
|
||||
#define ETH_ETHERNET_HEADER_SIZE (14)
|
||||
|
||||
#define HEADER_FETCH_PROPS(T,h) (T *)(&((h)->props))
|
||||
|
||||
typedef uint8_t EthernetAddress[ETH_HW_ADDR_LEN];
|
||||
|
||||
/**
|
||||
@ -25,14 +27,16 @@ typedef struct {
|
||||
uint16_t length_type; ///< frame length_type
|
||||
} EthernetProps;
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Parse raw Ethernet frames.
|
||||
* @param hdr pointer to the Ethernet packet header
|
||||
* @param size total frame size
|
||||
* @param props pointer to property storage
|
||||
* @param pcktHdrLe pointer to property storage
|
||||
* @return EtherType or 0 (if size is less than 1500) on success or -1 on error
|
||||
*/
|
||||
int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktProps * props);
|
||||
int parse_ethernet(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
|
||||
|
||||
/**
|
||||
* Insert Ethernet header to specified address.
|
||||
|
||||
@ -3,28 +3,26 @@
|
||||
//
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "ethernet_frame.h"
|
||||
#include "ipv4_packet.h"
|
||||
#include "../../utils.h"
|
||||
|
||||
static uint16_t ip_checksum(const IPv4Props * ipProps) {
|
||||
#include "../../eth_interface.h"
|
||||
|
||||
static uint16_t ip_checksum(const uint8_t * hdr) {
|
||||
// sum fields
|
||||
uint32_t sum = 0;
|
||||
const uint16_t * pField = (const uint16_t *) &ipProps;
|
||||
for (uint8_t i = 0; i < ipProps->IHL * 2; i++) {
|
||||
if (i != 5) { // 6. 16-bit long field is the checksum itself, do not count int
|
||||
sum += pField[i];
|
||||
const uint16_t *pField = (const uint16_t *) hdr;
|
||||
for (uint8_t i = 0; i < (ETH_IPv4_HEADER_SIZE / sizeof(uint16_t)); i++) {
|
||||
uint16_t field = pField[i];
|
||||
if (i != 5) { // 6. 16-bit long field is the checksum itself, do not count in
|
||||
sum += field;
|
||||
}
|
||||
}
|
||||
|
||||
// 16-31 bit carry
|
||||
uint16_t carry = sum >> 16;
|
||||
|
||||
// add carry
|
||||
sum += carry;
|
||||
|
||||
// if a new carry bit arisen, then
|
||||
// add to the sum
|
||||
while (sum >> 16) {
|
||||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||||
}
|
||||
|
||||
// invert result
|
||||
uint16_t sum16 = sum ^ 0xFFFF;
|
||||
@ -33,16 +31,17 @@ static uint16_t ip_checksum(const IPv4Props * ipProps) {
|
||||
|
||||
#define ETH_IP_HEADER_LENGTH (20)
|
||||
|
||||
static bool check_ipv4_validity(const IPv4Props * ipProps) {
|
||||
static bool check_ipv4_validity(const uint8_t * hdr, const IPv4Props *ipProps) {
|
||||
bool valid =
|
||||
ipProps->Version != 4 &&
|
||||
ipProps->IHL == ETH_IP_HEADER_LENGTH &&
|
||||
ipProps->HeaderChecksum == ip_checksum(ipProps);
|
||||
(ipProps->Version == 4) &&
|
||||
(ipProps->IHL == (ETH_IP_HEADER_LENGTH / 4)) &&
|
||||
(ntohs(ipProps->HeaderChecksum) == ip_checksum(hdr));
|
||||
return valid;
|
||||
}
|
||||
|
||||
int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
||||
IPv4Props * ipProps = (IPv4Props *)props;
|
||||
int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
||||
const uint8_t * hdrBegin = hdr;
|
||||
IPv4Props *ipProps = HEADER_FETCH_PROPS(IPv4Props, pcktHdrLe);
|
||||
uint8_t version_length;
|
||||
FETCH_ADVANCE(&version_length, hdr, 1);
|
||||
ipProps->Version = (version_length) >> 4;
|
||||
@ -57,31 +56,47 @@ int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
||||
FETCH_ADVANCE(&ipProps->TTL, hdr, 1);
|
||||
FETCH_ADVANCE(&ipProps->Protocol, hdr, 1);
|
||||
FETCH_WORD_H2N_ADVANCE(&ipProps->HeaderChecksum, hdr);
|
||||
//FETCH_DWORD_H2N_ADVANCE(&ipProps->SourceIPAddr, hdr);
|
||||
//FETCH_DWORD_H2N_ADVANCE(&ipProps->DestIPAddr, hdr);
|
||||
FETCH_ADVANCE(&ipProps->SourceIPAddr, hdr, 4);
|
||||
FETCH_ADVANCE(&ipProps->SourceIPAddr, hdr, 4); // NO network -> host byte order conversion!
|
||||
FETCH_ADVANCE(&ipProps->DestIPAddr, hdr, 4);
|
||||
|
||||
// fill-in common packet header fields
|
||||
ipProps->validityOK = check_ipv4_validity(ipProps);
|
||||
ipProps->validityOK = check_ipv4_validity(hdrBegin, ipProps);
|
||||
ipProps->containedPacketClass = ipProps->Protocol;
|
||||
ipProps->headerSize = ETH_IP_HEADER_LENGTH;
|
||||
|
||||
// if packet is valid, learn MAC-IP mapping
|
||||
if (ipProps->validityOK) {
|
||||
EthernetProps * ethProps = HEADER_FETCH_PROPS(EthernetProps, pcktHdrLe->prev); // fetch Ethernet header
|
||||
ArpEntry entry; // fill entry
|
||||
memcpy(&entry.eth, ethProps->sourceAddr, ETH_HW_ADDR_LEN);
|
||||
entry.ip = ipProps->SourceIPAddr;
|
||||
arpc_learn(intf->arpc, &entry); // learn new assignment
|
||||
}
|
||||
|
||||
return ipProps->validityOK ? ipProps->Protocol : -1;
|
||||
}
|
||||
|
||||
static uint16_t nextIpIdentification = 0;
|
||||
|
||||
void insert_ipv4_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
||||
IPv4Props * ipProps = (IPv4Props *)&headers->props;
|
||||
uint8_t * hdrOrig = hdr;
|
||||
IPv4Props *ipProps = (IPv4Props *) &headers->props;
|
||||
ipProps->Identification = nextIpIdentification++; // auto-insert identification
|
||||
uint8_t version_length = (ipProps->Version << 4) | (ipProps->IHL & 0x0F);
|
||||
FILL_ADVANCE(hdr, &version_length, 1);
|
||||
FILL_ADVANCE(hdr, &ipProps->DSF, 1);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, ipProps->TotalLength);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, ipProps->Identification);
|
||||
uint16_t flags_fragOffset = ((ipProps->Flags & 0x07) << 13) | (ipProps->FragmentOffset & ~(0x07 << 13));
|
||||
FILL_WORD_H2N_ADVANCE(hdr, flags_fragOffset); // TODO...
|
||||
FILL_ADVANCE(hdr, &ipProps->TTL, 1);
|
||||
FILL_ADVANCE(hdr, &ipProps->Protocol, 1);
|
||||
uint16_t checksum = ip_checksum(ipProps);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, checksum);
|
||||
uint8_t * ChkSumPtr = hdr;
|
||||
FILL_WORD_H2N_ADVANCE(hdr, 0x0000);
|
||||
FILL_ADVANCE(hdr, &ipProps->SourceIPAddr, 4);
|
||||
FILL_ADVANCE(hdr, &ipProps->DestIPAddr, 4);
|
||||
|
||||
// calculate checksum after filling header
|
||||
uint16_t checksum = ip_checksum(hdrOrig);
|
||||
memcpy(ChkSumPtr, &checksum, 2);
|
||||
}
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include "../../packet_sieve.h"
|
||||
|
||||
#define ETH_IPv4_PACKET_CLASS (0x0800)
|
||||
#define ETH_IPv4_HEADER_SIZE (20)
|
||||
|
||||
/**
|
||||
* IPv4 header structure
|
||||
@ -34,14 +35,16 @@ typedef struct {
|
||||
uint32_t DestIPAddr; ///< destination IP-address
|
||||
} IPv4Props;
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Parse raw IPv4 packets.
|
||||
* @param hdr pointer to the IPv4 packet header
|
||||
* @param size total packet size
|
||||
* @param props pointer to property storage
|
||||
* @param pcktHdrLe pointer to property storage
|
||||
* @return Protocol on success or -1 on failure
|
||||
*/
|
||||
int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktProps * props);
|
||||
int parse_ipv4(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
|
||||
|
||||
/**
|
||||
* Insert IPv4 header.
|
||||
|
||||
10
prefab/packet_parsers/ipv4_types.h
Normal file
10
prefab/packet_parsers/ipv4_types.h
Normal file
@ -0,0 +1,10 @@
|
||||
#ifndef ETHERLIB_TEST_IPV4_TYPES_H
|
||||
#define ETHERLIB_TEST_IPV4_TYPES_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
typedef uint32_t ip4_addr;
|
||||
|
||||
#define IPv4_ANY_ADDR (0xFFFFFFFF)
|
||||
|
||||
#endif //ETHERLIB_TEST_IPV4_TYPES_H
|
||||
@ -2,6 +2,7 @@
|
||||
#include "udp_packet.h"
|
||||
#include "../../utils.h"
|
||||
#include "ipv4_packet.h"
|
||||
#include "ethernet_frame.h"
|
||||
|
||||
#define ETH_UDP_HEADER_SIZE (8)
|
||||
|
||||
@ -13,30 +14,40 @@ typedef struct {
|
||||
uint16_t udpLength;
|
||||
} UdpPseudoHeader;
|
||||
|
||||
static uint16_t udp_checksum(const UdpPseudoHeader * pseudoHeader, const uint8_t * headerPayload, uint32_t headerPayloadSize) {
|
||||
uint16_t chksum = 0;
|
||||
uint16_t i;
|
||||
static uint16_t udp_checksum(const UdpPseudoHeader *pseudoHeader, const uint8_t * hdr, uint32_t size) {
|
||||
uint32_t chksum = 0;
|
||||
|
||||
// pseudoheader
|
||||
for (i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) {
|
||||
chksum += ~((uint16_t *)pseudoHeader)[i];
|
||||
for (uint16_t i = 0; i < sizeof(UdpPseudoHeader) / 2; i++) {
|
||||
uint16_t field = ((uint16_t *) pseudoHeader)[i];
|
||||
field = htons(field);
|
||||
chksum += field;
|
||||
}
|
||||
|
||||
// UDP header and payload
|
||||
for (i = 0; i < headerPayloadSize / 2; i++) {
|
||||
// UDP header and hdr
|
||||
for (uint16_t i = 0; i < size / 2; i++) {
|
||||
if (i != 3) { // skip Checksum field
|
||||
chksum += ~((uint16_t *) headerPayload)[i];
|
||||
uint16_t field = ((uint16_t *) hdr)[i];
|
||||
field = htons(field);
|
||||
chksum += field;
|
||||
}
|
||||
}
|
||||
|
||||
// pad if packet size is odd
|
||||
chksum += ~((uint16_t)headerPayload[headerPayloadSize - 1] << 8);
|
||||
if (size % 2) {
|
||||
chksum += ((uint16_t) (hdr[size - 1] << 8));
|
||||
}
|
||||
|
||||
return chksum;
|
||||
while (chksum >> 16) {
|
||||
chksum = (chksum & 0xFFFF) + (chksum >> 16);
|
||||
}
|
||||
|
||||
uint16_t sum16 = ~(chksum & 0xFFFF);
|
||||
return sum16;
|
||||
}
|
||||
|
||||
int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
||||
UdpProps * udpProps = (UdpProps *)props;
|
||||
int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf) {
|
||||
UdpProps *udpProps = HEADER_FETCH_PROPS(UdpProps, pcktHdrLe);
|
||||
FETCH_WORD_H2N_ADVANCE(&udpProps->SourcePort, hdr);
|
||||
FETCH_WORD_H2N_ADVANCE(&udpProps->DestinationPort, hdr);
|
||||
FETCH_WORD_H2N_ADVANCE(&udpProps->Length, hdr);
|
||||
@ -44,22 +55,23 @@ int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props) {
|
||||
|
||||
// common fields...
|
||||
udpProps->headerSize = ETH_UDP_HEADER_SIZE;
|
||||
udpProps->validityOK = size == udpProps->Length; // TODO UDP checksum validation!
|
||||
udpProps->validityOK = (size == udpProps->Length); // TODO UDP checksum validation!
|
||||
udpProps->containedPacketClass = 0;
|
||||
|
||||
return udpProps->validityOK ? 0 : -1;
|
||||
}
|
||||
|
||||
void insert_udp_header(uint8_t *hdr, const PcktHeaderElement *headers) {
|
||||
UdpProps * udpProps = (UdpProps *) &headers->props;
|
||||
uint8_t *hdrBegin = hdr;
|
||||
UdpProps *udpProps = (UdpProps *) &headers->props;
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->SourcePort);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->DestinationPort);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Length);
|
||||
|
||||
// calculate checksum
|
||||
const IPv4Props * ipProps = (IPv4Props *)&headers->prev->props;
|
||||
UdpPseudoHeader ph = { ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, udpProps->Length };
|
||||
udpProps->Checksum = udp_checksum(&ph, hdr, udpProps->Length);
|
||||
const IPv4Props *ipProps = (IPv4Props *) &headers->prev->props;
|
||||
UdpPseudoHeader ph = {ipProps->SourceIPAddr, ipProps->DestIPAddr, 0, ETH_UDP_PACKET_CLASS, htons(udpProps->Length)};
|
||||
udpProps->Checksum = udp_checksum(&ph, hdrBegin, udpProps->Length);
|
||||
FILL_WORD_H2N_ADVANCE(hdr, udpProps->Checksum);
|
||||
}
|
||||
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include "../../packet_sieve.h"
|
||||
|
||||
#define ETH_UDP_PACKET_CLASS (17)
|
||||
#define ETH_UDP_HEADER_SIZE (8)
|
||||
|
||||
/**
|
||||
* @struct UdpProps
|
||||
@ -20,14 +21,16 @@ typedef struct {
|
||||
uint16_t Checksum; ///> UDP checksum
|
||||
} UdpProps;
|
||||
|
||||
struct EthInterface_;
|
||||
|
||||
/**
|
||||
* Parse raw UDP packets.
|
||||
* @param hdr pointer to the UDP packet header
|
||||
* @param size total packet size
|
||||
* @param props pointer to property storage
|
||||
* @param pcktHdrLe pointer to property storage
|
||||
* @return 0 on success or -1 on failure
|
||||
*/
|
||||
int parse_udp(const uint8_t *hdr, uint32_t size, PcktProps * props);
|
||||
int parse_udp(const uint8_t *hdr, uint32_t size, PcktHeaderElement *pcktHdrLe, struct EthInterface_ *intf);
|
||||
|
||||
/**
|
||||
* Insert UDP header.
|
||||
|
||||
2
utils.c
2
utils.c
@ -54,7 +54,7 @@ uint32_t crc32(const uint8_t * data, uint32_t size) {
|
||||
uint32_t checksum = ~0;
|
||||
uint32_t i;
|
||||
for (i = 0; i < size; i++) {
|
||||
checksum = (checksum >> 8) ^ crc32table[checksum ^ data[i]];
|
||||
checksum = (checksum >> 8) ^ crc32table[(checksum ^ data[i]) & 0xFF];
|
||||
}
|
||||
checksum = ~checksum;
|
||||
return checksum;
|
||||
|
||||
6
utils.h
6
utils.h
@ -34,6 +34,8 @@
|
||||
#define ERROR(...) 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))
|
||||
|
||||
@ -44,10 +46,12 @@
|
||||
|
||||
#define FETCH_ADVANCE(dst,src,n) memcpy((dst), (src), n), (src) += n
|
||||
#define FILL_ADVANCE(dst,src,n) memcpy((dst), (src), n), (dst) += n
|
||||
#define FETCH_BYTE_ADVANCE(dst,src) FETCH_ADVANCE(dst,src,1)
|
||||
#define FILL_BYTE_ADVANCE(dst,src) FILL_ADVANCE(dst,src,1)
|
||||
#define FETCH_WORD_H2N_ADVANCE(dst,w) { uint16_t u; memcpy(&u, w, 2); u = htons(u); memcpy((dst), &u, 2); (w) += 2; }
|
||||
#define FILL_WORD_H2N_ADVANCE(dst,w) { uint16_t u = htons(w); memcpy((dst), &u, 2); (dst) += 2; }
|
||||
#define FETCH_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du; memcpy(&du, dw, 4); du = htonl(du); memcpy((dst), &du, 4); (dw) += 4; }
|
||||
#define FILL_DWORD_H2N_ADVANCE(dst,w) { uint32_t du = htonl(dw); memcpy(dst, &du, 4); (dst) += 4; }
|
||||
#define FILL_DWORD_H2N_ADVANCE(dst,dw) { uint32_t du = htonl(dw); memcpy(dst, &du, 4); (dst) += 4; }
|
||||
|
||||
|
||||
// ------------------
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user